From e69107a73a053ba8478bcdaf47a1565de84e1a52 Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Fri, 3 Oct 2025 23:56:33 +0200 Subject: [PATCH 01/97] pdf: updates for core.pdf branch --- plot/text.go | 2 +- tensorcore/tensorgrid.go | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/plot/text.go b/plot/text.go index 54e1ca88..c5188b43 100644 --- a/plot/text.go +++ b/plot/text.go @@ -104,7 +104,7 @@ func (tx *Text) Config(pt *Plot) { // txs := &pt.StandardTextStyle rt, _ := htmltext.HTMLToRich([]byte(tx.Text), fs, nil) - tx.PaintText = pt.TextShaper.WrapLines(rt, fs, ts, &rich.DefaultSettings, math32.Vec2(hsz, fht)) + tx.PaintText = pt.TextShaper.WrapLines(rt, fs, ts, math32.Vec2(hsz, fht)) } func (tx *Text) ToDots(uc *units.Context) { diff --git a/tensorcore/tensorgrid.go b/tensorcore/tensorgrid.go index 47d8c8a0..184fa652 100644 --- a/tensorcore/tensorgrid.go +++ b/tensorcore/tensorgrid.go @@ -197,7 +197,7 @@ func (tg *TensorGrid) SizeLabel(lbs []string, col bool) (minBlank, ngps int, sz if ts != nil { sty, tsty := tg.Styles.NewRichText() tx := rich.NewText(sty, []rune(lbs[mxi])) - lns := ts.WrapLines(tx, sty, tsty, &rich.DefaultSettings, math32.Vec2(10000, 1000)) + lns := ts.WrapLines(tx, sty, tsty, math32.Vec2(10000, 1000)) sz = lns.Bounds.Size().Ceil() if col { sz.X, sz.Y = sz.Y, sz.X @@ -314,7 +314,7 @@ func (tg *TensorGrid) Render() { } yex := float32(ygp) * dimEx tx := rich.NewText(sty, []rune(lb)) - lns := ts.WrapLines(tx, sty, tsty, &rich.DefaultSettings, math32.Vec2(10000, 1000)) + lns := ts.WrapLines(tx, sty, tsty, math32.Vec2(10000, 1000)) cr := math32.Vec2(0, float32(y)+yex) pr := epos.Add(cr.Mul(gsz)) pc.DrawText(lns, pr) @@ -343,7 +343,7 @@ func (tg *TensorGrid) Render() { } xex := float32(xgp) * dimEx tx := rich.NewText(sty, []rune(lb)) - lns := ts.WrapLines(tx, sty, tsty, &rich.DefaultSettings, math32.Vec2(10000, 1000)) + lns := ts.WrapLines(tx, sty, tsty, math32.Vec2(10000, 1000)) cr := math32.Vec2(float32(x)+xex, 0) pr := epos.Add(cr.Mul(gsz)) rot := tg.GridStyle.ColumnRotation From b81fa706d621cae4f9a4258d9af2e2fad6450844 Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Thu, 9 Oct 2025 15:10:26 +0200 Subject: [PATCH 02/97] pdf: update to latest core mod (combined pdf and xyzplan) --- go.mod | 10 +++++++--- go.sum | 11 ++++++----- 2 files changed, 13 insertions(+), 8 deletions(-) diff --git a/go.mod b/go.mod index 7c7b0816..b8dfe00f 100644 --- a/go.mod +++ b/go.mod @@ -10,7 +10,7 @@ go 1.23.4 // https://github.com/googleapis/go-genproto/issues/1015 require ( - cogentcore.org/core v0.3.13-0.20250909222513-1cba37a21a69 + cogentcore.org/core v0.3.13-0.20251009125244-9993be515523 github.com/alecthomas/assert/v2 v2.6.0 github.com/cogentcore/readline v0.1.3 github.com/cogentcore/yaegi v0.0.0-20250622201820-b7838bdd95eb @@ -40,6 +40,7 @@ require ( github.com/dlclark/regexp2 v1.11.0 // indirect github.com/ericchiang/css v1.3.0 // indirect github.com/fsnotify/fsnotify v1.8.0 // indirect + github.com/go-fonts/latin-modern v0.3.3 // indirect github.com/go-gl/glfw/v3.3/glfw v0.0.0-20240506104042-037f3cc74f2a // indirect github.com/go-text/typesetting v0.3.1-0.20250402122313-7a0f05577ff5 // indirect github.com/gobwas/glob v0.2.3 // indirect @@ -56,10 +57,10 @@ require ( github.com/mattn/go-isatty v0.0.20 // indirect github.com/mattn/go-runewidth v0.0.15 // indirect github.com/mattn/go-shellwords v1.0.12 // indirect - github.com/muesli/termenv v0.15.2 // indirect + github.com/muesli/termenv v0.16.0 // indirect github.com/pelletier/go-toml/v2 v2.1.2-0.20240227203013-2b69615b5d55 // indirect github.com/pmezard/go-difflib v1.0.1-0.20181226105442-5d4384ee4fb2 // indirect - github.com/rivo/uniseg v0.2.0 // indirect + github.com/rivo/uniseg v0.4.7 // indirect github.com/rogpeppe/go-internal v1.13.1 // indirect github.com/tdewolff/parse/v2 v2.7.19 // indirect github.com/tdewolff/test v1.0.11-0.20240106005702-7de5f7df4739 // indirect @@ -73,4 +74,7 @@ require ( google.golang.org/genproto v0.0.0-20250804133106-a7a43d27e69b // indirect google.golang.org/genproto/googleapis/rpc v0.0.0-20250728155136-f173205681a0 // indirect gopkg.in/yaml.v3 v3.0.1 // indirect + modernc.org/knuth v0.5.4 // indirect + modernc.org/token v1.1.0 // indirect + star-tex.org/x/tex v0.6.0 // indirect ) diff --git a/go.sum b/go.sum index 00e58493..fa81bf3b 100644 --- a/go.sum +++ b/go.sum @@ -1,5 +1,5 @@ -cogentcore.org/core v0.3.13-0.20250909222513-1cba37a21a69 h1:P2aEix7cbJtYrRzBU6xtvQDRWt452OPEi1dcoD6SWUQ= -cogentcore.org/core v0.3.13-0.20250909222513-1cba37a21a69/go.mod h1:8zfSRZhuM/4OTMt8i1Q8W0P2VcD1riw2AgEQNmv8wHQ= +cogentcore.org/core v0.3.13-0.20251009125244-9993be515523 h1:vbmBkuQJcMoKyxa+yH/BMT0p7ikX3Zz9paQ2ySzGizE= +cogentcore.org/core v0.3.13-0.20251009125244-9993be515523/go.mod h1:bVZvk2HinEs96r3d16y4BlxS/K6jN/SJlkR5MGjOVLY= github.com/Bios-Marcel/wastebasket/v2 v2.0.3 h1:TkoDPcSqluhLGE+EssHu7UGmLgUEkWg7kNyHyyJ3Q9g= github.com/Bios-Marcel/wastebasket/v2 v2.0.3/go.mod h1:769oPCv6eH7ugl90DYIsWwjZh4hgNmMS3Zuhe1bH6KU= github.com/BurntSushi/toml v0.3.1/go.mod h1:xHWCNGjB5oqiDr8zfno3MHue2Ht5sIBksp03qcyfWMU= @@ -102,8 +102,8 @@ github.com/mattn/go-shellwords v1.0.12/go.mod h1:EZzvwXDESEeg03EKmM+RmDnNOPKG4lL github.com/mitchellh/go-homedir v1.1.0 h1:lukF9ziXFxDFPkA1vsr5zpc1XuPDn/wFntq5mG+4E0Y= github.com/mitchellh/go-homedir v1.1.0/go.mod h1:SfyaCUpYCn1Vlf4IUYiD9fPX4A5wJrkLzIz1N1q0pr0= github.com/mitchellh/mapstructure v1.1.2/go.mod h1:FVVH3fgwuzCH5S8UJGiWEs2h04kUh9fWfEaFds41c1Y= -github.com/muesli/termenv v0.15.2 h1:GohcuySI0QmI3wN8Ok9PtKGkgkFIk7y6Vpb5PvrY+Wo= -github.com/muesli/termenv v0.15.2/go.mod h1:Epx+iuz8sNs7mNKhxzH4fWXGNpZwUaJKRS1noLXviQ8= +github.com/muesli/termenv v0.16.0 h1:S5AlUN9dENB57rsbnkPyfdGuWIlkmzJjbFf0Tf5FWUc= +github.com/muesli/termenv v0.16.0/go.mod h1:ZRfOIKPFDYQoDFF4Olj7/QJbW60Ol/kL1pU3VfY/Cnk= github.com/nsf/termbox-go v1.1.1 h1:nksUPLCb73Q++DwbYUBEglYBRPZyoXJdrj5L+TkjyZY= github.com/nsf/termbox-go v1.1.1/go.mod h1:T0cTdVuOwf7pHQNtfhnEbzHbcNyCEcVU4YPpouCbVxo= github.com/pelletier/go-toml v1.2.0/go.mod h1:5z9KED0ma1S8pY6P1sdut58dfprrGBbd/94hg7ilaic= @@ -112,8 +112,9 @@ github.com/pelletier/go-toml/v2 v2.1.2-0.20240227203013-2b69615b5d55/go.mod h1:t github.com/pmezard/go-difflib v1.0.0/go.mod h1:iKH77koFhYxTK1pcRnkKkqfTogsbg7gZNVY4sRDYZ/4= github.com/pmezard/go-difflib v1.0.1-0.20181226105442-5d4384ee4fb2 h1:Jamvg5psRIccs7FGNTlIRMkT8wgtp5eCXdBlqhYGL6U= github.com/pmezard/go-difflib v1.0.1-0.20181226105442-5d4384ee4fb2/go.mod h1:iKH77koFhYxTK1pcRnkKkqfTogsbg7gZNVY4sRDYZ/4= -github.com/rivo/uniseg v0.2.0 h1:S1pD9weZBuJdFmowNwbpi7BJ8TNftyUImj/0WQi72jY= github.com/rivo/uniseg v0.2.0/go.mod h1:J6wj4VEh+S6ZtnVlnTBMWIodfgj8LQOQFoIToxlJtxc= +github.com/rivo/uniseg v0.4.7 h1:WUdvkW8uEhrYfLC4ZzdpI2ztxP1I582+49Oc5Mq64VQ= +github.com/rivo/uniseg v0.4.7/go.mod h1:FN3SvrM+Zdj16jyLfmOkMNblXMcoc8DfTHruCPUcx88= github.com/rogpeppe/go-internal v1.13.1 h1:KvO1DLK/DRN07sQ1LQKScxyZJuNnedQ5/wKSR38lUII= github.com/rogpeppe/go-internal v1.13.1/go.mod h1:uMEvuHeurkdAXX61udpOXGD/AzZDWNMNyH2VO9fmH0o= github.com/russross/blackfriday v1.5.2/go.mod h1:JO/DiYxRf+HjHt06OyowR9PTA263kcR/rfWxYHBV53g= From a56fa29a4b7198c9ec9c61562b142e59e1a0277d Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Tue, 14 Oct 2025 15:15:21 +0200 Subject: [PATCH 03/97] pdf: plot save to PDF --- go.mod | 3 ++- go.sum | 6 ++++-- plotcore/editor.go | 19 ++++++++++++++++++- plotcore/typegen.go | 2 +- 4 files changed, 25 insertions(+), 5 deletions(-) diff --git a/go.mod b/go.mod index b8dfe00f..d16e8b87 100644 --- a/go.mod +++ b/go.mod @@ -10,7 +10,7 @@ go 1.23.4 // https://github.com/googleapis/go-genproto/issues/1015 require ( - cogentcore.org/core v0.3.13-0.20251009125244-9993be515523 + cogentcore.org/core v0.3.13-0.20251014131306-9996ffd52fd8 github.com/alecthomas/assert/v2 v2.6.0 github.com/cogentcore/readline v0.1.3 github.com/cogentcore/yaegi v0.0.0-20250622201820-b7838bdd95eb @@ -51,6 +51,7 @@ require ( github.com/hack-pad/hackpadfs v0.2.1 // indirect github.com/hack-pad/safejs v0.1.1 // indirect github.com/hexops/gotextdiff v1.0.3 // indirect + github.com/jeandeaual/go-locale v0.0.0-20250612000132-0ef82f21eade // indirect github.com/jinzhu/copier v0.4.0 // indirect github.com/kr/text v0.2.0 // indirect github.com/lucasb-eyer/go-colorful v1.2.0 // indirect diff --git a/go.sum b/go.sum index fa81bf3b..670dbcbd 100644 --- a/go.sum +++ b/go.sum @@ -1,5 +1,5 @@ -cogentcore.org/core v0.3.13-0.20251009125244-9993be515523 h1:vbmBkuQJcMoKyxa+yH/BMT0p7ikX3Zz9paQ2ySzGizE= -cogentcore.org/core v0.3.13-0.20251009125244-9993be515523/go.mod h1:bVZvk2HinEs96r3d16y4BlxS/K6jN/SJlkR5MGjOVLY= +cogentcore.org/core v0.3.13-0.20251014131306-9996ffd52fd8 h1:p4wJuqxRDXAjv9uY3WhgFdWvxXKhkgivDi2AXM+wdAg= +cogentcore.org/core v0.3.13-0.20251014131306-9996ffd52fd8/go.mod h1:eDHnTCy1sBhAKN9NPsSCnBW3VAnwQBNA9nbGMo9r+Xs= github.com/Bios-Marcel/wastebasket/v2 v2.0.3 h1:TkoDPcSqluhLGE+EssHu7UGmLgUEkWg7kNyHyyJ3Q9g= github.com/Bios-Marcel/wastebasket/v2 v2.0.3/go.mod h1:769oPCv6eH7ugl90DYIsWwjZh4hgNmMS3Zuhe1bH6KU= github.com/BurntSushi/toml v0.3.1/go.mod h1:xHWCNGjB5oqiDr8zfno3MHue2Ht5sIBksp03qcyfWMU= @@ -83,6 +83,8 @@ github.com/hashicorp/hcl v1.0.0/go.mod h1:E5yfLk+7swimpb2L/Alb/PJmXilQ/rhwaUYs4T github.com/hexops/gotextdiff v1.0.3 h1:gitA9+qJrrTCsiCl7+kh75nPqQt1cx4ZkudSTLoUqJM= github.com/hexops/gotextdiff v1.0.3/go.mod h1:pSWU5MAI3yDq+fZBTazCSJysOMbxWL1BSow5/V2vxeg= github.com/inconshreveable/mousetrap v1.0.0/go.mod h1:PxqpIevigyE2G7u3NXJIT2ANytuPF1OarO4DADm73n8= +github.com/jeandeaual/go-locale v0.0.0-20250612000132-0ef82f21eade h1:FmusiCI1wHw+XQbvL9M+1r/C3SPqKrmBaIOYwVfQoDE= +github.com/jeandeaual/go-locale v0.0.0-20250612000132-0ef82f21eade/go.mod h1:ZDXo8KHryOWSIqnsb/CiDq7hQUYryCgdVnxbj8tDG7o= github.com/jinzhu/copier v0.4.0 h1:w3ciUoD19shMCRargcpm0cm91ytaBhDvuRpz1ODO/U8= github.com/jinzhu/copier v0.4.0/go.mod h1:DfbEm0FYsaqBcKcFuvmOZb218JkPGtvSHsKg8S8hyyg= github.com/kr/pretty v0.3.1 h1:flRD4NNwYAUpkphVc1HcthR4KEIFJ65n8Mw5qdRn3LE= diff --git a/plotcore/editor.go b/plotcore/editor.go index 6e3d82ea..01e0485c 100644 --- a/plotcore/editor.go +++ b/plotcore/editor.go @@ -169,7 +169,7 @@ func (pl *Editor) SetSlice(sl any, stylers ...func(s *plot.Style)) *Editor { return pl.SetTable(dt) } -// SaveSVG saves the plot to an svg -- first updates to ensure that plot is current +// SaveSVG saves the plot to an SVG file. func (pl *Editor) SaveSVG(fname core.Filename) { //types:add plt := pl.plotWidget.Plot mp := plt.PaintBox.Min @@ -185,6 +185,21 @@ func (pl *Editor) SaveSVG(fname core.Filename) { //types:add pl.svgFile = fname } +// SavePDF saves the plot to a PDF file. +func (pl *Editor) SavePDF(fname core.Filename) { //types:add + plt := pl.plotWidget.Plot + mp := plt.PaintBox.Min + plt.PaintBox = plt.PaintBox.Sub(mp) + ptr := paint.NewPainter(math32.FromPoint(plt.PaintBox.Size())) + ptr.Paint.UnitContext = pl.Styles.UnitContext // preserve DPI from current + pd := paint.RenderToPDF(plt.Draw(ptr)) + err := os.WriteFile(string(fname), pd, 0666) + plt.PaintBox = plt.PaintBox.Add(mp) + if err != nil { + core.ErrorSnackbar(pl, err) + } +} + // SaveImage saves the current plot as an image (e.g., png). func (pl *Editor) SaveImage(fname core.Filename) { //types:add plt := pl.plotWidget.Plot @@ -211,6 +226,7 @@ func (pl *Editor) SaveAll(fname core.Filename) { //types:add pl.SaveCSV(core.Filename(fn+".tsv"), tensor.Tab) pl.SaveImage(core.Filename(fn + ".png")) pl.SaveSVG(core.Filename(fn + ".svg")) + pl.SavePDF(core.Filename(fn + ".pdf")) } // OpenCSV opens the Table data from a csv (comma-separated values) file (or any delim) @@ -624,6 +640,7 @@ func (pl *Editor) MakeToolbar(p *tree.Plan) { tree.Add(p, func(w *core.Button) { w.SetText("Save").SetIcon(icons.Save).SetMenu(func(m *core.Scene) { core.NewFuncButton(m).SetFunc(pl.SaveSVG).SetIcon(icons.Save) + core.NewFuncButton(m).SetFunc(pl.SavePDF).SetIcon(icons.Save) core.NewFuncButton(m).SetFunc(pl.SaveImage).SetIcon(icons.Save) core.NewFuncButton(m).SetFunc(pl.SaveCSV).SetIcon(icons.Save) core.NewSeparator(m) diff --git a/plotcore/typegen.go b/plotcore/typegen.go index a87fbd59..27e4bc53 100644 --- a/plotcore/typegen.go +++ b/plotcore/typegen.go @@ -8,7 +8,7 @@ import ( "cogentcore.org/lab/plot" ) -var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/plotcore.Editor", IDName: "editor", Doc: "Editor is a widget that provides an interactive 2D plot\nof selected columns of tabular data, represented by a [table.Table] into\na [table.Table]. Other types of tabular data can be converted into this format.\nThe user can change various options for the plot and also modify the underlying data.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Methods: []types.Method{{Name: "SaveSVG", Doc: "SaveSVG saves the plot to an svg -- first updates to ensure that plot is current", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"fname"}}, {Name: "SaveImage", Doc: "SaveImage saves the current plot as an image (e.g., png).", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"fname"}}, {Name: "SaveCSV", Doc: "SaveCSV saves the Table data to a csv (comma-separated values) file with headers (any delim)", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"fname", "delim"}}, {Name: "SaveAll", Doc: "SaveAll saves the current plot to a png, svg, and the data to a tsv -- full save\nAny extension is removed and appropriate extensions are added", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"fname"}}, {Name: "OpenCSV", Doc: "OpenCSV opens the Table data from a csv (comma-separated values) file (or any delim)", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"filename", "delim"}}, {Name: "setColumnsByName", Doc: "setColumnsByName turns columns on or off if their name contains\nthe given string.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"nameContains", "on"}}}, Embeds: []types.Field{{Name: "Frame"}}, Fields: []types.Field{{Name: "table", Doc: "table is the table of data being plotted."}, {Name: "PlotStyle", Doc: "PlotStyle has the overall plot style parameters."}, {Name: "plot", Doc: "plot is the plot object."}, {Name: "svgFile", Doc: "current svg file"}, {Name: "dataFile", Doc: "current csv data file"}, {Name: "inPlot", Doc: "currently doing a plot"}, {Name: "columnsFrame"}, {Name: "plotWidget"}, {Name: "plotStyleModified"}}}) +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/plotcore.Editor", IDName: "editor", Doc: "Editor is a widget that provides an interactive 2D plot\nof selected columns of tabular data, represented by a [table.Table] into\na [table.Table]. Other types of tabular data can be converted into this format.\nThe user can change various options for the plot and also modify the underlying data.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Methods: []types.Method{{Name: "SaveSVG", Doc: "SaveSVG saves the plot to an SVG file.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"fname"}}, {Name: "SavePDF", Doc: "SavePDF saves the plot to a PDF file.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"fname"}}, {Name: "SaveImage", Doc: "SaveImage saves the current plot as an image (e.g., png).", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"fname"}}, {Name: "SaveCSV", Doc: "SaveCSV saves the Table data to a csv (comma-separated values) file with headers (any delim)", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"fname", "delim"}}, {Name: "SaveAll", Doc: "SaveAll saves the current plot to a png, svg, and the data to a tsv -- full save\nAny extension is removed and appropriate extensions are added", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"fname"}}, {Name: "OpenCSV", Doc: "OpenCSV opens the Table data from a csv (comma-separated values) file (or any delim)", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"filename", "delim"}}, {Name: "setColumnsByName", Doc: "setColumnsByName turns columns on or off if their name contains\nthe given string.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"nameContains", "on"}}}, Embeds: []types.Field{{Name: "Frame"}}, Fields: []types.Field{{Name: "table", Doc: "table is the table of data being plotted."}, {Name: "PlotStyle", Doc: "PlotStyle has the overall plot style parameters."}, {Name: "plot", Doc: "plot is the plot object."}, {Name: "svgFile", Doc: "current svg file"}, {Name: "dataFile", Doc: "current csv data file"}, {Name: "inPlot", Doc: "currently doing a plot"}, {Name: "columnsFrame"}, {Name: "plotWidget"}, {Name: "plotStyleModified"}}}) // NewEditor returns a new [Editor] with the given optional parent: // Editor is a widget that provides an interactive 2D plot From a12de9e660c776a3d4a4ddf777c613a33bea2742 Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Tue, 14 Oct 2025 16:46:42 +0200 Subject: [PATCH 04/97] pdf: adding RecoverJobs functionality to simmer and baremetal. not pdf but who cares. --- examples/baremetal/api.go | 8 ++++ examples/baremetal/baremetal/baremetal.pb.go | 31 ++++++++------- examples/baremetal/baremetal/baremetal.proto | 1 + .../baremetal/baremetal/baremetal_grpc.pb.go | 38 +++++++++++++++++++ examples/baremetal/client.go | 14 +++++++ examples/baremetal/cmd/baremetal/main.go | 8 ++++ examples/baremetal/jobs.go | 13 ++++++- examples/baremetal/jobs.goal | 13 ++++++- examples/simmer/bare.go | 30 +++++++++++++++ examples/simmer/bare.goal | 30 +++++++++++++++ examples/simmer/jobs.go | 19 ++++++++++ examples/simmer/jobs.goal | 19 ++++++++++ examples/simmer/simmer.go | 3 ++ examples/simmer/typegen.go | 2 +- 14 files changed, 213 insertions(+), 16 deletions(-) diff --git a/examples/baremetal/api.go b/examples/baremetal/api.go index a81eee4d..2381fa1a 100644 --- a/examples/baremetal/api.go +++ b/examples/baremetal/api.go @@ -79,3 +79,11 @@ func (bm *BareMetal) UpdateJobs() (nrun, nfinished int, err error) { bm.saveState() return } + +// RecoverJob reinstates job information so files can be recovered etc. +func (bm *BareMetal) RecoverJob(job *Job) (*Job, error) { + bm.Lock() + defer bm.Unlock() + + return bm.recoverJob(job) +} diff --git a/examples/baremetal/baremetal/baremetal.pb.go b/examples/baremetal/baremetal/baremetal.pb.go index 665f1aa3..d221cc04 100644 --- a/examples/baremetal/baremetal/baremetal.pb.go +++ b/examples/baremetal/baremetal/baremetal.pb.go @@ -534,7 +534,7 @@ var file_baremetal_baremetal_proto_rawDesc = []byte{ 0x10, 0x01, 0x12, 0x0b, 0x0a, 0x07, 0x52, 0x75, 0x6e, 0x6e, 0x69, 0x6e, 0x67, 0x10, 0x02, 0x12, 0x0d, 0x0a, 0x09, 0x43, 0x6f, 0x6d, 0x70, 0x6c, 0x65, 0x74, 0x65, 0x64, 0x10, 0x03, 0x12, 0x0c, 0x0a, 0x08, 0x43, 0x61, 0x6e, 0x63, 0x65, 0x6c, 0x65, 0x64, 0x10, 0x04, 0x12, 0x0b, 0x0a, 0x07, - 0x45, 0x72, 0x72, 0x6f, 0x72, 0x65, 0x64, 0x10, 0x05, 0x32, 0xa1, 0x02, 0x0a, 0x09, 0x42, 0x61, + 0x45, 0x72, 0x72, 0x6f, 0x72, 0x65, 0x64, 0x10, 0x05, 0x32, 0xcf, 0x02, 0x0a, 0x09, 0x42, 0x61, 0x72, 0x65, 0x4d, 0x65, 0x74, 0x61, 0x6c, 0x12, 0x2f, 0x0a, 0x06, 0x53, 0x75, 0x62, 0x6d, 0x69, 0x74, 0x12, 0x15, 0x2e, 0x62, 0x61, 0x72, 0x65, 0x6d, 0x65, 0x74, 0x61, 0x6c, 0x2e, 0x53, 0x75, 0x62, 0x6d, 0x69, 0x73, 0x73, 0x69, 0x6f, 0x6e, 0x1a, 0x0e, 0x2e, 0x62, 0x61, 0x72, 0x65, 0x6d, @@ -552,11 +552,14 @@ var file_baremetal_baremetal_proto_rawDesc = []byte{ 0x3c, 0x0a, 0x0a, 0x55, 0x70, 0x64, 0x61, 0x74, 0x65, 0x4a, 0x6f, 0x62, 0x73, 0x12, 0x16, 0x2e, 0x67, 0x6f, 0x6f, 0x67, 0x6c, 0x65, 0x2e, 0x70, 0x72, 0x6f, 0x74, 0x6f, 0x62, 0x75, 0x66, 0x2e, 0x45, 0x6d, 0x70, 0x74, 0x79, 0x1a, 0x16, 0x2e, 0x67, 0x6f, 0x6f, 0x67, 0x6c, 0x65, 0x2e, 0x70, - 0x72, 0x6f, 0x74, 0x6f, 0x62, 0x75, 0x66, 0x2e, 0x45, 0x6d, 0x70, 0x74, 0x79, 0x42, 0x31, 0x5a, - 0x2f, 0x63, 0x6f, 0x67, 0x65, 0x6e, 0x74, 0x63, 0x6f, 0x72, 0x65, 0x2e, 0x6f, 0x72, 0x67, 0x2f, - 0x6c, 0x61, 0x62, 0x2f, 0x65, 0x78, 0x61, 0x6d, 0x70, 0x6c, 0x65, 0x73, 0x2f, 0x62, 0x61, 0x72, - 0x65, 0x6d, 0x65, 0x74, 0x61, 0x6c, 0x2f, 0x62, 0x61, 0x72, 0x65, 0x6d, 0x65, 0x74, 0x61, 0x6c, - 0x62, 0x06, 0x70, 0x72, 0x6f, 0x74, 0x6f, 0x33, + 0x72, 0x6f, 0x74, 0x6f, 0x62, 0x75, 0x66, 0x2e, 0x45, 0x6d, 0x70, 0x74, 0x79, 0x12, 0x2c, 0x0a, + 0x0a, 0x52, 0x65, 0x63, 0x6f, 0x76, 0x65, 0x72, 0x4a, 0x6f, 0x62, 0x12, 0x0e, 0x2e, 0x62, 0x61, + 0x72, 0x65, 0x6d, 0x65, 0x74, 0x61, 0x6c, 0x2e, 0x4a, 0x6f, 0x62, 0x1a, 0x0e, 0x2e, 0x62, 0x61, + 0x72, 0x65, 0x6d, 0x65, 0x74, 0x61, 0x6c, 0x2e, 0x4a, 0x6f, 0x62, 0x42, 0x31, 0x5a, 0x2f, 0x63, + 0x6f, 0x67, 0x65, 0x6e, 0x74, 0x63, 0x6f, 0x72, 0x65, 0x2e, 0x6f, 0x72, 0x67, 0x2f, 0x6c, 0x61, + 0x62, 0x2f, 0x65, 0x78, 0x61, 0x6d, 0x70, 0x6c, 0x65, 0x73, 0x2f, 0x62, 0x61, 0x72, 0x65, 0x6d, + 0x65, 0x74, 0x61, 0x6c, 0x2f, 0x62, 0x61, 0x72, 0x65, 0x6d, 0x65, 0x74, 0x61, 0x6c, 0x62, 0x06, + 0x70, 0x72, 0x6f, 0x74, 0x6f, 0x33, } var ( @@ -594,13 +597,15 @@ var file_baremetal_baremetal_proto_depIdxs = []int32{ 4, // 7: baremetal.BareMetal.CancelJobs:input_type -> baremetal.JobIDList 4, // 8: baremetal.BareMetal.FetchResults:input_type -> baremetal.JobIDList 7, // 9: baremetal.BareMetal.UpdateJobs:input_type -> google.protobuf.Empty - 2, // 10: baremetal.BareMetal.Submit:output_type -> baremetal.Job - 3, // 11: baremetal.BareMetal.JobStatus:output_type -> baremetal.JobList - 5, // 12: baremetal.BareMetal.CancelJobs:output_type -> baremetal.Error - 3, // 13: baremetal.BareMetal.FetchResults:output_type -> baremetal.JobList - 7, // 14: baremetal.BareMetal.UpdateJobs:output_type -> google.protobuf.Empty - 10, // [10:15] is the sub-list for method output_type - 5, // [5:10] is the sub-list for method input_type + 2, // 10: baremetal.BareMetal.RecoverJob:input_type -> baremetal.Job + 2, // 11: baremetal.BareMetal.Submit:output_type -> baremetal.Job + 3, // 12: baremetal.BareMetal.JobStatus:output_type -> baremetal.JobList + 5, // 13: baremetal.BareMetal.CancelJobs:output_type -> baremetal.Error + 3, // 14: baremetal.BareMetal.FetchResults:output_type -> baremetal.JobList + 7, // 15: baremetal.BareMetal.UpdateJobs:output_type -> google.protobuf.Empty + 2, // 16: baremetal.BareMetal.RecoverJob:output_type -> baremetal.Job + 11, // [11:17] is the sub-list for method output_type + 5, // [5:11] is the sub-list for method input_type 5, // [5:5] is the sub-list for extension type_name 5, // [5:5] is the sub-list for extension extendee 0, // [0:5] is the sub-list for field type_name diff --git a/examples/baremetal/baremetal/baremetal.proto b/examples/baremetal/baremetal/baremetal.proto index 3b980e1b..f6f6a03d 100644 --- a/examples/baremetal/baremetal/baremetal.proto +++ b/examples/baremetal/baremetal/baremetal.proto @@ -12,6 +12,7 @@ service BareMetal { rpc CancelJobs (JobIDList) returns (Error); rpc FetchResults (JobIDList) returns (JobList); rpc UpdateJobs (google.protobuf.Empty) returns (google.protobuf.Empty); + rpc RecoverJob (Job) returns (Job); } // Submission is a job submission. diff --git a/examples/baremetal/baremetal/baremetal_grpc.pb.go b/examples/baremetal/baremetal/baremetal_grpc.pb.go index 2765164c..7bc914ed 100644 --- a/examples/baremetal/baremetal/baremetal_grpc.pb.go +++ b/examples/baremetal/baremetal/baremetal_grpc.pb.go @@ -25,6 +25,7 @@ const ( BareMetal_CancelJobs_FullMethodName = "/baremetal.BareMetal/CancelJobs" BareMetal_FetchResults_FullMethodName = "/baremetal.BareMetal/FetchResults" BareMetal_UpdateJobs_FullMethodName = "/baremetal.BareMetal/UpdateJobs" + BareMetal_RecoverJob_FullMethodName = "/baremetal.BareMetal/RecoverJob" ) // BareMetalClient is the client API for BareMetal service. @@ -36,6 +37,7 @@ type BareMetalClient interface { CancelJobs(ctx context.Context, in *JobIDList, opts ...grpc.CallOption) (*Error, error) FetchResults(ctx context.Context, in *JobIDList, opts ...grpc.CallOption) (*JobList, error) UpdateJobs(ctx context.Context, in *emptypb.Empty, opts ...grpc.CallOption) (*emptypb.Empty, error) + RecoverJob(ctx context.Context, in *Job, opts ...grpc.CallOption) (*Job, error) } type bareMetalClient struct { @@ -96,6 +98,16 @@ func (c *bareMetalClient) UpdateJobs(ctx context.Context, in *emptypb.Empty, opt return out, nil } +func (c *bareMetalClient) RecoverJob(ctx context.Context, in *Job, opts ...grpc.CallOption) (*Job, error) { + cOpts := append([]grpc.CallOption{grpc.StaticMethod()}, opts...) + out := new(Job) + err := c.cc.Invoke(ctx, BareMetal_RecoverJob_FullMethodName, in, out, cOpts...) + if err != nil { + return nil, err + } + return out, nil +} + // BareMetalServer is the server API for BareMetal service. // All implementations must embed UnimplementedBareMetalServer // for forward compatibility. @@ -105,6 +117,7 @@ type BareMetalServer interface { CancelJobs(context.Context, *JobIDList) (*Error, error) FetchResults(context.Context, *JobIDList) (*JobList, error) UpdateJobs(context.Context, *emptypb.Empty) (*emptypb.Empty, error) + RecoverJob(context.Context, *Job) (*Job, error) mustEmbedUnimplementedBareMetalServer() } @@ -130,6 +143,9 @@ func (UnimplementedBareMetalServer) FetchResults(context.Context, *JobIDList) (* func (UnimplementedBareMetalServer) UpdateJobs(context.Context, *emptypb.Empty) (*emptypb.Empty, error) { return nil, status.Errorf(codes.Unimplemented, "method UpdateJobs not implemented") } +func (UnimplementedBareMetalServer) RecoverJob(context.Context, *Job) (*Job, error) { + return nil, status.Errorf(codes.Unimplemented, "method RecoverJob not implemented") +} func (UnimplementedBareMetalServer) mustEmbedUnimplementedBareMetalServer() {} func (UnimplementedBareMetalServer) testEmbeddedByValue() {} @@ -241,6 +257,24 @@ func _BareMetal_UpdateJobs_Handler(srv interface{}, ctx context.Context, dec fun return interceptor(ctx, in, info, handler) } +func _BareMetal_RecoverJob_Handler(srv interface{}, ctx context.Context, dec func(interface{}) error, interceptor grpc.UnaryServerInterceptor) (interface{}, error) { + in := new(Job) + if err := dec(in); err != nil { + return nil, err + } + if interceptor == nil { + return srv.(BareMetalServer).RecoverJob(ctx, in) + } + info := &grpc.UnaryServerInfo{ + Server: srv, + FullMethod: BareMetal_RecoverJob_FullMethodName, + } + handler := func(ctx context.Context, req interface{}) (interface{}, error) { + return srv.(BareMetalServer).RecoverJob(ctx, req.(*Job)) + } + return interceptor(ctx, in, info, handler) +} + // BareMetal_ServiceDesc is the grpc.ServiceDesc for BareMetal service. // It's only intended for direct use with grpc.RegisterService, // and not to be introspected or modified (even as a copy) @@ -268,6 +302,10 @@ var BareMetal_ServiceDesc = grpc.ServiceDesc{ MethodName: "UpdateJobs", Handler: _BareMetal_UpdateJobs_Handler, }, + { + MethodName: "RecoverJob", + Handler: _BareMetal_RecoverJob_Handler, + }, }, Streams: []grpc.StreamDesc{}, Metadata: "baremetal/baremetal.proto", diff --git a/examples/baremetal/client.go b/examples/baremetal/client.go index 420da7c5..0eafe941 100644 --- a/examples/baremetal/client.go +++ b/examples/baremetal/client.go @@ -114,3 +114,17 @@ func (cl *Client) FetchResults(resultsGlob string, ids ...int) ([]*Job, error) { func (cl *Client) UpdateJobs() { return } + +// RecoverJob recovers a job which has been lost somehow. +// It just adds the given job to the job table. +func (cl *Client) RecoverJob(job *Job) (*Job, error) { + ctx, cancel := context.WithTimeout(context.Background(), cl.Timeout) + defer cancel() + + pjob := JobToPB(job) + rjob, err := cl.client.RecoverJob(ctx, pjob) + if err != nil { + return nil, errors.Log(fmt.Errorf("RecoverJob failed: %v", err)) + } + return JobFromPB(rjob), nil +} diff --git a/examples/baremetal/cmd/baremetal/main.go b/examples/baremetal/cmd/baremetal/main.go index 7a14d84b..b10ebf74 100644 --- a/examples/baremetal/cmd/baremetal/main.go +++ b/examples/baremetal/cmd/baremetal/main.go @@ -73,6 +73,14 @@ func (s *server) UpdateJobs(_ context.Context, in *emptypb.Empty) (*emptypb.Empt return &emptypb.Empty{}, nil } +// RecoverJob +func (s *server) RecoverJob(_ context.Context, in *pb.Job) (*pb.Job, error) { + slog.Info("RecoverJob") + job, err := s.bm.RecoverJob(baremetal.JobFromPB(in)) + errors.Log(err) + return baremetal.JobToPB(job), err +} + func main() { logx.UserLevel = slog.LevelInfo opts := cli.DefaultOptions("baremetal", "Bare metal server for job running on bare servers over ssh") diff --git a/examples/baremetal/jobs.go b/examples/baremetal/jobs.go index acace12d..b6581606 100644 --- a/examples/baremetal/jobs.go +++ b/examples/baremetal/jobs.go @@ -211,7 +211,9 @@ func (bm *BareMetal) runPendingJobs() (int, error) { return nRun, errors.Join(errs...) } -// cancelJobs cancels list of job IDs. Returns error for jobs not found. +// cancelJobs cancels list of job IDs. +// This is robust to jobs that are not found, and will +// create data for them, as a way of recovering the status of these jobs. func (bm *BareMetal) cancelJobs(jobs ...int) error { var errs []error for _, jid := range jobs { @@ -413,3 +415,12 @@ func (bm *BareMetal) setServerUsedFromJobs() error { } return errors.Join(errs...) } + +// recoverJob attempts to recover the job information +// from given job, using remote or local data. +func (bm *BareMetal) recoverJob(job *Job) (*Job, error) { + // do we need to do anything actually? + bm.Active.Add(job.ID, job) + bm.saveState() + return job, nil +} diff --git a/examples/baremetal/jobs.goal b/examples/baremetal/jobs.goal index 6c7f9014..ded1155c 100644 --- a/examples/baremetal/jobs.goal +++ b/examples/baremetal/jobs.goal @@ -210,7 +210,9 @@ func (bm *BareMetal) runPendingJobs() (int, error) { return nRun, errors.Join(errs...) } -// cancelJobs cancels list of job IDs. Returns error for jobs not found. +// cancelJobs cancels list of job IDs. +// This is robust to jobs that are not found, and will +// create data for them, as a way of recovering the status of these jobs. func (bm *BareMetal) cancelJobs(jobs ...int) error { var errs []error for _, jid := range jobs { @@ -413,3 +415,12 @@ func (bm *BareMetal) setServerUsedFromJobs() error { return errors.Join(errs...) } +// recoverJob attempts to recover the job information +// from given job, using remote or local data. +func (bm *BareMetal) recoverJob(job *Job) (*Job, error) { + // do we need to do anything actually? + bm.Active.Add(job.ID, job) + bm.saveState() + return job, nil +} + diff --git a/examples/simmer/bare.go b/examples/simmer/bare.go index e3b1893f..a9395144 100644 --- a/examples/simmer/bare.go +++ b/examples/simmer/bare.go @@ -13,6 +13,7 @@ import ( "os" "strconv" "strings" + "time" "cogentcore.org/core/base/errors" "cogentcore.org/core/core" @@ -147,3 +148,32 @@ func (sr *Simmer) CancelJobsBare(jobs []string) { } sr.BareMetal.CancelJobs(jnos...) } + +func (sr *Simmer) RecoverJobsBare(jobs []string) { + for _, jid := range jobs { + jno := 0 + sjob := sr.ValueForJob(jid, "ServerJob") + if sjob != "" { + jno = errors.Log1(strconv.Atoi(sjob)) + } else { + fmt.Println("job does not have a ServerJob id") + } + job := &baremetal.Job{ID: jno} + job.Status.SetString(sr.ValueForJob(jid, "Status")) + job.Path = sr.ServerJobPath(jid) + job.Source = sr.Config.Project + job.Script = "job.sbatch" + job.ResultsGlob = sr.Config.FetchFiles + job.Submit = errors.Log1(time.Parse(sr.Config.TimeFormat, sr.ValueForJob(jid, "Submit"))) + job.Start = errors.Log1(time.Parse(sr.Config.TimeFormat, sr.ValueForJob(jid, "Start"))) + job.End = errors.Log1(time.Parse(sr.Config.TimeFormat, sr.ValueForJob(jid, "End"))) + job.ServerName = sr.ValueForJob(jid, "Server") + // todo: PID + // jpath := sr.JobPath(jid) + // @0 + // cd {jpath} + // job.Status = goalib.ReadFile("job.status") + // job.Label = goalib.ReadFile("job.label") + sr.BareMetal.RecoverJob(job) + } +} diff --git a/examples/simmer/bare.goal b/examples/simmer/bare.goal index ed63fc3b..c5abfab1 100644 --- a/examples/simmer/bare.goal +++ b/examples/simmer/bare.goal @@ -10,6 +10,7 @@ import ( "io" "strconv" "strings" + "time" "os" "cogentcore.org/core/base/errors" @@ -146,3 +147,32 @@ func (sr *Simmer) CancelJobsBare(jobs []string) { sr.BareMetal.CancelJobs(jnos...) } +func (sr *Simmer) RecoverJobsBare(jobs []string) { + for _, jid := range jobs { + jno := 0 + sjob := sr.ValueForJob(jid, "ServerJob") + if sjob != "" { + jno = errors.Log1(strconv.Atoi(sjob)) + } else { + fmt.Println("job does not have a ServerJob id") + } + job := &baremetal.Job{ID: jno} + job.Status.SetString(sr.ValueForJob(jid, "Status")) + job.Path = sr.ServerJobPath(jid) + job.Source = sr.Config.Project + job.Script = "job.sbatch" + job.ResultsGlob = sr.Config.FetchFiles + job.Submit = errors.Log1(time.Parse(sr.Config.TimeFormat, sr.ValueForJob(jid, "Submit"))) + job.Start = errors.Log1(time.Parse(sr.Config.TimeFormat, sr.ValueForJob(jid, "Start"))) + job.End = errors.Log1(time.Parse(sr.Config.TimeFormat, sr.ValueForJob(jid, "End"))) + job.ServerName = sr.ValueForJob(jid, "Server") + // todo: PID + // jpath := sr.JobPath(jid) + // @0 + // cd {jpath} + // job.Status = goalib.ReadFile("job.status") + // job.Label = goalib.ReadFile("job.label") + sr.BareMetal.RecoverJob(job) + } +} + diff --git a/examples/simmer/jobs.go b/examples/simmer/jobs.go index e0a2bef9..f6481f3a 100644 --- a/examples/simmer/jobs.go +++ b/examples/simmer/jobs.go @@ -424,3 +424,22 @@ func (sr *Simmer) Archive() { //types:add sr.UpdateSims() }) } + +// Recover recovers the jobs selected in the Jobs table, +// with a confirmation prompt. +func (sr *Simmer) Recover() { //types:add + tv := sr.JobsTableView + jobs := tv.SelectedColumnStrings("JobID") + if len(jobs) == 0 { + core.MessageSnackbar(sr, "No jobs selected for cancel") + return + } + lab.PromptOKCancel(sr, "Ok to recover these jobs: "+strings.Join(jobs, " "), func() { + // if sr.IsSlurm() { + // sr.CancelJobsSlurm(jobs) + // } else { + sr.RecoverJobsBare(jobs) + // } + sr.UpdateSims() + }) +} diff --git a/examples/simmer/jobs.goal b/examples/simmer/jobs.goal index 73e02b1d..25954acc 100644 --- a/examples/simmer/jobs.goal +++ b/examples/simmer/jobs.goal @@ -423,4 +423,23 @@ func (sr *Simmer) Archive() { //types:add }) } +// Recover recovers the jobs selected in the Jobs table, +// with a confirmation prompt. +func (sr *Simmer) Recover() { //types:add + tv := sr.JobsTableView + jobs := tv.SelectedColumnStrings("JobID") + if len(jobs) == 0 { + core.MessageSnackbar(sr, "No jobs selected for cancel") + return + } + lab.PromptOKCancel(sr, "Ok to recover these jobs: " + strings.Join(jobs, " "), func() { + // if sr.IsSlurm() { + // sr.CancelJobsSlurm(jobs) + // } else { + sr.RecoverJobsBare(jobs) + // } + sr.UpdateSims() + }) +} + diff --git a/examples/simmer/simmer.go b/examples/simmer/simmer.go index 17521d16..89f6f67b 100644 --- a/examples/simmer/simmer.go +++ b/examples/simmer/simmer.go @@ -261,6 +261,9 @@ func (sr *Simmer) MakeToolbar(p *tree.Plan) { tree.Add(p, func(w *core.FuncButton) { w.SetFunc(sr.Archive).SetIcon(icons.Archive) }) + tree.Add(p, func(w *core.FuncButton) { + w.SetFunc(sr.Recover).SetIcon(icons.Archive) + }) tree.Add(p, func(w *core.FuncButton) { w.SetFunc(sr.EditConfig).SetIcon(icons.Edit) }) diff --git a/examples/simmer/typegen.go b/examples/simmer/typegen.go index dab15b16..e447236f 100644 --- a/examples/simmer/typegen.go +++ b/examples/simmer/typegen.go @@ -23,7 +23,7 @@ var _ = types.AddType(&types.Type{Name: "main.Configuration", IDName: "configura var _ = types.AddType(&types.Type{Name: "main.Result", IDName: "result", Doc: "Result has info for one loaded result, as a table.Table", Fields: []types.Field{{Name: "JobID", Doc: "job id for results"}, {Name: "Label", Doc: "short label used as a legend in the plot"}, {Name: "Message", Doc: "description of job"}, {Name: "Args", Doc: "args used in running job"}, {Name: "Path", Doc: "path to data"}, {Name: "Table", Doc: "result data"}}}) -var _ = types.AddType(&types.Type{Name: "main.Simmer", IDName: "simmer", Doc: "Simmer manages the running and data analysis of results from simulations\nthat are run on remote server(s), within a Cogent Lab browser environment,\nwith the files as the left panel, and the Tabber as the right panel.", Methods: []types.Method{{Name: "FetchJobBare", Doc: "FetchJobBare downloads results files from bare metal server.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"jid", "force"}}, {Name: "EditConfig", Doc: "EditConfig edits the configuration", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "Jobs", Doc: "Jobs updates the Jobs tab with a Table showing all the Jobs\nwith their meta data. Uses the dbmeta.toml data compiled from\nthe Status function.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "UpdateSims", Doc: "Jobs updates the Jobs tab with a Table showing all the Jobs\nwith their meta data. Uses the dbmeta.toml data compiled from\nthe Status function.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "Queue", Doc: "Queue runs a queue query command on the server and shows the results.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "Status", Doc: "Status gets updated job.* files from the server for any job that\ndoesn't have a Finalized or Fetched status. It updates the\nstatus based on the server job status query, assigning a\nstatus of Finalized if job is done. Updates the dbmeta.toml\ndata based on current job data.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "Fetch", Doc: "Fetch retrieves all the .tsv data files from the server\nfor any jobs not already marked as Fetched.\nOperates on the jobs selected in the Jobs table,\nor on all jobs if none selected.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "Cancel", Doc: "Cancel cancels the jobs selected in the Jobs table,\nwith a confirmation prompt.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "Delete", Doc: "Delete deletes the selected Jobs, with a confirmation prompt.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "Archive", Doc: "Archive moves the selected Jobs to the Archive directory,\nlocally, and deletes them from the server,\nfor results that are useful but not immediately relevant.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "Results", Doc: "Results loads specific .tsv data files from the jobs selected\nin the Jobs table, into the Results table. There are often\nmultiple result files per job, so this step is necessary to\nchoose which such files to select for plotting.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "Diff", Doc: "Diff shows the differences between two selected jobs, or if only\none job is selected, between that job and the current source directory.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "Plot", Doc: "Plot concatenates selected Results data files and generates a plot\nof the resulting data.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "PlotMean", Doc: "PlotMean concatenates selected Results data files and generates a plot\nof the resulting data, computing the mean over the values in\n[Config.GroupColumns] to group values (e.g., across Epochs).", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "Reset", Doc: "Reset resets the Results table", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "Submit", Doc: "Submit submits a job on the server.\nCreates a new job dir based on incrementing counter,\nsynchronizing the job files.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "Search", Doc: "Search runs parameter search jobs, one for each parameter.\nThe number of parameters is obtained by running the currently built\nsimulation executable locally with the -search-n argument, which\nreturns the total number of parameter searches to run.\nTHUS, YOU MUST BUILD THE LOCAL SIM WITH THE PARAM SEARCH CONFIGURED.\nThen, it launches that number of jobs with -search-at values from 1..n.\nThe jobs should write a job.label file for the searched parameter.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}}, Embeds: []types.Field{{Name: "Frame"}, {Name: "Browser"}}, Fields: []types.Field{{Name: "Config", Doc: "Config holds all the configuration settings."}, {Name: "JobsTableView", Doc: "JobsTableView is the view of the jobs table."}, {Name: "JobsTable", Doc: "JobsTable is the jobs Table with one row per job."}, {Name: "ResultsTableView", Doc: "ResultsTableView has the results table."}, {Name: "ResultsList", Doc: "ResultsList is the list of result records."}, {Name: "BareMetal", Doc: "BareMetal RPC client."}, {Name: "BareMetalActive", Doc: "Status info from BareMetal"}, {Name: "BareMetalActiveTable"}}}) +var _ = types.AddType(&types.Type{Name: "main.Simmer", IDName: "simmer", Doc: "Simmer manages the running and data analysis of results from simulations\nthat are run on remote server(s), within a Cogent Lab browser environment,\nwith the files as the left panel, and the Tabber as the right panel.", Methods: []types.Method{{Name: "FetchJobBare", Doc: "FetchJobBare downloads results files from bare metal server.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"jid", "force"}}, {Name: "EditConfig", Doc: "EditConfig edits the configuration", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "Jobs", Doc: "Jobs updates the Jobs tab with a Table showing all the Jobs\nwith their meta data. Uses the dbmeta.toml data compiled from\nthe Status function.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "UpdateSims", Doc: "Jobs updates the Jobs tab with a Table showing all the Jobs\nwith their meta data. Uses the dbmeta.toml data compiled from\nthe Status function.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "Queue", Doc: "Queue runs a queue query command on the server and shows the results.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "Status", Doc: "Status gets updated job.* files from the server for any job that\ndoesn't have a Finalized or Fetched status. It updates the\nstatus based on the server job status query, assigning a\nstatus of Finalized if job is done. Updates the dbmeta.toml\ndata based on current job data.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "Fetch", Doc: "Fetch retrieves all the .tsv data files from the server\nfor any jobs not already marked as Fetched.\nOperates on the jobs selected in the Jobs table,\nor on all jobs if none selected.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "Cancel", Doc: "Cancel cancels the jobs selected in the Jobs table,\nwith a confirmation prompt.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "Delete", Doc: "Delete deletes the selected Jobs, with a confirmation prompt.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "Archive", Doc: "Archive moves the selected Jobs to the Archive directory,\nlocally, and deletes them from the server,\nfor results that are useful but not immediately relevant.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "Recover", Doc: "Recover recovers the jobs selected in the Jobs table,\nwith a confirmation prompt.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "Results", Doc: "Results loads specific .tsv data files from the jobs selected\nin the Jobs table, into the Results table. There are often\nmultiple result files per job, so this step is necessary to\nchoose which such files to select for plotting.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "Diff", Doc: "Diff shows the differences between two selected jobs, or if only\none job is selected, between that job and the current source directory.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "Plot", Doc: "Plot concatenates selected Results data files and generates a plot\nof the resulting data.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "PlotMean", Doc: "PlotMean concatenates selected Results data files and generates a plot\nof the resulting data, computing the mean over the values in\n[Config.GroupColumns] to group values (e.g., across Epochs).", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "Reset", Doc: "Reset resets the Results table", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "Submit", Doc: "Submit submits a job on the server.\nCreates a new job dir based on incrementing counter,\nsynchronizing the job files.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "Search", Doc: "Search runs parameter search jobs, one for each parameter.\nThe number of parameters is obtained by running the currently built\nsimulation executable locally with the -search-n argument, which\nreturns the total number of parameter searches to run.\nTHUS, YOU MUST BUILD THE LOCAL SIM WITH THE PARAM SEARCH CONFIGURED.\nThen, it launches that number of jobs with -search-at values from 1..n.\nThe jobs should write a job.label file for the searched parameter.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}}, Embeds: []types.Field{{Name: "Frame"}, {Name: "Browser"}}, Fields: []types.Field{{Name: "Config", Doc: "Config holds all the configuration settings."}, {Name: "JobsTableView", Doc: "JobsTableView is the view of the jobs table."}, {Name: "JobsTable", Doc: "JobsTable is the jobs Table with one row per job."}, {Name: "ResultsTableView", Doc: "ResultsTableView has the results table."}, {Name: "ResultsList", Doc: "ResultsList is the list of result records."}, {Name: "BareMetal", Doc: "BareMetal RPC client."}, {Name: "BareMetalActive", Doc: "Status info from BareMetal"}, {Name: "BareMetalActiveTable"}}}) // NewSimmer returns a new [Simmer] with the given optional parent: // Simmer manages the running and data analysis of results from simulations From 9afaabf3768a743796dbcaf2e0e2de641e7d1432 Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Wed, 15 Oct 2025 09:26:42 +0200 Subject: [PATCH 05/97] pdf: add OnlineURL for docs --- docs/docs.go | 1 + 1 file changed, 1 insertion(+) diff --git a/docs/docs.go b/docs/docs.go index 9dff8684..ac19b7a3 100644 --- a/docs/docs.go +++ b/docs/docs.go @@ -22,6 +22,7 @@ func main() { b := core.NewBody("Cogent Lab") ct := content.NewContent(b).SetContent(econtent) ctx := ct.Context + content.OfflineURL = "https://cogentcore.org/lab" ctx.AddWikilinkHandler(htmlcore.GoDocWikilink("doc", "cogentcore.org/lab")) b.AddTopBar(func(bar *core.Frame) { tb := core.NewToolbar(bar) From 6f6835bdce0feae4b0ca817b49dc4039280908e7 Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Wed, 15 Oct 2025 10:59:43 +0200 Subject: [PATCH 06/97] pdf: xy plot Size role: min dot size is 1 dot. --- plot/plots/xy.go | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/plot/plots/xy.go b/plot/plots/xy.go index 4fb3ed5d..83e52fde 100644 --- a/plot/plots/xy.go +++ b/plot/plots/xy.go @@ -270,7 +270,7 @@ func (ln *XY) Plot(plt *plot.Plot) { } } if ln.Size != nil { - ln.Style.Point.Size.Dots *= float32(plt.SizeAxis.Norm(ln.Size.Float1D(i))) + ln.Style.Point.Size.Dots = 1 + ln.Style.Point.Size.Dots*float32(plt.SizeAxis.Norm(ln.Size.Float1D(i))) } ln.Style.Point.SetColorIndex(pc, i) ln.Style.Point.DrawShape(pc, math32.Vec2(ptx, pty)) From 2efc956ff003257519e316fa7a12cf9526fe6e7b Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Sat, 22 Nov 2025 09:25:23 +0100 Subject: [PATCH 07/97] pdf: update to core --- go.mod | 2 +- go.sum | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/go.mod b/go.mod index d16e8b87..e2f74ffa 100644 --- a/go.mod +++ b/go.mod @@ -10,7 +10,7 @@ go 1.23.4 // https://github.com/googleapis/go-genproto/issues/1015 require ( - cogentcore.org/core v0.3.13-0.20251014131306-9996ffd52fd8 + cogentcore.org/core v0.3.13-0.20251122080528-7dff9fe2d85b github.com/alecthomas/assert/v2 v2.6.0 github.com/cogentcore/readline v0.1.3 github.com/cogentcore/yaegi v0.0.0-20250622201820-b7838bdd95eb diff --git a/go.sum b/go.sum index 670dbcbd..1dffdaf7 100644 --- a/go.sum +++ b/go.sum @@ -1,5 +1,5 @@ -cogentcore.org/core v0.3.13-0.20251014131306-9996ffd52fd8 h1:p4wJuqxRDXAjv9uY3WhgFdWvxXKhkgivDi2AXM+wdAg= -cogentcore.org/core v0.3.13-0.20251014131306-9996ffd52fd8/go.mod h1:eDHnTCy1sBhAKN9NPsSCnBW3VAnwQBNA9nbGMo9r+Xs= +cogentcore.org/core v0.3.13-0.20251122080528-7dff9fe2d85b h1:YF5zD4OoObOKKI2C8atfgJ3giEw1ySTOCqxpUq3OyZs= +cogentcore.org/core v0.3.13-0.20251122080528-7dff9fe2d85b/go.mod h1:eDHnTCy1sBhAKN9NPsSCnBW3VAnwQBNA9nbGMo9r+Xs= github.com/Bios-Marcel/wastebasket/v2 v2.0.3 h1:TkoDPcSqluhLGE+EssHu7UGmLgUEkWg7kNyHyyJ3Q9g= github.com/Bios-Marcel/wastebasket/v2 v2.0.3/go.mod h1:769oPCv6eH7ugl90DYIsWwjZh4hgNmMS3Zuhe1bH6KU= github.com/BurntSushi/toml v0.3.1/go.mod h1:xHWCNGjB5oqiDr8zfno3MHue2Ht5sIBksp03qcyfWMU= From 53a7c5b47a76f329d58af56806060093b9df2534 Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Sun, 30 Nov 2025 21:46:30 +0100 Subject: [PATCH 08/97] pdf: a number of fixes for gosl: docs, borrowing GPU device externally, slvec for patting compatible vec4 fields. tensor.CopyFromLargerShape. key crash fix in tensorcore GridView with non-fixed range. --- docs/content/gosl.md | 29 +++++-- docs/content/math.md | 4 +- docs/content/tensorfs.md | 4 +- gosl/alignsl/alignsl.go | 1 + gosl/examples/basic/compute.go | 7 +- gosl/examples/basic/compute.goal | 7 +- gosl/examples/basic/gosl.go | 35 ++++++-- gosl/examples/basic/main.go | 1 + gosl/examples/basic/shaders/Atomic.wgsl | 2 +- gosl/examples/basic/shaders/Compute.wgsl | 101 ++++++++++++----------- gosl/examples/rand/gosl.go | 29 +++++-- gosl/examples/rand/rand.go | 1 + gosl/examples/rand/rand.goal | 1 + gosl/gotosl/extract.go | 1 + gosl/gotosl/gengpu.go | 29 +++++-- gosl/gotosl/nodes.go | 7 ++ gosl/gotosl/sledits.go | 6 ++ gosl/gotosl/testdata/Compute.golden | 4 +- gosl/gotosl/testdata/CycleUpdt.golden | 2 + gosl/gotosl/testdata/basic.go | 8 +- gosl/gotosl/testdata/basic.goal | 8 +- gosl/gotosl/testdata/gosl.golden | 29 +++++-- gosl/sltype/int.go | 20 +++++ gosl/slvec/slvec.go | 63 ++++++++++++++ tensor/convert.go | 23 +++++- 25 files changed, 329 insertions(+), 93 deletions(-) create mode 100644 gosl/slvec/slvec.go diff --git a/docs/content/gosl.md b/docs/content/gosl.md index 3301e822..d80e3955 100644 --- a/docs/content/gosl.md +++ b/docs/content/gosl.md @@ -1,6 +1,10 @@ -**Gosl** allows you to write Go programs that run on [[GPU]] hardware, by transpiling Go into the WGSL shader language used by [WebGPU](https://www.w3.org/TR/webgpu/), thereby establishing the _Go shader language_. ++++ +Name = "GoSL" ++++ -Gosl uses the [core gpu](https://github.com/cogentcore/core/tree/main/gpu) compute shader system, and operates within the overall [[Goal]] framework of an augmented version of the Go language. +**GoSL** allows you to write Go programs that run on [[GPU]] hardware, by transpiling Go into the WGSL shader language used by [WebGPU](https://www.w3.org/TR/webgpu/), thereby establishing the _Go shader language_. + +GoSL uses the [core gpu](https://github.com/cogentcore/core/tree/main/gpu) compute shader system, and operates within the overall [[Goal]] framework of an augmented version of the Go language. The relevant regions of Go code to be run on the GPU are tagged using the `//gosl:start` and `//gosl:end` comment directives, and this code must only use basic expressions and concrete types that will compile correctly in a GPU shader (see [[#Restrictions]] below). Method functions and pass-by-reference pointer arguments to `struct` types are supported and incur no additional compute cost due to inlining (see notes below for more detail). @@ -27,6 +31,8 @@ There are two key elements for GPU-enabled code: 2. [[#Global variables]] on which the kernel functions _exclusively_ operate: all relevant data must be specifically copied from the CPU to the GPU and back. As explained in the [[GPU]] docs, each GPU compute shader is effectively a _standalone_ program operating on these global variables. To replicate this environment on the CPU, so the code works in both contexts, we need to make these variables global in the CPU (Go) environment as well. +**IMPORTANT:** All tensor variables must be sent to the GPU at least once before running, because the generated code does not know the size of the tensor until this is done! This is true even if a variable is just a results variable that does not logically need to be uploaded to the GPU -- the overhead at startup for a single such transfer is not worth the extra complexity of creating the necessary alternative init code. + `gosl` generates a file named `gosl.go` in your package directory that initializes the GPU with all of the global variables, and functions for running the kernels and syncing the gobal variable data back and forth between the CPu and GPU. ## Kernels @@ -35,11 +41,18 @@ Each distinct compute kernel must be tagged with a `//gosl:kernel` comment direc ```go // Compute does the main computation. func Compute(i uint32) { //gosl:kernel + if i >= Params[0].DataLen { // note: essential to bounds check b/c i in 64 blocks + return + } Params[0].IntegFromRaw(int(i)) } ``` -The kernel functions receive a `uint32` index argument, and use this to index into the global variables containing the relevant data. Typically the kernel code itself just calls other relevant function(s) using the index, as in the above example. Critically, _all_ of the data that a kernel function ultimately depends on must be contained with the global variables, and these variables must have been sync'd up to the GPU from the CPU prior to running the kernel (more on this below). +The kernel functions receive a `uint32` index argument, and use this to index into the global variables containing the relevant data. + +**IMPORTANT:** the dispatch of kernels on the GPU is in blocks of 64 processors, so i will exceed the number that you pass into the `Run` function! It is essential to check bounds in every kernel. + +Typically the kernel code itself just calls other relevant function(s) using the index, as in the above example. Critically, _all_ of the data that a kernel function ultimately depends on must be contained with the global variables, and these variables must have been sync'd up to the GPU from the CPU prior to running the kernel (more on this below). In the CPU mode, the kernel is effectively run in a `for` loop like this: ```go @@ -68,7 +81,7 @@ var ( ``` All such variables must be either: -1. A `slice` of GPU-alignment compatible `struct` types, such as `ParamStruct` in the above example. In general such structs should be marked as `//gosl:read-only` due to various challenges associated with writing to structs, detailed below. +1. A `slice` of GPU-alignment compatible `struct` types, such as `ParamStruct` in the above example. In general such structs should be marked as `//gosl:read-only` due to various challenges associated with writing to structs, detailed below. Due to lack of package-relative naming in the final WGSL file, any struct type defined in a sub package will show up unqualified in the generated `gosl.go` file, so a type alias is required to allow the resulting Go file to build properly. 2. A `tensor` of a GPU-compatible elemental data type (`float32`, `uint32`, or `int32`), with the number of dimensions indicated by the `//gosl:dims ` tag as shown above. This is the preferred type for writable data. You can also just declare a slice of elemental GPU-compatible data values such as `float32`, but it is generally preferable to use the tensor instead, because it has built-in support for higher-dimensional indexing in a way that is transparent between CPU and GPU. @@ -232,7 +245,7 @@ var PathGBuf: array>; atomicAdd(&PathGBuf[idx], val); ``` -This also unfortunately has the side-effect that you cannot do _non-atomic_ operations on atomic variables, as discussed extensively here: https://github.com/gpuweb/gpuweb/issues/2377 Gosl automatically detects the use of atomic functions on GPU variables, and tags them as atomic. +This also unfortunately has the side-effect that you cannot do _non-atomic_ operations on atomic variables, as discussed extensively here: https://github.com/gpuweb/gpuweb/issues/2377 GoSL automatically detects the use of atomic functions on GPU variables, and tags them as atomic. ## Random numbers: slrand @@ -244,9 +257,13 @@ See [[doc:gosl/slrand]] for a shader-optimized random number generation package, //gosl:end mycode ``` +## WGSL vector variables from math32.Vector2 etc: not yet + +WGSL supports variables like `vec4` which is equivalent to `math32.Vector4`. Unfortunately, it would be difficult to have simultaneous, transparent support for both of these types across Go and WGSL, requiring rewriting expressions on the WGSL side. It is possible, but would take a fair amount of work, and is not yet planned. + ## Performance With sufficiently large N, and ignoring the data copying setup time, around ~80x speedup is typical on a Macbook Pro with M1 processor. The `rand` example produces a 175x speedup! -## Gosl pages + diff --git a/docs/content/math.md b/docs/content/math.md index d97a917c..21b89bef 100644 --- a/docs/content/math.md +++ b/docs/content/math.md @@ -64,7 +64,7 @@ fmt.Println("d:", d) See [[tensor math#Alignment of shapes]] for more details on [[tensor math]] operations, using the NumPy [broadcasting](https://numpy.org/doc/stable/user/basics.broadcasting.html) logic. -### Tensorfs +### TensorFS In an interactive Goal shell (which we simulate here in the docs), variables in math mode are automatically saved to the [[tensorfs]] virtual data filesystem: @@ -371,7 +371,7 @@ todo: huge amount of work needed to support complex numbers throughout! | . | . |`np.fft.ifft(a)` | inverse Fourier transform of `a` | | . | . |`signal.resample(x, np.ceil(len(x)/q))` | downsample with low-pass filtering | -### Tensorfs +### TensorFS The [[tensorfs]] data filesystem provides a global filesystem-like workspace for storing tensor data, and [[Goal]] has special commands and functions to facilitate interacting with it. diff --git a/docs/content/tensorfs.md b/docs/content/tensorfs.md index 2a42e84a..7cc4c102 100644 --- a/docs/content/tensorfs.md +++ b/docs/content/tensorfs.md @@ -1,8 +1,8 @@ +++ -Categories = ["Tensorfs"] +Name = "TensorFS" +++ -**tensorfs** provides a virtual filesystem for [[tensor]] data, which can be accessed for example in [[Goal]] [[math]] mode expressions, like the variable storage system in [IPython / Jupyter](https://ipython.readthedocs.io/en/stable/interactive/tutorial.html), with the advantage that the hierarchical structure of a filesystem allows data to be organized in more intuitive and effective ways. For example, data at different time scales can be put into different directories, or multiple different statistics computed on a given set of data can be put into a subdirectory. [[stats#Groups]] creates pivot-table style groups of values as directories, for example. +**TensorFS** provides a virtual filesystem for [[tensor]] data, which can be accessed for example in [[Goal]] [[math]] mode expressions, like the variable storage system in [IPython / Jupyter](https://ipython.readthedocs.io/en/stable/interactive/tutorial.html), with the advantage that the hierarchical structure of a filesystem allows data to be organized in more intuitive and effective ways. For example, data at different time scales can be put into different directories, or multiple different statistics computed on a given set of data can be put into a subdirectory. [[stats#Groups]] creates pivot-table style groups of values as directories, for example. `tensorfs` implements the Go [fs](https://pkg.go.dev/io/fs) interface, and can be accessed using fs-general tools, including the cogent core `filetree` and the [[Goal]] shell. diff --git a/gosl/alignsl/alignsl.go b/gosl/alignsl/alignsl.go index 27d39250..e62247a0 100644 --- a/gosl/alignsl/alignsl.go +++ b/gosl/alignsl/alignsl.go @@ -84,6 +84,7 @@ func CheckStruct(cx *Context, st *types.Struct, stName string) bool { kind := bt.Kind() if !(kind == types.Uint32 || kind == types.Int32 || kind == types.Float32 || kind == types.Uint64) { hasErr = cx.AddError(fmt.Sprintf(" %s: basic type != [U]Int32 or Float32: %s", fl.Name(), bt.String()), hasErr, stName) + fmt.Println("kind:", kind, "ft:", ft.String()) } } else { if sst, is := ut.(*types.Struct); is { diff --git a/gosl/examples/basic/compute.go b/gosl/examples/basic/compute.go index dcdeac40..ee1141ce 100644 --- a/gosl/examples/basic/compute.go +++ b/gosl/examples/basic/compute.go @@ -67,7 +67,9 @@ type ParamStruct struct { // 1/Tau Dt float32 - pad float32 + // number of data items -- must have avail for GPU to exclude extra. + DataLen uint32 + pad1 float32 Sub SubStruct @@ -93,6 +95,9 @@ func (ps *ParamStruct) IntegFromRaw(idx int) { // Compute does the main computation. func Compute(i uint32) { //gosl:kernel + if i >= Params[0].DataLen { // note: essential to bounds check b/c i in 64 blocks + return + } Params[0].IntegFromRaw(int(i)) } diff --git a/gosl/examples/basic/compute.goal b/gosl/examples/basic/compute.goal index 6ef71331..4ad27883 100644 --- a/gosl/examples/basic/compute.goal +++ b/gosl/examples/basic/compute.goal @@ -62,7 +62,9 @@ type ParamStruct struct { // 1/Tau Dt float32 - pad float32 + // number of data items -- must have avail for GPU to exclude extra. + DataLen uint32 + pad1 float32 Sub SubStruct @@ -88,6 +90,9 @@ func (ps *ParamStruct) IntegFromRaw(idx int) { // Compute does the main computation. func Compute(i uint32) { //gosl:kernel + if i >= Params[0].DataLen { // note: essential to bounds check b/c i in 64 blocks + return + } Params[0].IntegFromRaw(int(i)) } diff --git a/gosl/examples/basic/gosl.go b/gosl/examples/basic/gosl.go index b22ad45b..6696b5d6 100644 --- a/gosl/examples/basic/gosl.go +++ b/gosl/examples/basic/gosl.go @@ -15,9 +15,19 @@ import ( var shaders embed.FS var ( - // ComputeGPU is the compute gpu device + // GPUInitialized is true once the GPU system has been initialized. + // Prevents multiple initializations. + GPUInitialized bool + + // ComputeGPU is the compute gpu device. + // Set this prior to calling GPUInit() to use an existing device. ComputeGPU *gpu.GPU + // BorrowedGPU is true if our ComputeGPU is set externally, + // versus created specifically for this system. If external, + // we don't release it. + BorrowedGPU bool + // UseGPU indicates whether to use GPU vs. CPU. UseGPU bool ) @@ -41,11 +51,17 @@ var TensorStrides tensor.Uint32 // configuring system(s), variables and kernels. // It is safe to call multiple times: detects if already run. func GPUInit() { - if ComputeGPU != nil { + if GPUInitialized { return } - gp := gpu.NewComputeGPU() - ComputeGPU = gp + GPUInitialized = true + if ComputeGPU == nil { // set prior to this call to use an external + ComputeGPU = gpu.NewComputeGPU() + } else { + BorrowedGPU = true + } + gp := ComputeGPU + _ = fmt.Sprintf("%g",math.NaN()) // keep imports happy { sy := gpu.NewComputeSystem(gp, "Default") @@ -103,10 +119,11 @@ func GPURelease() { GPUSystem = nil } - if ComputeGPU != nil { + if !BorrowedGPU && ComputeGPU != nil { ComputeGPU.Release() - ComputeGPU = nil + } + ComputeGPU = nil } // RunAtomic runs the Atomic kernel with given number of elements, @@ -222,7 +239,7 @@ func ToGPU(vars ...GPUVars) { v, _ := syVars.ValueByIndex(0, "Params", 0) gpu.SetValueFrom(v, Params) case DataVar: - bsz := 536870912 + bsz := 536870904 n := Data.Len() nb := int(math.Ceil(float64(n) / float64(bsz))) for bi := range nb { @@ -274,7 +291,7 @@ func ReadFromGPU(vars ...GPUVars) { v, _ := syVars.ValueByIndex(0, "Params", 0) v.GPUToRead(sy.CommandEncoder) case DataVar: - bsz := 536870912 + bsz := 536870904 n := Data.Len() nb := int(math.Ceil(float64(n) / float64(bsz))) for bi := range nb { @@ -299,7 +316,7 @@ func SyncFromGPU(vars ...GPUVars) { v.ReadSync() gpu.ReadToBytes(v, Params) case DataVar: - bsz := 536870912 + bsz := 536870904 n := Data.Len() nb := int(math.Ceil(float64(n) / float64(bsz))) for bi := range nb { diff --git a/gosl/examples/basic/main.go b/gosl/examples/basic/main.go index 2952cacb..e71e9080 100644 --- a/gosl/examples/basic/main.go +++ b/gosl/examples/basic/main.go @@ -39,6 +39,7 @@ func main() { Data = tensor.NewFloat32() Data.SetShapeSizes(n, 3) nt := Data.Len() + Params[0].DataLen = uint32(nt) IntData = tensor.NewInt32() IntData.SetShapeSizes(n, 3) diff --git a/gosl/examples/basic/shaders/Atomic.wgsl b/gosl/examples/basic/shaders/Atomic.wgsl index 4a78c6b4..81700ea0 100644 --- a/gosl/examples/basic/shaders/Atomic.wgsl +++ b/gosl/examples/basic/shaders/Atomic.wgsl @@ -35,7 +35,7 @@ struct SubStruct { struct ParamStruct { Tau: f32, Dt: f32, - pad: f32, + DataLen: u32, pad1: f32, Sub: SubStruct, } diff --git a/gosl/examples/basic/shaders/Compute.wgsl b/gosl/examples/basic/shaders/Compute.wgsl index a64c2e67..c136db09 100644 --- a/gosl/examples/basic/shaders/Compute.wgsl +++ b/gosl/examples/basic/shaders/Compute.wgsl @@ -33,181 +33,181 @@ fn main(@builtin(workgroup_id) wgid: vec3, @builtin(num_workgroups) nwg: ve } fn DataGet(ix: u32) -> f32 { - let ii = ix / 536870912; + let ii = ix / 536870904; switch ii { case u32(0): { return Data0[ix]; } case u32(1): { - return Data1[ix - 536870912]; + return Data1[ix - 536870904]; } case u32(2): { - return Data2[ix - 1073741824]; + return Data2[ix - 1073741808]; } case u32(3): { - return Data3[ix - 1610612736]; + return Data3[ix - 1610612712]; } case u32(4): { - return Data4[ix - 2147483648]; + return Data4[ix - 2147483616]; } case u32(5): { - return Data5[ix - 2684354560]; + return Data5[ix - 2684354520]; } case u32(6): { - return Data6[ix - 3221225472]; + return Data6[ix - 3221225424]; } default: { - return Data7[ix - 3758096384]; + return Data7[ix - 3758096328]; } } } fn DataSet(vl: f32, ix: u32) { - let ii = ix / 536870912; + let ii = ix / 536870904; switch ii { case u32(0): { Data0[ix] = vl; } case u32(1): { - Data1[ix - 536870912] = vl; + Data1[ix - 536870904] = vl; } case u32(2): { - Data2[ix - 1073741824] = vl; + Data2[ix - 1073741808] = vl; } case u32(3): { - Data3[ix - 1610612736] = vl; + Data3[ix - 1610612712] = vl; } case u32(4): { - Data4[ix - 2147483648] = vl; + Data4[ix - 2147483616] = vl; } case u32(5): { - Data5[ix - 2684354560] = vl; + Data5[ix - 2684354520] = vl; } case u32(6): { - Data6[ix - 3221225472] = vl; + Data6[ix - 3221225424] = vl; } default: { - Data7[ix - 3758096384] = vl; + Data7[ix - 3758096328] = vl; } } } fn DataSetAdd(vl: f32, ix: u32) { - let ii = ix / 536870912; + let ii = ix / 536870904; switch ii { case u32(0): { Data0[ix] += vl; } case u32(1): { - Data1[ix - 536870912] += vl; + Data1[ix - 536870904] += vl; } case u32(2): { - Data2[ix - 1073741824] += vl; + Data2[ix - 1073741808] += vl; } case u32(3): { - Data3[ix - 1610612736] += vl; + Data3[ix - 1610612712] += vl; } case u32(4): { - Data4[ix - 2147483648] += vl; + Data4[ix - 2147483616] += vl; } case u32(5): { - Data5[ix - 2684354560] += vl; + Data5[ix - 2684354520] += vl; } case u32(6): { - Data6[ix - 3221225472] += vl; + Data6[ix - 3221225424] += vl; } default: { - Data7[ix - 3758096384] += vl; + Data7[ix - 3758096328] += vl; } } } fn DataSetSub(vl: f32, ix: u32) { - let ii = ix / 536870912; + let ii = ix / 536870904; switch ii { case u32(0): { Data0[ix] -= vl; } case u32(1): { - Data1[ix - 536870912] -= vl; + Data1[ix - 536870904] -= vl; } case u32(2): { - Data2[ix - 1073741824] -= vl; + Data2[ix - 1073741808] -= vl; } case u32(3): { - Data3[ix - 1610612736] -= vl; + Data3[ix - 1610612712] -= vl; } case u32(4): { - Data4[ix - 2147483648] -= vl; + Data4[ix - 2147483616] -= vl; } case u32(5): { - Data5[ix - 2684354560] -= vl; + Data5[ix - 2684354520] -= vl; } case u32(6): { - Data6[ix - 3221225472] -= vl; + Data6[ix - 3221225424] -= vl; } default: { - Data7[ix - 3758096384] -= vl; + Data7[ix - 3758096328] -= vl; } } } fn DataSetMul(vl: f32, ix: u32) { - let ii = ix / 536870912; + let ii = ix / 536870904; switch ii { case u32(0): { Data0[ix] *= vl; } case u32(1): { - Data1[ix - 536870912] *= vl; + Data1[ix - 536870904] *= vl; } case u32(2): { - Data2[ix - 1073741824] *= vl; + Data2[ix - 1073741808] *= vl; } case u32(3): { - Data3[ix - 1610612736] *= vl; + Data3[ix - 1610612712] *= vl; } case u32(4): { - Data4[ix - 2147483648] *= vl; + Data4[ix - 2147483616] *= vl; } case u32(5): { - Data5[ix - 2684354560] *= vl; + Data5[ix - 2684354520] *= vl; } case u32(6): { - Data6[ix - 3221225472] *= vl; + Data6[ix - 3221225424] *= vl; } default: { - Data7[ix - 3758096384] *= vl; + Data7[ix - 3758096328] *= vl; } } } fn DataSetDiv(vl: f32, ix: u32) { - let ii = ix / 536870912; + let ii = ix / 536870904; switch ii { case u32(0): { Data0[ix] /= vl; } case u32(1): { - Data1[ix - 536870912] /= vl; + Data1[ix - 536870904] /= vl; } case u32(2): { - Data2[ix - 1073741824] /= vl; + Data2[ix - 1073741808] /= vl; } case u32(3): { - Data3[ix - 1610612736] /= vl; + Data3[ix - 1610612712] /= vl; } case u32(4): { - Data4[ix - 2147483648] /= vl; + Data4[ix - 2147483616] /= vl; } case u32(5): { - Data5[ix - 2684354560] /= vl; + Data5[ix - 2684354520] /= vl; } case u32(6): { - Data6[ix - 3221225472] /= vl; + Data6[ix - 3221225424] /= vl; } default: { - Data7[ix - 3758096384] /= vl; + Data7[ix - 3758096328] /= vl; } } } @@ -231,7 +231,7 @@ struct SubStruct { struct ParamStruct { Tau: f32, Dt: f32, - pad: f32, + DataLen: u32, pad1: f32, Sub: SubStruct, } @@ -250,6 +250,9 @@ fn ParamStruct_IntegFromRaw(ps: ParamStruct, idx: i32) { SubStruct_IntegFromRaw(ps.Sub, idx); } fn Compute(i: u32) { //gosl:kernel + if (i >= Params[0].DataLen) { // note: essential to bounds check b/c i in 64 blocks + return; + } let params=Params[0]; ParamStruct_IntegFromRaw(params, i32(i)); } diff --git a/gosl/examples/rand/gosl.go b/gosl/examples/rand/gosl.go index c5e1ef80..66a8fd00 100644 --- a/gosl/examples/rand/gosl.go +++ b/gosl/examples/rand/gosl.go @@ -15,9 +15,19 @@ import ( var shaders embed.FS var ( - // ComputeGPU is the compute gpu device + // GPUInitialized is true once the GPU system has been initialized. + // Prevents multiple initializations. + GPUInitialized bool + + // ComputeGPU is the compute gpu device. + // Set this prior to calling GPUInit() to use an existing device. ComputeGPU *gpu.GPU + // BorrowedGPU is true if our ComputeGPU is set externally, + // versus created specifically for this system. If external, + // we don't release it. + BorrowedGPU bool + // UseGPU indicates whether to use GPU vs. CPU. UseGPU bool ) @@ -41,11 +51,17 @@ var TensorStrides tensor.Uint32 // configuring system(s), variables and kernels. // It is safe to call multiple times: detects if already run. func GPUInit() { - if ComputeGPU != nil { + if GPUInitialized { return } - gp := gpu.NewComputeGPU() - ComputeGPU = gp + GPUInitialized = true + if ComputeGPU == nil { // set prior to this call to use an external + ComputeGPU = gpu.NewComputeGPU() + } else { + BorrowedGPU = true + } + gp := ComputeGPU + _ = fmt.Sprintf("%g",math.NaN()) // keep imports happy { sy := gpu.NewComputeSystem(gp, "Default") @@ -81,10 +97,11 @@ func GPURelease() { GPUSystem = nil } - if ComputeGPU != nil { + if !BorrowedGPU && ComputeGPU != nil { ComputeGPU.Release() - ComputeGPU = nil + } + ComputeGPU = nil } // RunCompute runs the Compute kernel with given number of elements, diff --git a/gosl/examples/rand/rand.go b/gosl/examples/rand/rand.go index 198a9b4a..09d8f96c 100644 --- a/gosl/examples/rand/rand.go +++ b/gosl/examples/rand/rand.go @@ -67,6 +67,7 @@ func RndGen(counter uint64, idx uint32) { } func Compute(i uint32) { //gosl:kernel + // note: this should have a bounds check here on i -- can be larger than Floats RndGen(Seed[0].Seed, i) } diff --git a/gosl/examples/rand/rand.goal b/gosl/examples/rand/rand.goal index 308620e6..ae25ab8b 100644 --- a/gosl/examples/rand/rand.goal +++ b/gosl/examples/rand/rand.goal @@ -62,6 +62,7 @@ func RndGen(counter uint64, idx uint32) { } func Compute(i uint32) { //gosl:kernel + // note: this should have a bounds check here on i -- can be larger than Floats RndGen(Seed[0].Seed, i) } diff --git a/gosl/gotosl/extract.go b/gosl/gotosl/extract.go index 4c6d3fe9..ed6cad08 100644 --- a/gosl/gotosl/extract.go +++ b/gosl/gotosl/extract.go @@ -153,6 +153,7 @@ func (st *State) AppendGoHeader(lines [][]byte) [][]byte { "cogentcore.org/lab/gosl/slbool" "cogentcore.org/lab/gosl/slrand" "cogentcore.org/lab/gosl/sltype" + "cogentcore.org/lab/gosl/slvec" "cogentcore.org/lab/tensor" `)) for impath := range st.GoImports { diff --git a/gosl/gotosl/gengpu.go b/gosl/gotosl/gengpu.go index bc8e1d94..4e6afa55 100644 --- a/gosl/gotosl/gengpu.go +++ b/gosl/gotosl/gengpu.go @@ -49,9 +49,19 @@ import ( var shaders embed.FS var ( - // ComputeGPU is the compute gpu device + // GPUInitialized is true once the GPU system has been initialized. + // Prevents multiple initializations. + GPUInitialized bool + + // ComputeGPU is the compute gpu device. + // Set this prior to calling GPUInit() to use an existing device. ComputeGPU *gpu.GPU + // BorrowedGPU is true if our ComputeGPU is set externally, + // versus created specifically for this system. If external, + // we don't release it. + BorrowedGPU bool + // UseGPU indicates whether to use GPU vs. CPU. UseGPU bool ) @@ -111,11 +121,17 @@ const ( // configuring system(s), variables and kernels. // It is safe to call multiple times: detects if already run. func GPUInit() { - if ComputeGPU != nil { + if GPUInitialized { return } - gp := gpu.NewComputeGPU() - ComputeGPU = gp + GPUInitialized = true + if ComputeGPU == nil { // set prior to this call to use an external + ComputeGPU = gpu.NewComputeGPU() + } else { + BorrowedGPU = true + } + gp := ComputeGPU + _ = fmt.Sprintf("%g",math.NaN()) // keep imports happy ` @@ -146,10 +162,11 @@ func GPURelease() { } gpuRelease := ` - if ComputeGPU != nil { + if !BorrowedGPU && ComputeGPU != nil { ComputeGPU.Release() - ComputeGPU = nil + } + ComputeGPU = nil } ` diff --git a/gosl/gotosl/nodes.go b/gosl/gotosl/nodes.go index 99b1525e..0b3afc73 100644 --- a/gosl/gotosl/nodes.go +++ b/gosl/gotosl/nodes.go @@ -2766,6 +2766,13 @@ func (p *printer) systemVars(d *ast.GenDecl, sysname string) { p.userError(err) continue } + // by the time this happens, all types have been moved to imports and show up there + // so we've lost the original origin. And we'd have to make up an incompatible type name + // anyway, so bottom line is: all var types need to be defined locally. + // tt := p.getIdType(id) + // if tt != nil { + // fmt.Println("idtyp:", tt.String()) + // } typ = "[]" + id.Name } else { sel, ok := vs.Type.(*ast.SelectorExpr) diff --git a/gosl/gotosl/sledits.go b/gosl/gotosl/sledits.go index 41718c49..c783c3a2 100644 --- a/gosl/gotosl/sledits.go +++ b/gosl/gotosl/sledits.go @@ -39,6 +39,12 @@ type Replace struct { var Replaces = []Replace{ {[]byte("sltype.Uint32Vec2"), []byte("vec2")}, {[]byte("sltype.Float32Vec2"), []byte("vec2")}, + {[]byte("slvec.Vector2i"), []byte("vec4")}, + {[]byte("slvec.Vector2"), []byte("vec4")}, + {[]byte("math32.Vector2i"), []byte("vec2")}, + {[]byte("math32.Vector2"), []byte("vec2")}, + {[]byte("math32.Vector3"), []byte("vec3")}, + {[]byte("math32.Vector4"), []byte("vec4")}, {[]byte("float32"), []byte("f32")}, {[]byte("float64"), []byte("f64")}, // TODO: not yet supported {[]byte("uint32"), []byte("u32")}, diff --git a/gosl/gotosl/testdata/Compute.golden b/gosl/gotosl/testdata/Compute.golden index 75abc4e1..abc0f07d 100644 --- a/gosl/gotosl/testdata/Compute.golden +++ b/gosl/gotosl/testdata/Compute.golden @@ -154,6 +154,8 @@ struct ParamStruct { Dt: f32, Option: i32, // note: standard bool doesn't work pad: f32, // comment this out to trigger alignment warning + VXYf: vec4, // translates to vec4 + VXYi: vec4, // translates to vec4 Subs: SubParamStruct, } fn ParamStruct_IntegFromRaw(ps: ParamStruct, idx: i32) -> f32 { @@ -165,7 +167,7 @@ fn ParamStruct_IntegFromRaw(ps: ParamStruct, idx: i32) -> f32 { integ += newVal; Data[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(Integ))] = integ; Data[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(Exp))] = exp(-integ); - var a: f32; + var a = ps.VXYf.x; let ctx = Ctx[0]; ParamStruct_AnotherMeth(ps, ctx, idx, &a); var bv = BigGet(Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(Integ))); diff --git a/gosl/gotosl/testdata/CycleUpdt.golden b/gosl/gotosl/testdata/CycleUpdt.golden index 2f79bc09..de6861f7 100644 --- a/gosl/gotosl/testdata/CycleUpdt.golden +++ b/gosl/gotosl/testdata/CycleUpdt.golden @@ -44,6 +44,8 @@ struct ParamStruct { Dt: f32, Option: i32, // note: standard bool doesn't work pad: f32, // comment this out to trigger alignment warning + VXYf: vec4, // translates to vec4 + VXYi: vec4, // translates to vec4 Subs: SubParamStruct, } struct Context { diff --git a/gosl/gotosl/testdata/basic.go b/gosl/gotosl/testdata/basic.go index 9d65206d..6ff432f4 100644 --- a/gosl/gotosl/testdata/basic.go +++ b/gosl/gotosl/testdata/basic.go @@ -8,6 +8,7 @@ import ( "cogentcore.org/core/math32" "cogentcore.org/lab/gosl/slbool" + "cogentcore.org/lab/gosl/slvec" "cogentcore.org/lab/tensor" ) @@ -126,6 +127,9 @@ type ParamStruct struct { pad float32 // comment this out to trigger alignment warning + VXYf slvec.Vector2 // translates to vec4 + VXYi slvec.Vector2i // translates to vec4 + // extra parameters Subs SubParamStruct } @@ -140,7 +144,9 @@ func (ps *ParamStruct) IntegFromRaw(idx int) float32 { integ += newVal Data.Set(integ, int(idx), int(Integ)) Data.Set(math32.Exp(-integ), int(idx), int(Exp)) - var a float32 + + a := ps.VXYf.X + ctx := GetCtx(0) ps.AnotherMeth(ctx, idx, &a) bv := Big.Value(int(idx), int(Integ)) diff --git a/gosl/gotosl/testdata/basic.goal b/gosl/gotosl/testdata/basic.goal index 54ea3d69..b9890bed 100644 --- a/gosl/gotosl/testdata/basic.goal +++ b/gosl/gotosl/testdata/basic.goal @@ -5,6 +5,7 @@ import ( "cogentcore.org/core/math32" "cogentcore.org/lab/gosl/slbool" + "cogentcore.org/lab/gosl/slvec" "cogentcore.org/lab/tensor" ) @@ -119,6 +120,9 @@ type ParamStruct struct { pad float32 // comment this out to trigger alignment warning + VXYf slvec.Vector2 // translates to vec4 + VXYi slvec.Vector2i // translates to vec4 + // extra parameters Subs SubParamStruct } @@ -133,7 +137,9 @@ func (ps *ParamStruct) IntegFromRaw(idx int) float32 { integ += newVal Data[idx, Integ] = integ Data[idx, Exp] = math32.Exp(-integ) - var a float32 + + a := ps.VXYf.X + ctx := GetCtx(0) ps.AnotherMeth(ctx, idx, &a) bv := Big[idx, Integ] diff --git a/gosl/gotosl/testdata/gosl.golden b/gosl/gotosl/testdata/gosl.golden index 7d49af58..10f1b1b9 100644 --- a/gosl/gotosl/testdata/gosl.golden +++ b/gosl/gotosl/testdata/gosl.golden @@ -15,9 +15,19 @@ import ( var shaders embed.FS var ( - // ComputeGPU is the compute gpu device + // GPUInitialized is true once the GPU system has been initialized. + // Prevents multiple initializations. + GPUInitialized bool + + // ComputeGPU is the compute gpu device. + // Set this prior to calling GPUInit() to use an existing device. ComputeGPU *gpu.GPU + // BorrowedGPU is true if our ComputeGPU is set externally, + // versus created specifically for this system. If external, + // we don't release it. + BorrowedGPU bool + // UseGPU indicates whether to use GPU vs. CPU. UseGPU bool ) @@ -42,11 +52,17 @@ var TensorStrides tensor.Uint32 // configuring system(s), variables and kernels. // It is safe to call multiple times: detects if already run. func GPUInit() { - if ComputeGPU != nil { + if GPUInitialized { return } - gp := gpu.NewComputeGPU() - ComputeGPU = gp + GPUInitialized = true + if ComputeGPU == nil { // set prior to this call to use an external + ComputeGPU = gpu.NewComputeGPU() + } else { + BorrowedGPU = true + } + gp := ComputeGPU + _ = fmt.Sprintf("%g",math.NaN()) // keep imports happy { sy := gpu.NewComputeSystem(gp, "Default") @@ -97,10 +113,11 @@ func GPURelease() { GPUSystem = nil } - if ComputeGPU != nil { + if !BorrowedGPU && ComputeGPU != nil { ComputeGPU.Release() - ComputeGPU = nil + } + ComputeGPU = nil } // RunCompute runs the Compute kernel with given number of elements, diff --git a/gosl/sltype/int.go b/gosl/sltype/int.go index 70468d4c..9caaa314 100644 --- a/gosl/sltype/int.go +++ b/gosl/sltype/int.go @@ -20,6 +20,26 @@ type Int32Vec4 struct { W int32 } +// Add returns the vector p+q. +func (p Int32Vec4) Add(q Int32Vec4) Int32Vec4 { + return Int32Vec4{p.X + q.X, p.Y + q.Y, p.Z + q.Z, p.W + q.W} +} + +// Sub returns the vector p-q. +func (p Int32Vec4) Sub(q Int32Vec4) Int32Vec4 { + return Int32Vec4{p.X - q.X, p.Y - q.Y, p.Z - q.Z, p.W - q.W} +} + +// MulScalar returns the vector p*k. +func (p Int32Vec4) MulScalar(k int32) Int32Vec4 { + return Int32Vec4{p.X * k, p.Y * k, p.Z * k, p.W * k} +} + +// DivScalar returns the vector p/k. +func (p Int32Vec4) DivScalar(k int32) Int32Vec4 { + return Int32Vec4{p.X / k, p.Y / k, p.Z / k, p.W / k} +} + //////// Unsigned // Uint32Vec2 is a length 2 vector of uint32 diff --git a/gosl/slvec/slvec.go b/gosl/slvec/slvec.go new file mode 100644 index 00000000..1171d107 --- /dev/null +++ b/gosl/slvec/slvec.go @@ -0,0 +1,63 @@ +// Copyright 2025 Cogent Core. All rights reserved. +// Use of this source code is governed by a BSD-style +// license that can be found in the LICENSE file. + +package slvec + +import "cogentcore.org/core/math32" + +//gosl:start + +// Vector2 is a 2D vector/point with X and Y components. +// with padding values so it works in a GPU struct. Use the +// V() method to get a math32.Vector2 that supports standard +// math operations. Cannot use those math ops in gosl GPU +// code at this point, unfortunately. +type Vector2 struct { + X float32 + Y float32 + + pad, pad1 float32 +} + +func (v *Vector2) V() math32.Vector2 { + return math32.Vec2(v.X, v.Y) +} + +func (v *Vector2) Set(x, y float32) { + v.X = x + v.Y = y +} + +func (v *Vector2) SetV(mv math32.Vector2) { + v.X = mv.X + v.Y = mv.Y +} + +// Vector2i is a 2D vector/point with X and Y integer components. +// with padding values so it works in a GPU struct. Use the +// V() method to get a math32.Vector2i that supports standard +// math operations. Cannot use those math ops in gosl GPU +// code at this point, unfortunately. +type Vector2i struct { + X int32 + Y int32 + + pad, pad1 int32 +} + +func (v *Vector2i) V() math32.Vector2i { + return math32.Vec2i(int(v.X), int(v.Y)) +} + +func (v *Vector2i) Set(x, y int) { + v.X = int32(x) + v.Y = int32(y) +} + +func (v *Vector2i) SetV(mv math32.Vector2i) { + v.X = mv.X + v.Y = mv.Y +} + +//gosl:end diff --git a/tensor/convert.go b/tensor/convert.go index 1db709ee..72b5801b 100644 --- a/tensor/convert.go +++ b/tensor/convert.go @@ -234,7 +234,7 @@ func Range(vals Values) (min, max float64, minIndex, maxIndex int) { maxIndex = -1 n := vals.Len() for j := range n { - fv := vals.Float1D(n) + fv := vals.Float1D(j) if math.IsNaN(fv) { continue } @@ -307,3 +307,24 @@ func ContainsString(tsr, vals Tensor, opts StringMatch) bool { } return false } + +// CopyFromLargerShape copies values from another tensor of a larger +// shape, using indexes in this shape. The other tensor must have at +// least the same or greater shape values on each dimension as the target. +// Uses float numbers to copy if not a string. +func CopyFromLargerShape(tsr, from Tensor) { + n := tsr.Len() + if tsr.IsString() { + for i := range n { + idx := tsr.Shape().IndexFrom1D(i) + v := from.StringValue(idx...) + tsr.SetString(v, idx...) + } + } else { + for i := range n { + idx := tsr.Shape().IndexFrom1D(i) + v := from.Float(idx...) + tsr.SetFloat(v, idx...) + } + } +} From b20007778c228469dd6b83edc74a916a682ff3a3 Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Thu, 11 Dec 2025 12:03:18 +0100 Subject: [PATCH 09/97] move physics from core/xyz to lab -- this is more specialized and requires significant additional development to get the actual physics part working. --- physics/README.md | 50 +++ physics/bbox.go | 63 ++++ physics/body.go | 38 +++ physics/box.go | 40 +++ physics/capsule.go | 49 +++ physics/collide.go | 83 +++++ physics/cylinder.go | 48 +++ physics/examples/virtroom/README.md | 8 + physics/examples/virtroom/typegen.go | 9 + physics/examples/virtroom/virtroom.go | 474 ++++++++++++++++++++++++++ physics/group.go | 251 ++++++++++++++ physics/node.go | 186 ++++++++++ physics/rigid.go | 33 ++ physics/sphere.go | 40 +++ physics/state.go | 155 +++++++++ physics/typegen.go | 140 ++++++++ physics/world/camera.go | 53 +++ physics/world/depth.go | 96 ++++++ physics/world/nogui.go | 24 ++ physics/world/typegen.go | 11 + physics/world/update.go | 151 ++++++++ physics/world/world.go | 117 +++++++ 22 files changed, 2119 insertions(+) create mode 100644 physics/README.md create mode 100644 physics/bbox.go create mode 100644 physics/body.go create mode 100644 physics/box.go create mode 100644 physics/capsule.go create mode 100644 physics/collide.go create mode 100644 physics/cylinder.go create mode 100644 physics/examples/virtroom/README.md create mode 100644 physics/examples/virtroom/typegen.go create mode 100644 physics/examples/virtroom/virtroom.go create mode 100644 physics/group.go create mode 100644 physics/node.go create mode 100644 physics/rigid.go create mode 100644 physics/sphere.go create mode 100644 physics/state.go create mode 100644 physics/typegen.go create mode 100644 physics/world/camera.go create mode 100644 physics/world/depth.go create mode 100644 physics/world/nogui.go create mode 100644 physics/world/typegen.go create mode 100644 physics/world/update.go create mode 100644 physics/world/world.go diff --git a/physics/README.md b/physics/README.md new file mode 100644 index 00000000..c94a402b --- /dev/null +++ b/physics/README.md @@ -0,0 +1,50 @@ +# Physics engine for virtual reality + +This `physics` engine is a scenegraph-based 3D physics simulator for creating virtual environments. It provides a `Body` node for rigid body physics, along with some basic geometrical shapes thereof. The `physics` scene contains just the bare physics bodies and other elements, which can be updated independent of any visualization. + +Currently, it provides collision detection and basic forward Euler physics updating, but it does not yet compute any forces for the interactions among the bodies. Ultimately we hope to figure out how the [Bullet](https://github.com/bulletphysics/bullet3) system works and get that running here, in a clean and simple implementation. + +Incrementally, we will start with a basic explicitly driven form of physics that is sufficient to get started, and build from there. + +The [world](world) visualization sub-package generates an [xyz](https://cogentcore.org/xyz) 3D scenegraph based on the physics bodies, and updates this visualization efficiently as the physics is updated. + +See [virtroom example](examples/virtroom) for an implemented example that shows how to do everything. + +# Organizing the World + +It is most efficient to create a relatively deep tree with `Group` nodes that collect nearby `Body` objects at multiple levels of spatial scale. Bounding Boxes are computed at every level of Group, and pruning is done at every level, so large chunks of the tree can be eliminated easily with this strategy. + +Also, Nodes must be specifically flagged as being `Dynamic` -- otherwise they are assumed to be static -- and each type should be organized into separate top-level Groups (there can be multiple of each, but don't mix Dynamic and Static). Static nodes are never collided against each-other. Ideally, all the Dynamic nodes are in separate top-level, or at least second-to-top level groups -- this eliminates redundant A vs. B and B vs. A collisions and focuses each collision on the most relevant information. + +# Updating Modes + +There are two major modes of updating: Scripted or Physics -- scripted requires a program to control what happens on every time step, while physics uses computed forces from contacts, plus joint constraints, to update velocities (not yet supported). The update modes are just about which methods you call. + +The `Group` has a set of `World*` methods that should be used on the top-level world Group node node to do all the init and update steps. The update loops automatically exclude non Dynamic nodes. + +* `WorldInit` -- everyone calls this at the start to set the initial config + +* `WorldRelToAbs` -- for scripted mode when updating relative positions, rotations. + +* `WorldStep` -- for either scripted or physics modes, to update state from current velocities. + +* `WorldCollide` -- returns list of potential collision contacts based on projected motion, focusing on dynamic vs. static and dynamic vs. dynamic bodies, with optimized tree filtering. This is the first pass for collision detection. + +## Scripted Mode + +For Scripted mode, each update step typically involves manually updating the `Rel.Pos` and `.Quat` fields on `Body` objects to update their relative positions. This field is a `State` type and has `MoveOnAxis` and `RotateOnAxis` (and a number of other rotation methods). The Move methods update the `LinVel` field to reflect any delta in movement. + +It is also possible to manually set the `Abs.LinVel` and `Abs.AngVel` fields and call `Step` to update. + +For collision detection, it is essential to have the `Abs.LinVel` field set to anticipate the effects of motion and determine likely future impacts. The RelToAbs update call does this automatically, and if you're instead using `Step` the `LinVel` is already set. Both calls will automatically compute an updated BBox and VelBBox. + +It is up to the user to manage the list of potential collisions, e.g., by setting velocity to 0 or bouncing back etc. + +## Physics Mode + +The good news so far is that the full physics version as in Bullet is actually not too bad. The core update step is a super simple forward Euler, intuitive update (just add velocity to position, with a step size factor). The remaining work is just in computing the forces to update those velocities. Bullet uses a hybrid approach that is clearly described in the [Mirtich thesis](https://people.eecs.berkeley.edu/~jfc/mirtich/thesis/mirtichThesis.pdf), which combines *impulses* with a particular way of handling joints, due originally to Featherstone. Impulses are really simple conceptually: when two objects collide, they bounce back off of each other in proportion to their `Bounce` (coefficient of restitution) factor -- these collision impact forces dominate everything else, and aren't that hard to compute (similar conceptually to the `marbles` example in GoGi). The joint constraint stuff is a bit more complicated but not the worst. Everything can be done incrementally. And the resulting system will avoid the brittle nature of the full constraint-based approach taken in ODE, which caused a lot of crashes and instability in `cemer`. + +One of the major problems with the impulse-based approach: it causes otherwise "still" objects to jiggle around and slip down planes, seems eminently tractable with special-case code that doesn't seem too hard. + +more info: https://caseymuratori.com/blog_0003 + diff --git a/physics/bbox.go b/physics/bbox.go new file mode 100644 index 00000000..9addc98e --- /dev/null +++ b/physics/bbox.go @@ -0,0 +1,63 @@ +// Copyright (c) 2019, Cogent Core. All rights reserved. +// Use of this source code is governed by a BSD-style +// license that can be found in the LICENSE file. + +package physics + +import "cogentcore.org/core/math32" + +// BBox contains bounding box and other gross object properties +type BBox struct { + + // bounding box in world coords (Axis-Aligned Bounding Box = AABB) + BBox math32.Box3 + + // velocity-projected bounding box in world coords: extend BBox to include future position of moving bodies -- collision must be made on this basis + VelBBox math32.Box3 + + // bounding sphere in local coords + BSphere math32.Sphere + + // area + Area float32 + + // volume + Volume float32 +} + +// SetBounds sets BBox from min, max and updates other factors based on that +func (bb *BBox) SetBounds(min, max math32.Vector3) { + bb.BBox.Set(&min, &max) + bb.UpdateFromBBox() +} + +// UpdateFromBBox updates other values from BBox +func (bb *BBox) UpdateFromBBox() { + bb.BSphere.SetFromBox(bb.BBox) + sz := bb.BBox.Size() + bb.Area = 2*sz.X + 2*sz.Y + 2*sz.Z + bb.Volume = sz.X * sz.Y * sz.Z +} + +// XForm transforms bounds with given quat and position offset to convert to world coords +func (bb *BBox) XForm(q math32.Quat, pos math32.Vector3) { + bb.BBox = bb.BBox.MulQuat(q).Translate(pos) + bb.BSphere.Translate(pos) +} + +// VelProject computes the velocity-projected bounding box for given velocity and step size +func (bb *BBox) VelProject(vel math32.Vector3, step float32) { + eb := bb.BBox.Translate(vel.MulScalar(step)) + bb.VelBBox = bb.BBox + bb.VelBBox.ExpandByBox(eb) +} + +// VelNilProject is for static items -- just copy the BBox +func (bb *BBox) VelNilProject() { + bb.VelBBox = bb.BBox +} + +// IntersectsVelBox returns true if two velocity-projected bounding boxes intersect +func (bb *BBox) IntersectsVelBox(oth *BBox) bool { + return bb.VelBBox.IntersectsBox(oth.VelBBox) +} diff --git a/physics/body.go b/physics/body.go new file mode 100644 index 00000000..d2334121 --- /dev/null +++ b/physics/body.go @@ -0,0 +1,38 @@ +// Copyright (c) 2019, Cogent Core. All rights reserved. +// Use of this source code is governed by a BSD-style +// license that can be found in the LICENSE file. + +package physics + +// Body is the common interface for all body types +type Body interface { + Node + + // AsBodyBase returns the body as a BodyBase + AsBodyBase() *BodyBase +} + +// BodyBase is the base type for all specific Body types +type BodyBase struct { + NodeBase + + // rigid body properties, including mass, bounce, friction etc. + Rigid Rigid + + // default color of body. + Color string +} + +func (bb *BodyBase) AsBody() Body { + return bb.This.(Body) +} + +func (bb *BodyBase) AsBodyBase() *BodyBase { + return bb +} + +func (bb *BodyBase) GroupBBox() {} + +func (bb *BodyBase) Init() { + // not calling Updater(UpdateFromMake) here -- no children +} diff --git a/physics/box.go b/physics/box.go new file mode 100644 index 00000000..3832f518 --- /dev/null +++ b/physics/box.go @@ -0,0 +1,40 @@ +// Copyright (c) 2019, Cogent Core. All rights reserved. +// Use of this source code is governed by a BSD-style +// license that can be found in the LICENSE file. + +package physics + +import ( + "cogentcore.org/core/math32" +) + +// Box is a box body shape +type Box struct { + BodyBase + + // size of box in each dimension (units arbitrary, as long as they are all consistent -- meters is typical) + Size math32.Vector3 +} + +func (bx *Box) SetBBox() { + bx.BBox.SetBounds(bx.Size.MulScalar(-.5), bx.Size.MulScalar(.5)) + bx.BBox.XForm(bx.Abs.Quat, bx.Abs.Pos) +} + +func (bx *Box) InitAbs(par *NodeBase) { + bx.InitAbsBase(par) + bx.SetBBox() + bx.BBox.VelNilProject() +} + +func (bx *Box) RelToAbs(par *NodeBase) { + bx.RelToAbsBase(par) + bx.SetBBox() + bx.BBox.VelProject(bx.Abs.LinVel, 1) +} + +func (bx *Box) Step(step float32) { + bx.StepBase(step) + bx.SetBBox() + bx.BBox.VelProject(bx.Abs.LinVel, step) +} diff --git a/physics/capsule.go b/physics/capsule.go new file mode 100644 index 00000000..cbd4b372 --- /dev/null +++ b/physics/capsule.go @@ -0,0 +1,49 @@ +// Copyright (c) 2019, Cogent Core. All rights reserved. +// Use of this source code is governed by a BSD-style +// license that can be found in the LICENSE file. + +package physics + +import ( + "cogentcore.org/core/math32" +) + +// Capsule is a generalized cylinder body shape, with hemispheres at each end, +// with separate radii for top and bottom. +type Capsule struct { + BodyBase + + // height of the cylinder portion of the capsule + Height float32 + + // radius of the top hemisphere + TopRad float32 + + // radius of the bottom hemisphere + BotRad float32 +} + +func (cp *Capsule) SetBBox() { + th := cp.Height + cp.TopRad + cp.BotRad + h2 := th / 2 + cp.BBox.SetBounds(math32.Vec3(-cp.BotRad, -h2, -cp.BotRad), math32.Vec3(cp.TopRad, h2, cp.TopRad)) + cp.BBox.XForm(cp.Abs.Quat, cp.Abs.Pos) +} + +func (cp *Capsule) InitAbs(par *NodeBase) { + cp.InitAbsBase(par) + cp.SetBBox() + cp.BBox.VelNilProject() +} + +func (cp *Capsule) RelToAbs(par *NodeBase) { + cp.RelToAbsBase(par) + cp.SetBBox() + cp.BBox.VelProject(cp.Abs.LinVel, 1) +} + +func (cp *Capsule) Step(step float32) { + cp.StepBase(step) + cp.SetBBox() + cp.BBox.VelProject(cp.Abs.LinVel, step) +} diff --git a/physics/collide.go b/physics/collide.go new file mode 100644 index 00000000..ecf271ad --- /dev/null +++ b/physics/collide.go @@ -0,0 +1,83 @@ +// Copyright (c) 2019, Cogent Core. All rights reserved. +// Use of this source code is governed by a BSD-style +// license that can be found in the LICENSE file. + +package physics + +import ( + "cogentcore.org/core/math32" + "cogentcore.org/core/tree" +) + +// Contact is one pairwise point of contact between two bodies. +// Contacts are represented in spherical terms relative to the +// spherical BBox of A and B. +type Contact struct { + + // one body + A Body + + // the other body + B Body + + // normal pointing from center of B to center of A + NormB math32.Vector3 + + // point on spherical shell of B where A is contacting + PtB math32.Vector3 + + // distance from PtB along NormB to contact point on spherical shell of A + Dist float32 +} + +// UpdateDist updates the distance information for the contact +func (c *Contact) UpdateDist() { + +} + +// Contacts is a slice list of contacts +type Contacts []*Contact + +// New adds a new contact to the list +func (cs *Contacts) New(a, b Body) *Contact { + c := &Contact{A: a, B: b} + *cs = append(*cs, c) + return c +} + +// BodyVelBBoxIntersects returns the list of potential contact nodes between a and b +// (could be the same or different groups) that have intersecting velocity-projected +// bounding boxes. In general a should be dynamic bodies and b either dynamic or static. +// This is the broad first-pass filtering. +func BodyVelBBoxIntersects(a, b Node) Contacts { + var cts Contacts + a.AsTree().WalkDown(func(k tree.Node) bool { + aii, ai := AsNode(k) + if aii == nil { + return false // going into a different type of thing, bail + } + abod := aii.AsBody() // only consider bodies for collision + if abod == nil { + return true + } + + b.AsTree().WalkDown(func(k tree.Node) bool { + bii, bi := AsNode(k) + if bii == nil { + return false // going into a different type of thing, bail + } + if !ai.BBox.IntersectsVelBox(&bi.BBox) { + return false // done + } + bbod := bii.AsBody() // only consider bodies for collision + if bbod == nil { + return true + } + cts.New(abod, bbod) + return false // done + }) + + return false + }) + return cts +} diff --git a/physics/cylinder.go b/physics/cylinder.go new file mode 100644 index 00000000..8a76182c --- /dev/null +++ b/physics/cylinder.go @@ -0,0 +1,48 @@ +// Copyright (c) 2019, Cogent Core. All rights reserved. +// Use of this source code is governed by a BSD-style +// license that can be found in the LICENSE file. + +package physics + +import ( + "cogentcore.org/core/math32" +) + +// Cylinder is a generalized cylinder body shape, with separate radii for top and bottom. +// A cone has a zero radius at one end. +type Cylinder struct { + BodyBase + + // height of the cylinder + Height float32 + + // radius of the top -- set to 0 for a cone + TopRad float32 + + // radius of the bottom + BotRad float32 +} + +func (cy *Cylinder) SetBBox() { + h2 := cy.Height / 2 + cy.BBox.SetBounds(math32.Vec3(-cy.BotRad, -h2, -cy.BotRad), math32.Vec3(cy.TopRad, h2, cy.TopRad)) + cy.BBox.XForm(cy.Abs.Quat, cy.Abs.Pos) +} + +func (cy *Cylinder) InitAbs(par *NodeBase) { + cy.InitAbsBase(par) + cy.SetBBox() + cy.BBox.VelNilProject() +} + +func (cy *Cylinder) RelToAbs(par *NodeBase) { + cy.RelToAbsBase(par) + cy.SetBBox() + cy.BBox.VelProject(cy.Abs.LinVel, 1) +} + +func (cy *Cylinder) Step(step float32) { + cy.StepBase(step) + cy.SetBBox() + cy.BBox.VelProject(cy.Abs.LinVel, step) +} diff --git a/physics/examples/virtroom/README.md b/physics/examples/virtroom/README.md new file mode 100644 index 00000000..de4ff342 --- /dev/null +++ b/physics/examples/virtroom/README.md @@ -0,0 +1,8 @@ +# xyz/physics example + +This demo shows how to construct and visualize a [physics](../../physics) model of a simple virtual room environment, with a virtual robot ("emer") having a first-person view of the world. + +You can hold down the motion buttons in the toolbar to move the robot around more easily. + +Use `-nogui` arg to run in no GUI mode (purely offscreen mode, e.g., for batch-mode running), which saves a .png image of the starting view, in `eyer_0.png`. + diff --git a/physics/examples/virtroom/typegen.go b/physics/examples/virtroom/typegen.go new file mode 100644 index 00000000..20f9a010 --- /dev/null +++ b/physics/examples/virtroom/typegen.go @@ -0,0 +1,9 @@ +// Code generated by "core generate"; DO NOT EDIT. + +package main + +import ( + "cogentcore.org/core/types" +) + +var _ = types.AddType(&types.Type{Name: "main.Env", IDName: "env", Doc: "Env encapsulates the virtual environment", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Methods: []types.Method{{Name: "WorldInit", Doc: "InitWorld does init on world.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "GrabEyeImg", Doc: "GrabEyeImg takes a snapshot from the perspective of Emer's right eye", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "StepForward", Doc: "StepForward moves Emer forward in current facing direction one step, and takes GrabEyeImg", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "StepBackward", Doc: "StepBackward moves Emer backward in current facing direction one step, and takes GrabEyeImg", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "RotBodyLeft", Doc: "RotBodyLeft rotates emer left and takes GrabEyeImg", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "RotBodyRight", Doc: "RotBodyRight rotates emer right and takes GrabEyeImg", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "RotHeadLeft", Doc: "RotHeadLeft rotates emer left and takes GrabEyeImg", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "RotHeadRight", Doc: "RotHeadRight rotates emer right and takes GrabEyeImg", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}}, Fields: []types.Field{{Name: "EmerAngry", Doc: "if true, emer is angry: changes face color"}, {Name: "EmerHt", Doc: "height of emer"}, {Name: "MoveStep", Doc: "how far to move every step"}, {Name: "RotStep", Doc: "how far to rotate every step"}, {Name: "Width", Doc: "width of room"}, {Name: "Depth", Doc: "depth of room"}, {Name: "Height", Doc: "height of room"}, {Name: "Thick", Doc: "thickness of walls of room"}, {Name: "DepthVals", Doc: "current depth map"}, {Name: "Camera", Doc: "offscreen render camera settings"}, {Name: "DepthMap", Doc: "color map to use for rendering depth map"}, {Name: "World", Doc: "The whole physics World, including visualization."}, {Name: "SceneEditor", Doc: "3D visualization of the Scene"}, {Name: "Emer", Doc: "emer object"}, {Name: "EyeR", Doc: "Right eye of emer"}, {Name: "Contacts", Doc: "contacts from last step, for body"}, {Name: "EyeRImg", Doc: "snapshot image"}, {Name: "DepthImage", Doc: "depth map image"}}}) diff --git a/physics/examples/virtroom/virtroom.go b/physics/examples/virtroom/virtroom.go new file mode 100644 index 00000000..03a748b4 --- /dev/null +++ b/physics/examples/virtroom/virtroom.go @@ -0,0 +1,474 @@ +// Copyright (c) 2019, Cogent Core. All rights reserved. +// Use of this source code is governed by a BSD-style +// license that can be found in the LICENSE file. + +package main + +//go:generate core generate + +import ( + "fmt" + "image" + "math/rand" + "os" + + "cogentcore.org/core/base/iox/imagex" + "cogentcore.org/core/colors" + "cogentcore.org/core/colors/colormap" + "cogentcore.org/core/core" + "cogentcore.org/core/events" + "cogentcore.org/core/gpu" + "cogentcore.org/core/icons" + "cogentcore.org/core/math32" + "cogentcore.org/core/styles" + "cogentcore.org/core/styles/abilities" + "cogentcore.org/core/tree" + "cogentcore.org/core/xyz" + "cogentcore.org/core/xyz/xyzcore" + "cogentcore.org/lab/physics" + "cogentcore.org/lab/physics/world" +) + +var NoGUI bool + +func main() { + if len(os.Args) > 1 && os.Args[1] == "-nogui" { + NoGUI = true + } + ev := &Env{} + ev.Defaults() + if NoGUI { + ev.NoGUIRun() + return + } + // core.RenderTrace = true + b := ev.ConfigGUI() + b.RunMainWindow() +} + +// Env encapsulates the virtual environment +type Env struct { //types:add + + // if true, emer is angry: changes face color + EmerAngry bool + + // height of emer + EmerHt float32 + + // how far to move every step + MoveStep float32 + + // how far to rotate every step + RotStep float32 + + // width of room + Width float32 + + // depth of room + Depth float32 + + // height of room + Height float32 + + // thickness of walls of room + Thick float32 + + // current depth map + DepthVals []float32 + + // offscreen render camera settings + Camera world.Camera + + // color map to use for rendering depth map + DepthMap core.ColorMapName + + // The whole physics World, including visualization. + World *world.World + + // 3D visualization of the Scene + SceneEditor *xyzcore.SceneEditor + + // emer object + Emer *physics.Group `display:"-"` + + // Right eye of emer + EyeR physics.Body `display:"-"` + + // contacts from last step, for body + Contacts physics.Contacts `display:"-"` + + // snapshot image + EyeRImg *core.Image `display:"-"` + + // depth map image + DepthImage *core.Image `display:"-"` +} + +func (ev *Env) Defaults() { + ev.Width = 10 + ev.Depth = 15 + ev.Height = 2 + ev.Thick = 0.2 + ev.EmerHt = 1 + ev.MoveStep = ev.EmerHt * .2 + ev.RotStep = 15 + ev.DepthMap = core.ColorMapName("ColdHot") + ev.Camera.Defaults() + ev.Camera.FOV = 90 +} + +// MakePhysicsWorld constructs a new virtual physics world. +func (ev *Env) MakePhysicsWorld() *physics.Group { + pw := physics.NewGroup() + pw.SetName("RoomWorld") + + ev.MakeRoom(pw, "room1", ev.Width, ev.Depth, ev.Height, ev.Thick) + ev.MakeEmer(pw, "emer", ev.EmerHt) + pw.WorldInit() + return pw +} + +func (ev *Env) MakeWorld(sc *xyz.Scene) { + pw := ev.MakePhysicsWorld() + sc.Background = colors.Scheme.Select.Container + xyz.NewAmbient(sc, "ambient", 0.3, xyz.DirectSun) + + dir := xyz.NewDirectional(sc, "dir", 1, xyz.DirectSun) + dir.Pos.Set(0, 2, 1) // default: 0,1,1 = above and behind us (we are at 0,0,X) + + ev.World = world.NewWorld(pw, sc) +} + +// InitWorld does init on world. +func (ev *Env) WorldInit() { //types:add + ev.World.Init() +} + +// ConfigView3D makes the 3D view +func (ev *Env) ConfigView3D(sc *xyz.Scene) { + // sc.MultiSample = 1 // we are using depth grab so we need this = 1 +} + +// RenderEyeImg returns a snapshot from the perspective of Emer's right eye +func (ev *Env) RenderEyeImg() image.Image { + return ev.World.RenderFromNode(ev.EyeR, &ev.Camera) +} + +// GrabEyeImg takes a snapshot from the perspective of Emer's right eye +func (ev *Env) GrabEyeImg() { //types:add + img := ev.RenderEyeImg() + if img != nil { + ev.EyeRImg.SetImage(img) + ev.EyeRImg.NeedsRender() + } + // depth, err := ev.View3D.DepthImage() + // if err == nil && depth != nil { + // ev.DepthVals = depth + // ev.ViewDepth(depth) + // } +} + +// ViewDepth updates depth bitmap with depth data +func (ev *Env) ViewDepth(depth []float32) { + cmap := colormap.AvailableMaps[string(ev.DepthMap)] + img := image.NewRGBA(image.Rectangle{Max: ev.Camera.Size}) + ev.DepthImage.SetImage(img) + world.DepthImage(img, depth, cmap, &ev.Camera) + ev.DepthImage.NeedsRender() +} + +// UpdateView tells 3D view it needs to update. +func (ev *Env) UpdateView() { + if ev.SceneEditor.IsVisible() { + ev.SceneEditor.NeedsRender() + } +} + +// WorldStep does one step of the world +func (ev *Env) WorldStep() { + pw := ev.World.World + pw.Update() // only need to call if there are updaters added to world + pw.WorldRelToAbs() + cts := pw.WorldCollide(physics.DynsTopGps) + ev.Contacts = nil + for _, cl := range cts { + if len(cl) > 1 { + for _, c := range cl { + if c.A.AsTree().Name == "body" { + ev.Contacts = cl + } + fmt.Printf("A: %v B: %v\n", c.A.AsTree().Name, c.B.AsTree().Name) + } + } + } + ev.EmerAngry = false + if len(ev.Contacts) > 1 { // turn around + ev.EmerAngry = true + fmt.Printf("hit wall: turn around!\n") + rot := 100.0 + 90.0*rand.Float32() + ev.Emer.Rel.RotateOnAxis(0, 1, 0, rot) + } + ev.World.Update() + ev.GrabEyeImg() + ev.UpdateView() +} + +// StepForward moves Emer forward in current facing direction one step, and takes GrabEyeImg +func (ev *Env) StepForward() { //types:add + ev.Emer.Rel.MoveOnAxis(0, 0, 1, -ev.MoveStep) + ev.WorldStep() +} + +// StepBackward moves Emer backward in current facing direction one step, and takes GrabEyeImg +func (ev *Env) StepBackward() { //types:add + ev.Emer.Rel.MoveOnAxis(0, 0, 1, ev.MoveStep) + ev.WorldStep() +} + +// RotBodyLeft rotates emer left and takes GrabEyeImg +func (ev *Env) RotBodyLeft() { //types:add + ev.Emer.Rel.RotateOnAxis(0, 1, 0, ev.RotStep) + ev.WorldStep() +} + +// RotBodyRight rotates emer right and takes GrabEyeImg +func (ev *Env) RotBodyRight() { //types:add + ev.Emer.Rel.RotateOnAxis(0, 1, 0, -ev.RotStep) + ev.WorldStep() +} + +// RotHeadLeft rotates emer left and takes GrabEyeImg +func (ev *Env) RotHeadLeft() { //types:add + hd := ev.Emer.ChildByName("head", 1).(*physics.Group) + hd.Rel.RotateOnAxis(0, 1, 0, ev.RotStep) + ev.WorldStep() +} + +// RotHeadRight rotates emer right and takes GrabEyeImg +func (ev *Env) RotHeadRight() { //types:add + hd := ev.Emer.ChildByName("head", 1).(*physics.Group) + hd.Rel.RotateOnAxis(0, 1, 0, -ev.RotStep) + ev.WorldStep() +} + +// MakeRoom constructs a new room in given parent group with given params +func (ev *Env) MakeRoom(par *physics.Group, name string, width, depth, height, thick float32) { + tree.AddChildAt(par, name, func(rm *physics.Group) { + rm.Maker(func(p *tree.Plan) { + tree.AddAt(p, "floor", func(n *physics.Box) { + n.SetSize(math32.Vec3(width, thick, depth)). + SetColor("grey").SetInitPos(math32.Vec3(0, -thick/2, 0)) + }) + tree.AddAt(p, "back-wall", func(n *physics.Box) { + n.SetSize(math32.Vec3(width, height, thick)). + SetColor("blue").SetInitPos(math32.Vec3(0, height/2, -depth/2)) + }) + tree.AddAt(p, "left-wall", func(n *physics.Box) { + n.SetSize(math32.Vec3(thick, height, depth)). + SetColor("red").SetInitPos(math32.Vec3(-width/2, height/2, 0)) + }) + tree.AddAt(p, "right-wall", func(n *physics.Box) { + n.SetSize(math32.Vec3(thick, height, depth)). + SetColor("green").SetInitPos(math32.Vec3(width/2, height/2, 0)) + }) + tree.AddAt(p, "front-wall", func(n *physics.Box) { + n.SetSize(math32.Vec3(width, height, thick)). + SetColor("yellow").SetInitPos(math32.Vec3(0, height/2, depth/2)) + }) + }) + }) +} + +// MakeEmer constructs a new Emer virtual robot of given height (e.g., 1). +func (ev *Env) MakeEmer(par *physics.Group, name string, height float32) { + tree.AddChildAt(par, name, func(emr *physics.Group) { + ev.Emer = emr + emr.Maker(func(p *tree.Plan) { + width := height * .4 + depth := height * .15 + tree.AddAt(p, "body", func(n *physics.Box) { + n.SetSize(math32.Vec3(width, height, depth)). + SetColor("purple").SetDynamic(true). + SetInitPos(math32.Vec3(0, height/2, 0)) + }) + // body := physics.NewCapsule(emr, "body", math32.Vec3(0, height / 2, 0), height, width/2) + // body := physics.NewCylinder(emr, "body", math32.Vec3(0, height / 2, 0), height, width/2) + + headsz := depth * 1.5 + eyesz := headsz * .2 + hhsz := .5 * headsz + tree.AddAt(p, "head", func(n *physics.Group) { + n.SetInitPos(math32.Vec3(0, height+hhsz, 0)) + n.Maker(func(p *tree.Plan) { + tree.AddAt(p, "head", func(n *physics.Box) { + n.SetSize(math32.Vec3(headsz, headsz, headsz)). + SetColor("tan").SetDynamic(true).SetInitPos(math32.Vec3(0, 0, 0)) + n.InitView = func(vn tree.Node) { + sld := vn.(*xyz.Solid) + world.BoxInit(n, sld) + sld.Updater(func() { + clr := n.Color + if ev.EmerAngry { + clr = "pink" + } + world.UpdateColor(clr, n.View.(*xyz.Solid)) + }) + } + }) + tree.AddAt(p, "eye-l", func(n *physics.Box) { + n.SetSize(math32.Vec3(eyesz, eyesz*.5, eyesz*.2)). + SetColor("green").SetDynamic(true). + SetInitPos(math32.Vec3(-hhsz*.6, headsz*.1, -(hhsz + eyesz*.3))) + }) + tree.AddAt(p, "eye-r", func(n *physics.Box) { + ev.EyeR = n + n.SetSize(math32.Vec3(eyesz, eyesz*.5, eyesz*.2)). + SetColor("green").SetDynamic(true). + SetInitPos(math32.Vec3(hhsz*.6, headsz*.1, -(hhsz + eyesz*.3))) + }) + }) + }) + }) + }) +} + +func (ev *Env) ConfigGUI() *core.Body { + // vgpu.Debug = true + + b := core.NewBody("virtroom").SetTitle("Emergent Virtual Engine") + split := core.NewSplits(b) + + tv := core.NewTree(core.NewFrame(split)) + sv := core.NewForm(split).SetStruct(ev) + imfr := core.NewFrame(split) + tbvw := core.NewTabs(split) + scfr, _ := tbvw.NewTab("3D View") + + split.SetSplits(.1, .2, .2, .5) + + tv.OnSelect(func(e events.Event) { + if len(tv.SelectedNodes) > 0 { + sv.SetStruct(tv.SelectedNodes[0].AsCoreTree().SyncNode) + } + }) + + //////// 3D Scene + + etb := core.NewToolbar(scfr) + ev.SceneEditor = xyzcore.NewSceneEditor(scfr) + ev.SceneEditor.UpdateWidget() + sc := ev.SceneEditor.SceneXYZ() + ev.MakeWorld(sc) + tv.SyncTree(ev.World.World) + + // local toolbar for manipulating emer + etb.Maker(world.MakeStateToolbar(&ev.Emer.Rel, func() { + ev.World.Update() + ev.SceneEditor.NeedsRender() + })) + + sc.Camera.Pose.Pos = math32.Vec3(0, 40, 3.5) + sc.Camera.LookAt(math32.Vec3(0, 5, 0), math32.Vec3(0, 1, 0)) + sc.SaveCamera("3") + + sc.Camera.Pose.Pos = math32.Vec3(0, 20, 30) + sc.Camera.LookAt(math32.Vec3(0, 5, 0), math32.Vec3(0, 1, 0)) + sc.SaveCamera("2") + + sc.Camera.Pose.Pos = math32.Vec3(-.86, .97, 2.7) + sc.Camera.LookAt(math32.Vec3(0, .8, 0), math32.Vec3(0, 1, 0)) + sc.SaveCamera("1") + sc.SaveCamera("default") + + //////// Image + + imfr.Styler(func(s *styles.Style) { + s.Direction = styles.Column + }) + core.NewText(imfr).SetText("Right Eye Image:") + ev.EyeRImg = core.NewImage(imfr) + ev.EyeRImg.SetName("eye-r-img") + ev.EyeRImg.Image = image.NewRGBA(image.Rectangle{Max: ev.Camera.Size}) + + core.NewText(imfr).SetText("Right Eye Depth:") + ev.DepthImage = core.NewImage(imfr) + ev.DepthImage.SetName("depth-img") + ev.DepthImage.Image = image.NewRGBA(image.Rectangle{Max: ev.Camera.Size}) + + //////// Toolbar + + b.AddTopBar(func(bar *core.Frame) { + core.NewToolbar(bar).Maker(ev.MakeToolbar) + }) + return b +} + +func (ev *Env) MakeToolbar(p *tree.Plan) { + tree.Add(p, func(w *core.FuncButton) { + w.SetFunc(ev.WorldInit).SetText("Init").SetIcon(icons.Update) + }) + tree.Add(p, func(w *core.FuncButton) { + w.SetFunc(ev.GrabEyeImg).SetText("Grab Image").SetIcon(icons.Image) + }) + tree.Add(p, func(w *core.Separator) {}) + + tree.Add(p, func(w *core.FuncButton) { + w.SetFunc(ev.StepForward).SetText("Fwd").SetIcon(icons.SkipNext). + Styler(func(s *styles.Style) { + s.SetAbilities(true, abilities.RepeatClickable) + }) + }) + tree.Add(p, func(w *core.FuncButton) { + w.SetFunc(ev.StepBackward).SetText("Bkw").SetIcon(icons.SkipPrevious). + Styler(func(s *styles.Style) { + s.SetAbilities(true, abilities.RepeatClickable) + }) + }) + tree.Add(p, func(w *core.FuncButton) { + w.SetFunc(ev.RotBodyLeft).SetText("Body Left").SetIcon(icons.KeyboardArrowLeft). + Styler(func(s *styles.Style) { + s.SetAbilities(true, abilities.RepeatClickable) + }) + }) + tree.Add(p, func(w *core.FuncButton) { + w.SetFunc(ev.RotBodyRight).SetText("Body Right").SetIcon(icons.KeyboardArrowRight). + Styler(func(s *styles.Style) { + s.SetAbilities(true, abilities.RepeatClickable) + }) + }) + tree.Add(p, func(w *core.FuncButton) { + w.SetFunc(ev.RotHeadLeft).SetText("Head Left").SetIcon(icons.KeyboardArrowLeft). + Styler(func(s *styles.Style) { + s.SetAbilities(true, abilities.RepeatClickable) + }) + }) + tree.Add(p, func(w *core.FuncButton) { + w.SetFunc(ev.RotHeadRight).SetText("Head Right").SetIcon(icons.KeyboardArrowRight). + Styler(func(s *styles.Style) { + s.SetAbilities(true, abilities.RepeatClickable) + }) + }) + tree.Add(p, func(w *core.Separator) {}) + + tree.Add(p, func(w *core.Button) { + w.SetText("README").SetIcon(icons.FileMarkdown). + SetTooltip("Open browser on README."). + OnClick(func(e events.Event) { + core.TheApp.OpenURL("https://github.com/cogentcore/core/blob/master/xyz/examples/physics/README.md") + }) + }) +} + +func (ev *Env) NoGUIRun() { + gp, dev, err := gpu.NoDisplayGPU() + if err != nil { + panic(err) + } + sc := world.NoDisplayScene(gp, dev) + ev.MakeWorld(sc) + + img := ev.RenderEyeImg() + if img != nil { + imagex.Save(img, "eyer_0.png") + } +} diff --git a/physics/group.go b/physics/group.go new file mode 100644 index 00000000..bc1791d0 --- /dev/null +++ b/physics/group.go @@ -0,0 +1,251 @@ +// Copyright (c) 2019, Cogent Core. All rights reserved. +// Use of this source code is governed by a BSD-style +// license that can be found in the LICENSE file. + +package physics + +import ( + "sort" + + "cogentcore.org/core/math32" + "cogentcore.org/core/tree" +) + +// Group is a container of bodies, joints, or other groups +// it should be used strategically to partition the space +// and its BBox is used to optimize tree-based collision detection. +// Use a group for the top-level World node as well. +type Group struct { + NodeBase +} + +func (gp *Group) Update() { + gp.RunUpdaters() + gp.WalkDown(func(n tree.Node) bool { + if n.AsTree().This == gp.This { + return tree.Continue + } + n.(Node).Update() + return tree.Continue + }) +} + +func (gp *Group) InitAbs(par *NodeBase) { + gp.InitAbsBase(par) +} + +func (gp *Group) RelToAbs(par *NodeBase) { + gp.RelToAbsBase(par) // yes we can move groups +} + +func (gp *Group) Step(step float32) { + // groups do NOT update physics +} + +func (gp *Group) GroupBBox() { + hasDyn := false + gp.BBox.BBox.SetEmpty() + gp.BBox.VelBBox.SetEmpty() + for _, kid := range gp.Children { + n, nb := AsNode(kid) + if n == nil { + continue + } + gp.BBox.BBox.ExpandByBox(nb.BBox.BBox) + gp.BBox.VelBBox.ExpandByBox(nb.BBox.VelBBox) + if nb.Dynamic { + hasDyn = true + } + } + gp.SetDynamic(hasDyn) +} + +// WorldDynGroupBBox does a GroupBBox on all dynamic nodes +func (gp *Group) WorldDynGroupBBox() { + gp.WalkDownPost(func(tn tree.Node) bool { + n, nb := AsNode(tn) + if n == nil { + return false + } + if !nb.Dynamic { + return false + } + return true + }, func(tn tree.Node) bool { + n, nb := AsNode(tn) + if n == nil { + return false + } + if !nb.Dynamic { + return false + } + n.GroupBBox() + return true + }) +} + +// WorldInit does the full tree InitAbs and GroupBBox updates +func (gp *Group) WorldInit() { + gp.Update() + gp.WalkDown(func(tn tree.Node) bool { + n, _ := AsNode(tn) + if n == nil { + return false + } + _, pi := AsNode(tn.AsTree().Parent) + n.InitAbs(pi) + return true + }) + + gp.WalkDownPost(func(tn tree.Node) bool { + n, _ := AsNode(tn) + if n == nil { + return false + } + return true + }, func(tn tree.Node) bool { + n, _ := AsNode(tn) + if n == nil { + return false + } + n.GroupBBox() + return true + }) + +} + +// WorldRelToAbs does a full RelToAbs update for all Dynamic groups, for +// Scripted mode updates with manual updating of Rel values. +func (gp *Group) WorldRelToAbs() { + gp.WalkDown(func(tn tree.Node) bool { + n, nb := AsNode(tn) + if n == nil { + return false // going into a different type of thing, bail + } + if !nb.Dynamic { + return false + } + _, pi := AsNode(tn.AsTree().Parent) + n.RelToAbs(pi) + return true + }) + + gp.WorldDynGroupBBox() +} + +// WorldStep does a full Step update for all Dynamic nodes, for +// either physics or scripted mode, based on current velocities. +func (gp *Group) WorldStep(step float32) { + gp.WalkDown(func(tn tree.Node) bool { + n, nb := AsNode(tn) + if n == nil { + return false // going into a different type of thing, bail + } + if !nb.Dynamic { + return false + } + n.Step(step) + return true + }) + + gp.WorldDynGroupBBox() +} + +const ( + // DynsTopGps is passed to WorldCollide when all dynamic objects are in separate top groups + DynsTopGps = true + + // DynsSubGps is passed to WorldCollide when all dynamic objects are in separate groups under top + // level (i.e., one level deeper) + DynsSubGps +) + +// WorldCollide does first pass filtering step of collision detection +// based on separate dynamic vs. dynamic and dynamic vs. static groups. +// If dynTop is true, then each Dynamic group is separate at the top level -- +// otherwise they are organized at the next group level. +// Contacts are organized by dynamic group, when non-nil, for easier +// processing. +func (gp *Group) WorldCollide(dynTop bool) []Contacts { + var stats []Node + var dyns []Node + for _, kid := range gp.Children { + n, nb := AsNode(kid) + if n == nil { + continue + } + if nb.Dynamic { + dyns = append(dyns, n) + } else { + stats = append(stats, n) + } + } + + var sdyns []Node + if !dynTop { + for _, d := range dyns { + for _, dk := range d.AsTree().Children { + nii, _ := AsNode(dk) + if nii == nil { + continue + } + sdyns = append(sdyns, nii) + } + } + dyns = sdyns + } + + var cts []Contacts + for i, d := range dyns { + var dct Contacts + for _, s := range stats { + cc := BodyVelBBoxIntersects(d, s) + dct = append(dct, cc...) + } + for di := 0; di < i; di++ { + od := dyns[di] + cc := BodyVelBBoxIntersects(d, od) + dct = append(dct, cc...) + } + if len(dct) > 0 { + cts = append(cts, dct) + } + } + return cts +} + +// BodyPoint contains a Body and a Point on that body +type BodyPoint struct { + Body Body + Point math32.Vector3 +} + +// RayBodyIntersections returns a list of bodies whose bounding box intersects +// with the given ray, with the point of intersection +func (gp *Group) RayBodyIntersections(ray math32.Ray) []*BodyPoint { + var bs []*BodyPoint + gp.WalkDown(func(k tree.Node) bool { + nii, ni := AsNode(k) + if nii == nil { + return false // going into a different type of thing, bail + } + pt, has := ray.IntersectBox(ni.BBox.BBox) + if !has { + return false + } + bd := nii.AsBody() + if bd == nil { + return true + } + bs = append(bs, &BodyPoint{bd, pt}) + return false + }) + + sort.Slice(bs, func(i, j int) bool { + di := bs[i].Point.DistanceTo(ray.Origin) + dj := bs[j].Point.DistanceTo(ray.Origin) + return di < dj + }) + + return bs +} diff --git a/physics/node.go b/physics/node.go new file mode 100644 index 00000000..578ff137 --- /dev/null +++ b/physics/node.go @@ -0,0 +1,186 @@ +// Copyright (c) 2019, Cogent Core. All rights reserved. +// Use of this source code is governed by a BSD-style +// license that can be found in the LICENSE file. + +package physics + +//go:generate core generate -add-types + +import ( + "cogentcore.org/core/math32" + "cogentcore.org/core/tree" +) + +// Node is the common interface for all nodes. +type Node interface { + tree.Node + + // AsNodeBase returns a generic NodeBase for our node -- gives generic + // access to all the base-level data structures without needing interface methods. + AsNodeBase() *NodeBase + + // AsBody returns a generic Body interface for our node -- nil if not a Body + AsBody() Body + + // GroupBBox sets bounding boxes for groups based on groups or bodies. + // called in a FuncDownMeLast traversal. + GroupBBox() + + // InitAbs sets current Abs physical state parameters from Initial values + // which are local, relative to parent -- is passed the parent (nil = top). + // Body nodes should also set their bounding boxes. + // Called in a FuncDownMeFirst traversal. + InitAbs(par *NodeBase) + + // RelToAbs updates current world Abs physical state parameters + // based on Rel values added to updated Abs values at higher levels. + // Abs.LinVel is updated from the resulting change from prior position. + // This is useful for manual updating of relative positions (scripted movement). + // It is passed the parent (nil = top). + // Body nodes should also update their bounding boxes. + // Called in a FuncDownMeFirst traversal. + RelToAbs(par *NodeBase) + + // Step computes one update of the world Abs physical state parameters, + // using *current* velocities -- add forces prior to calling. + // Use this for physics-based state updates. + // Body nodes should also update their bounding boxes. + Step(step float32) + + // Update does [tree] updating to dynamically update nodes / tree config. + Update() +} + +// NodeBase is the basic node, which has position, rotation, velocity +// and computed bounding boxes, etc. +// There are only three different kinds of Nodes: Group, Body, and Joint +type NodeBase struct { + tree.NodeBase + + // Dynamic is whether this node can move. If it is false, then this is a Static node. + // Any top-level group that is not Dynamic is immediately pruned from further consideration, + // so top-level groups should be separated into Dynamic and Static nodes at the start. + Dynamic bool + + // initial position, orientation, velocity in *local* coordinates (relative to parent) + Initial State `display:"inline"` + + // current relative (local) position, orientation, velocity -- only change these values, as abs values are computed therefrom + Rel State `display:"inline"` + + // current absolute (world) position, orientation, velocity + Abs State `set:"-" edit:"-" display:"inline"` + + // bounding box in world coordinates (aggregated for groups) + BBox BBox `set:"-"` + + // NewView is a function that returns a new [xyz.Node] + // to represent this node. If nil, Groups make Groups, + // and bodies make corresponding Solid shape. + NewView func() tree.Node + + // InitView is a function that initializes a new [xyz.Node] + // that represents this physics node. If nil, Groups make Group children, + // and bodies configure corresponding Solid shape. + InitView func(n tree.Node) + + // View is the current view node for this node, set when made. + View tree.Node `set:"-"` +} + +func (nb *NodeBase) Init() { + nb.Updater(nb.UpdateFromMake) +} + +func (nb *NodeBase) AsNodeBase() *NodeBase { + return nb +} + +func (nb *NodeBase) AsBody() Body { + return nil +} + +// SetInitPos sets the initial position +func (nb *NodeBase) SetInitPos(pos math32.Vector3) *NodeBase { + nb.Initial.Pos = pos + return nb +} + +// SetInitQuat sets the initial rotation as a Quaternion +func (nb *NodeBase) SetInitQuat(quat math32.Quat) *NodeBase { + nb.Initial.Quat = quat + return nb +} + +// SetInitLinVel sets the initial linear velocity +func (nb *NodeBase) SetInitLinVel(vel math32.Vector3) *NodeBase { + nb.Initial.LinVel = vel + return nb +} + +// SetInitAngVel sets the initial angular velocity +func (nb *NodeBase) SetInitAngVel(vel math32.Vector3) *NodeBase { + nb.Initial.AngVel = vel + return nb +} + +// InitAbsBase is the base-level version of InitAbs -- most nodes call this. +// InitAbs sets current Abs physical state parameters from Initial values +// which are local, relative to parent -- is passed the parent (nil = top). +// Body nodes should also set their bounding boxes. +// Called in a FuncDownMeFirst traversal. +func (nb *NodeBase) InitAbsBase(par *NodeBase) { + if nb.Initial.Quat.IsNil() { + nb.Initial.Quat.SetIdentity() + } + nb.Rel = nb.Initial + if par != nil { + nb.Abs.FromRel(&nb.Initial, &par.Abs) + } else { + nb.Abs = nb.Initial + } +} + +// RelToAbsBase is the base-level version of RelToAbs -- most nodes call this. +// note: Group WorldRelToAbs ensures only called on Dynamic nodes. +// RelToAbs updates current world Abs physical state parameters +// based on Rel values added to updated Abs values at higher levels. +// Abs.LinVel is updated from the resulting change from prior position. +// This is useful for manual updating of relative positions (scripted movement). +// It is passed the parent (nil = top). +// Body nodes should also update their bounding boxes. +// Called in a FuncDownMeFirst traversal. +func (nb *NodeBase) RelToAbsBase(par *NodeBase) { + ppos := nb.Abs.Pos + if par != nil { + nb.Abs.FromRel(&nb.Rel, &par.Abs) + } else { + nb.Abs = nb.Rel + } + nb.Abs.LinVel = nb.Abs.Pos.Sub(ppos) // needed for VelBBox projection +} + +// StepBase is base-level version of Step -- most nodes call this. +// note: Group WorldRelToAbs ensures only called on Dynamic nodes. +// Computes one update of the world Abs physical state parameters, +// using *current* velocities -- add forces prior to calling. +// Use this for physics-based state updates. +// Body nodes should also update their bounding boxes. +func (nb *NodeBase) StepBase(step float32) { + nb.Abs.StepByAngVel(step) + nb.Abs.StepByLinVel(step) +} + +func (nb *NodeBase) Update() { + nb.RunUpdaters() +} + +// AsNode converts a [tree.Node] to a [Node] interface and a [Node3DBase] object, +// or nil if not possible. +func AsNode(n tree.Node) (Node, *NodeBase) { + nii, ok := n.(Node) + if ok { + return nii, nii.AsNodeBase() + } + return nil, nil +} diff --git a/physics/rigid.go b/physics/rigid.go new file mode 100644 index 00000000..7cde5331 --- /dev/null +++ b/physics/rigid.go @@ -0,0 +1,33 @@ +// Copyright (c) 2019, Cogent Core. All rights reserved. +// Use of this source code is governed by a BSD-style +// license that can be found in the LICENSE file. + +package physics + +import ( + "cogentcore.org/core/math32" +) + +// Rigid contains the full specification of a given object's basic physics +// properties including position, orientation, velocity. These +type Rigid struct { + + // 1/mass -- 0 for no mass + InvMass float32 + + // COR or coefficient of restitution -- how elastic is the collision i.e., final velocity / initial velocity + Bounce float32 `min:"0" max:"1"` + + // friction coefficient -- how much friction is generated by transverse motion + Friction float32 + + // record of computed force vector from last iteration + Force math32.Vector3 + + // Last calculated rotational inertia matrix in local coords + RotInertia math32.Matrix3 +} + +// Defaults sets defaults only if current values are nil +func (ps *Rigid) Defaults() { +} diff --git a/physics/sphere.go b/physics/sphere.go new file mode 100644 index 00000000..bf95b8d3 --- /dev/null +++ b/physics/sphere.go @@ -0,0 +1,40 @@ +// Copyright (c) 2019, Cogent Core. All rights reserved. +// Use of this source code is governed by a BSD-style +// license that can be found in the LICENSE file. + +package physics + +import ( + "cogentcore.org/core/math32" +) + +// Sphere is a spherical body shape. +type Sphere struct { + BodyBase + + // radius + Radius float32 +} + +func (sp *Sphere) SetBBox() { + sp.BBox.SetBounds(math32.Vec3(-sp.Radius, -sp.Radius, -sp.Radius), math32.Vec3(sp.Radius, sp.Radius, sp.Radius)) + sp.BBox.XForm(sp.Abs.Quat, sp.Abs.Pos) +} + +func (sp *Sphere) InitAbs(par *NodeBase) { + sp.InitAbsBase(par) + sp.SetBBox() + sp.BBox.VelNilProject() +} + +func (sp *Sphere) RelToAbs(par *NodeBase) { + sp.RelToAbsBase(par) + sp.SetBBox() + sp.BBox.VelProject(sp.Abs.LinVel, 1) +} + +func (sp *Sphere) Step(step float32) { + sp.StepBase(step) + sp.SetBBox() + sp.BBox.VelProject(sp.Abs.LinVel, step) +} diff --git a/physics/state.go b/physics/state.go new file mode 100644 index 00000000..5f4098dc --- /dev/null +++ b/physics/state.go @@ -0,0 +1,155 @@ +// Copyright (c) 2019, Cogent Core. All rights reserved. +// Use of this source code is governed by a BSD-style +// license that can be found in the LICENSE file. + +package physics + +import ( + "math" + + "cogentcore.org/core/math32" +) + +// State contains the basic physical state including position, orientation, velocity. +// These are only the values that can be either relative or absolute -- other physical +// state values such as Mass should go in Rigid. +type State struct { + + // position of center of mass of object + Pos math32.Vector3 + + // rotation specified as a Quat + Quat math32.Quat + + // linear velocity + LinVel math32.Vector3 + + // angular velocity + AngVel math32.Vector3 +} + +// Defaults sets defaults only if current values are nil +func (ps *State) Defaults() { + if ps.Quat.IsNil() { + ps.Quat.SetIdentity() + } +} + +//////// State updates + +// FromRel sets state from relative values compared to a parent state +func (ps *State) FromRel(rel, par *State) { + ps.Quat = rel.Quat.Mul(par.Quat) + ps.Pos = rel.Pos.MulQuat(par.Quat).Add(par.Pos) + ps.LinVel = rel.LinVel.MulQuat(rel.Quat).Add(par.LinVel) + ps.AngVel = rel.AngVel.MulQuat(rel.Quat).Add(par.AngVel) +} + +// AngMotionMax is maximum angular motion that can be taken per update +const AngMotionMax = math.Pi / 4 + +// StepByAngVel steps the Quat rotation from angular velocity +func (ps *State) StepByAngVel(step float32) { + ang := math32.Sqrt(ps.AngVel.Dot(ps.AngVel)) + + // limit the angular motion + if ang*step > AngMotionMax { + ang = AngMotionMax / step + } + var axis math32.Vector3 + if ang < 0.001 { + // use Taylor's expansions of sync function + axis = ps.AngVel.MulScalar(0.5*step - (step*step*step)*0.020833333333*ang*ang) + } else { + // sync(fAngle) = sin(c*fAngle)/t + axis = ps.AngVel.MulScalar(math32.Sin(0.5*ang*step) / ang) + } + var dq math32.Quat + dq.SetFromAxisAngle(axis, ang*step) + ps.Quat = dq.Mul(ps.Quat) + ps.Quat.Normalize() +} + +// StepByLinVel steps the Pos from the linear velocity +func (ps *State) StepByLinVel(step float32) { + ps.Pos = ps.Pos.Add(ps.LinVel.MulScalar(step)) +} + +//////// Moving + +// Move moves (translates) Pos by given amount, and sets the LinVel to the given +// delta -- this can be useful for Scripted motion to track movement. +func (ps *State) Move(delta math32.Vector3) { + ps.LinVel = delta + ps.Pos.SetAdd(delta) +} + +// MoveOnAxis moves (translates) the specified distance on the specified local axis, +// relative to the current rotation orientation. +// The axis is normalized prior to aplying the distance factor. +// Sets the LinVel to motion vector. +func (ps *State) MoveOnAxis(x, y, z, dist float32) { //types:add + ps.LinVel = math32.Vec3(x, y, z).Normal().MulQuat(ps.Quat).MulScalar(dist) + ps.Pos.SetAdd(ps.LinVel) +} + +// MoveOnAxisAbs moves (translates) the specified distance on the specified local axis, +// in absolute X,Y,Z coordinates (does not apply the Quat rotation factor. +// The axis is normalized prior to aplying the distance factor. +// Sets the LinVel to motion vector. +func (ps *State) MoveOnAxisAbs(x, y, z, dist float32) { //types:add + ps.LinVel = math32.Vec3(x, y, z).Normal().MulScalar(dist) + ps.Pos.SetAdd(ps.LinVel) +} + +//////// Rotating + +// SetEulerRotation sets the rotation in Euler angles (degrees). +func (ps *State) SetEulerRotation(x, y, z float32) { //types:add + ps.Quat.SetFromEuler(math32.Vec3(x, y, z).MulScalar(math32.DegToRadFactor)) +} + +// SetEulerRotationRad sets the rotation in Euler angles (radians). +func (ps *State) SetEulerRotationRad(x, y, z float32) { + ps.Quat.SetFromEuler(math32.Vec3(x, y, z)) +} + +// EulerRotation returns the current rotation in Euler angles (degrees). +func (ps *State) EulerRotation() math32.Vector3 { //types:add + return ps.Quat.ToEuler().MulScalar(math32.RadToDegFactor) +} + +// EulerRotationRad returns the current rotation in Euler angles (radians). +func (ps *State) EulerRotationRad() math32.Vector3 { + return ps.Quat.ToEuler() +} + +// SetAxisRotation sets rotation from local axis and angle in degrees. +func (ps *State) SetAxisRotation(x, y, z, angle float32) { //types:add + ps.Quat.SetFromAxisAngle(math32.Vec3(x, y, z), math32.DegToRad(angle)) +} + +// SetAxisRotationRad sets rotation from local axis and angle in radians. +func (ps *State) SetAxisRotationRad(x, y, z, angle float32) { + ps.Quat.SetFromAxisAngle(math32.Vec3(x, y, z), angle) +} + +// RotateOnAxis rotates around the specified local axis the specified angle in degrees. +func (ps *State) RotateOnAxis(x, y, z, angle float32) { //types:add + ps.Quat.SetMul(math32.NewQuatAxisAngle(math32.Vec3(x, y, z), math32.DegToRad(angle))) +} + +// RotateOnAxisRad rotates around the specified local axis the specified angle in radians. +func (ps *State) RotateOnAxisRad(x, y, z, angle float32) { + ps.Quat.SetMul(math32.NewQuatAxisAngle(math32.Vec3(x, y, z), angle)) +} + +// RotateEuler rotates by given Euler angles (in degrees) relative to existing rotation. +func (ps *State) RotateEuler(x, y, z float32) { //types:add + ps.Quat.SetMul(math32.NewQuatEuler(math32.Vec3(x, y, z).MulScalar(math32.DegToRadFactor))) +} + +// RotateEulerRad rotates by given Euler angles (in radians) relative to existing rotation. +func (ps *State) RotateEulerRad(x, y, z, angle float32) { + ps.Quat.SetMul(math32.NewQuatEuler(math32.Vec3(x, y, z))) +} diff --git a/physics/typegen.go b/physics/typegen.go new file mode 100644 index 00000000..c21ef0dd --- /dev/null +++ b/physics/typegen.go @@ -0,0 +1,140 @@ +// Code generated by "core generate -add-types"; DO NOT EDIT. + +package physics + +import ( + "cogentcore.org/core/math32" + "cogentcore.org/core/tree" + "cogentcore.org/core/types" +) + +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.BBox", IDName: "b-box", Doc: "BBox contains bounding box and other gross object properties", Fields: []types.Field{{Name: "BBox", Doc: "bounding box in world coords (Axis-Aligned Bounding Box = AABB)"}, {Name: "VelBBox", Doc: "velocity-projected bounding box in world coords: extend BBox to include future position of moving bodies -- collision must be made on this basis"}, {Name: "BSphere", Doc: "bounding sphere in local coords"}, {Name: "Area", Doc: "area"}, {Name: "Volume", Doc: "volume"}}}) + +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.Body", IDName: "body", Doc: "Body is the common interface for all body types", Methods: []types.Method{{Name: "AsBodyBase", Doc: "AsBodyBase returns the body as a BodyBase", Returns: []string{"BodyBase"}}}}) + +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.BodyBase", IDName: "body-base", Doc: "BodyBase is the base type for all specific Body types", Embeds: []types.Field{{Name: "NodeBase"}}, Fields: []types.Field{{Name: "Rigid", Doc: "rigid body properties, including mass, bounce, friction etc."}, {Name: "Color", Doc: "default color of body."}}}) + +// NewBodyBase returns a new [BodyBase] with the given optional parent: +// BodyBase is the base type for all specific Body types +func NewBodyBase(parent ...tree.Node) *BodyBase { return tree.New[BodyBase](parent...) } + +// SetRigid sets the [BodyBase.Rigid]: +// rigid body properties, including mass, bounce, friction etc. +func (t *BodyBase) SetRigid(v Rigid) *BodyBase { t.Rigid = v; return t } + +// SetColor sets the [BodyBase.Color]: +// default color of body. +func (t *BodyBase) SetColor(v string) *BodyBase { t.Color = v; return t } + +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.Box", IDName: "box", Doc: "Box is a box body shape", Embeds: []types.Field{{Name: "BodyBase"}}, Fields: []types.Field{{Name: "Size", Doc: "size of box in each dimension (units arbitrary, as long as they are all consistent -- meters is typical)"}}}) + +// NewBox returns a new [Box] with the given optional parent: +// Box is a box body shape +func NewBox(parent ...tree.Node) *Box { return tree.New[Box](parent...) } + +// SetSize sets the [Box.Size]: +// size of box in each dimension (units arbitrary, as long as they are all consistent -- meters is typical) +func (t *Box) SetSize(v math32.Vector3) *Box { t.Size = v; return t } + +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.Capsule", IDName: "capsule", Doc: "Capsule is a generalized cylinder body shape, with hemispheres at each end,\nwith separate radii for top and bottom.", Embeds: []types.Field{{Name: "BodyBase"}}, Fields: []types.Field{{Name: "Height", Doc: "height of the cylinder portion of the capsule"}, {Name: "TopRad", Doc: "radius of the top hemisphere"}, {Name: "BotRad", Doc: "radius of the bottom hemisphere"}}}) + +// NewCapsule returns a new [Capsule] with the given optional parent: +// Capsule is a generalized cylinder body shape, with hemispheres at each end, +// with separate radii for top and bottom. +func NewCapsule(parent ...tree.Node) *Capsule { return tree.New[Capsule](parent...) } + +// SetHeight sets the [Capsule.Height]: +// height of the cylinder portion of the capsule +func (t *Capsule) SetHeight(v float32) *Capsule { t.Height = v; return t } + +// SetTopRad sets the [Capsule.TopRad]: +// radius of the top hemisphere +func (t *Capsule) SetTopRad(v float32) *Capsule { t.TopRad = v; return t } + +// SetBotRad sets the [Capsule.BotRad]: +// radius of the bottom hemisphere +func (t *Capsule) SetBotRad(v float32) *Capsule { t.BotRad = v; return t } + +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.Contact", IDName: "contact", Doc: "Contact is one pairwise point of contact between two bodies.\nContacts are represented in spherical terms relative to the\nspherical BBox of A and B.", Fields: []types.Field{{Name: "A", Doc: "one body"}, {Name: "B", Doc: "the other body"}, {Name: "NormB", Doc: "normal pointing from center of B to center of A"}, {Name: "PtB", Doc: "point on spherical shell of B where A is contacting"}, {Name: "Dist", Doc: "distance from PtB along NormB to contact point on spherical shell of A"}}}) + +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.Contacts", IDName: "contacts", Doc: "Contacts is a slice list of contacts"}) + +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.Cylinder", IDName: "cylinder", Doc: "Cylinder is a generalized cylinder body shape, with separate radii for top and bottom.\nA cone has a zero radius at one end.", Embeds: []types.Field{{Name: "BodyBase"}}, Fields: []types.Field{{Name: "Height", Doc: "height of the cylinder"}, {Name: "TopRad", Doc: "radius of the top -- set to 0 for a cone"}, {Name: "BotRad", Doc: "radius of the bottom"}}}) + +// NewCylinder returns a new [Cylinder] with the given optional parent: +// Cylinder is a generalized cylinder body shape, with separate radii for top and bottom. +// A cone has a zero radius at one end. +func NewCylinder(parent ...tree.Node) *Cylinder { return tree.New[Cylinder](parent...) } + +// SetHeight sets the [Cylinder.Height]: +// height of the cylinder +func (t *Cylinder) SetHeight(v float32) *Cylinder { t.Height = v; return t } + +// SetTopRad sets the [Cylinder.TopRad]: +// radius of the top -- set to 0 for a cone +func (t *Cylinder) SetTopRad(v float32) *Cylinder { t.TopRad = v; return t } + +// SetBotRad sets the [Cylinder.BotRad]: +// radius of the bottom +func (t *Cylinder) SetBotRad(v float32) *Cylinder { t.BotRad = v; return t } + +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.Group", IDName: "group", Doc: "Group is a container of bodies, joints, or other groups\nit should be used strategically to partition the space\nand its BBox is used to optimize tree-based collision detection.\nUse a group for the top-level World node as well.", Embeds: []types.Field{{Name: "NodeBase"}}}) + +// NewGroup returns a new [Group] with the given optional parent: +// Group is a container of bodies, joints, or other groups +// it should be used strategically to partition the space +// and its BBox is used to optimize tree-based collision detection. +// Use a group for the top-level World node as well. +func NewGroup(parent ...tree.Node) *Group { return tree.New[Group](parent...) } + +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.BodyPoint", IDName: "body-point", Doc: "BodyPoint contains a Body and a Point on that body", Fields: []types.Field{{Name: "Body"}, {Name: "Point"}}}) + +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.Node", IDName: "node", Doc: "Node is the common interface for all nodes.", Methods: []types.Method{{Name: "AsNodeBase", Doc: "AsNodeBase returns a generic NodeBase for our node -- gives generic\naccess to all the base-level data structures without needing interface methods.", Returns: []string{"NodeBase"}}, {Name: "AsBody", Doc: "AsBody returns a generic Body interface for our node -- nil if not a Body", Returns: []string{"Body"}}, {Name: "GroupBBox", Doc: "GroupBBox sets bounding boxes for groups based on groups or bodies.\ncalled in a FuncDownMeLast traversal."}, {Name: "InitAbs", Doc: "InitAbs sets current Abs physical state parameters from Initial values\nwhich are local, relative to parent -- is passed the parent (nil = top).\nBody nodes should also set their bounding boxes.\nCalled in a FuncDownMeFirst traversal.", Args: []string{"par"}}, {Name: "RelToAbs", Doc: "RelToAbs updates current world Abs physical state parameters\nbased on Rel values added to updated Abs values at higher levels.\nAbs.LinVel is updated from the resulting change from prior position.\nThis is useful for manual updating of relative positions (scripted movement).\nIt is passed the parent (nil = top).\nBody nodes should also update their bounding boxes.\nCalled in a FuncDownMeFirst traversal.", Args: []string{"par"}}, {Name: "Step", Doc: "Step computes one update of the world Abs physical state parameters,\nusing *current* velocities -- add forces prior to calling.\nUse this for physics-based state updates.\nBody nodes should also update their bounding boxes.", Args: []string{"step"}}, {Name: "Update", Doc: "Update does [tree] updating to dynamically update nodes / tree config."}}}) + +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.NodeBase", IDName: "node-base", Doc: "NodeBase is the basic node, which has position, rotation, velocity\nand computed bounding boxes, etc.\nThere are only three different kinds of Nodes: Group, Body, and Joint", Embeds: []types.Field{{Name: "NodeBase"}}, Fields: []types.Field{{Name: "Dynamic", Doc: "Dynamic is whether this node can move. If it is false, then this is a Static node.\nAny top-level group that is not Dynamic is immediately pruned from further consideration,\nso top-level groups should be separated into Dynamic and Static nodes at the start."}, {Name: "Initial", Doc: "initial position, orientation, velocity in *local* coordinates (relative to parent)"}, {Name: "Rel", Doc: "current relative (local) position, orientation, velocity -- only change these values, as abs values are computed therefrom"}, {Name: "Abs", Doc: "current absolute (world) position, orientation, velocity"}, {Name: "BBox", Doc: "bounding box in world coordinates (aggregated for groups)"}, {Name: "NewView", Doc: "NewView is a function that returns a new [xyz.Node]\nto represent this node. If nil, Groups make Groups,\nand bodies make corresponding Solid shape."}, {Name: "InitView", Doc: "InitView is a function that initializes a new [xyz.Node]\nthat represents this physics node. If nil, Groups make Group children,\nand bodies configure corresponding Solid shape."}, {Name: "View", Doc: "View is the current view node for this node, set when made."}}}) + +// NewNodeBase returns a new [NodeBase] with the given optional parent: +// NodeBase is the basic node, which has position, rotation, velocity +// and computed bounding boxes, etc. +// There are only three different kinds of Nodes: Group, Body, and Joint +func NewNodeBase(parent ...tree.Node) *NodeBase { return tree.New[NodeBase](parent...) } + +// SetDynamic sets the [NodeBase.Dynamic]: +// Dynamic is whether this node can move. If it is false, then this is a Static node. +// Any top-level group that is not Dynamic is immediately pruned from further consideration, +// so top-level groups should be separated into Dynamic and Static nodes at the start. +func (t *NodeBase) SetDynamic(v bool) *NodeBase { t.Dynamic = v; return t } + +// SetInitial sets the [NodeBase.Initial]: +// initial position, orientation, velocity in *local* coordinates (relative to parent) +func (t *NodeBase) SetInitial(v State) *NodeBase { t.Initial = v; return t } + +// SetRel sets the [NodeBase.Rel]: +// current relative (local) position, orientation, velocity -- only change these values, as abs values are computed therefrom +func (t *NodeBase) SetRel(v State) *NodeBase { t.Rel = v; return t } + +// SetNewView sets the [NodeBase.NewView]: +// NewView is a function that returns a new [xyz.Node] +// to represent this node. If nil, Groups make Groups, +// and bodies make corresponding Solid shape. +func (t *NodeBase) SetNewView(v func() tree.Node) *NodeBase { t.NewView = v; return t } + +// SetInitView sets the [NodeBase.InitView]: +// InitView is a function that initializes a new [xyz.Node] +// that represents this physics node. If nil, Groups make Group children, +// and bodies configure corresponding Solid shape. +func (t *NodeBase) SetInitView(v func(n tree.Node)) *NodeBase { t.InitView = v; return t } + +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.Rigid", IDName: "rigid", Doc: "Rigid contains the full specification of a given object's basic physics\nproperties including position, orientation, velocity. These", Fields: []types.Field{{Name: "InvMass", Doc: "1/mass -- 0 for no mass"}, {Name: "Bounce", Doc: "COR or coefficient of restitution -- how elastic is the collision i.e., final velocity / initial velocity"}, {Name: "Friction", Doc: "friction coefficient -- how much friction is generated by transverse motion"}, {Name: "Force", Doc: "record of computed force vector from last iteration"}, {Name: "RotInertia", Doc: "Last calculated rotational inertia matrix in local coords"}}}) + +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.Sphere", IDName: "sphere", Doc: "Sphere is a spherical body shape.", Embeds: []types.Field{{Name: "BodyBase"}}, Fields: []types.Field{{Name: "Radius", Doc: "radius"}}}) + +// NewSphere returns a new [Sphere] with the given optional parent: +// Sphere is a spherical body shape. +func NewSphere(parent ...tree.Node) *Sphere { return tree.New[Sphere](parent...) } + +// SetRadius sets the [Sphere.Radius]: +// radius +func (t *Sphere) SetRadius(v float32) *Sphere { t.Radius = v; return t } + +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.State", IDName: "state", Doc: "State contains the basic physical state including position, orientation, velocity.\nThese are only the values that can be either relative or absolute -- other physical\nstate values such as Mass should go in Rigid.", Methods: []types.Method{{Name: "MoveOnAxis", Doc: "MoveOnAxis moves (translates) the specified distance on the specified local axis,\nrelative to the current rotation orientation.\nThe axis is normalized prior to aplying the distance factor.\nSets the LinVel to motion vector.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z", "dist"}}, {Name: "MoveOnAxisAbs", Doc: "MoveOnAxisAbs moves (translates) the specified distance on the specified local axis,\nin absolute X,Y,Z coordinates (does not apply the Quat rotation factor.\nThe axis is normalized prior to aplying the distance factor.\nSets the LinVel to motion vector.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z", "dist"}}, {Name: "SetEulerRotation", Doc: "SetEulerRotation sets the rotation in Euler angles (degrees).", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z"}}, {Name: "EulerRotation", Doc: "EulerRotation returns the current rotation in Euler angles (degrees).", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Returns: []string{"Vector3"}}, {Name: "SetAxisRotation", Doc: "SetAxisRotation sets rotation from local axis and angle in degrees.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z", "angle"}}, {Name: "RotateOnAxis", Doc: "RotateOnAxis rotates around the specified local axis the specified angle in degrees.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z", "angle"}}, {Name: "RotateEuler", Doc: "RotateEuler rotates by given Euler angles (in degrees) relative to existing rotation.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z"}}}, Fields: []types.Field{{Name: "Pos", Doc: "position of center of mass of object"}, {Name: "Quat", Doc: "rotation specified as a Quat"}, {Name: "LinVel", Doc: "linear velocity"}, {Name: "AngVel", Doc: "angular velocity"}}}) diff --git a/physics/world/camera.go b/physics/world/camera.go new file mode 100644 index 00000000..a88ca255 --- /dev/null +++ b/physics/world/camera.go @@ -0,0 +1,53 @@ +// Copyright (c) 2019, Cogent Core. All rights reserved. +// Use of this source code is governed by a BSD-style +// license that can be found in the LICENSE file. + +package world + +import ( + "image" + + "cogentcore.org/core/math32" +) + +// Camera defines the properties of a camera needed for rendering from a node. +type Camera struct { + + // size of image to record + Size image.Point + + // field of view in degrees + FOV float32 + + // near plane z coordinate + Near float32 `default:"0.01"` + + // far plane z coordinate + Far float32 `default:"1000"` + + // maximum distance for depth maps. Anything above is 1. + // This is independent of Near / Far rendering (though must be < Far) + // and is for normalized depth maps. + MaxD float32 `default:"20"` + + // use the natural log of 1 + depth for normalized depth values in display etc. + LogD bool `default:"true"` + + // number of multi-samples to use for antialising -- 4 is best and default. + MSample int `default:"4"` + + // up direction for camera. Defaults to positive Y axis, + // and is reset by call to LookAt method. + UpDir math32.Vector3 +} + +func (cm *Camera) Defaults() { + cm.Size = image.Point{320, 180} + cm.FOV = 30 + cm.Near = .01 + cm.Far = 1000 + cm.MaxD = 20 + cm.LogD = true + cm.MSample = 4 + cm.UpDir = math32.Vec3(0, 1, 0) +} diff --git a/physics/world/depth.go b/physics/world/depth.go new file mode 100644 index 00000000..6ac87c18 --- /dev/null +++ b/physics/world/depth.go @@ -0,0 +1,96 @@ +// Copyright (c) 2019, Cogent Core. All rights reserved. +// Use of this source code is governed by a BSD-style +// license that can be found in the LICENSE file. + +package world + +import ( + "image" + + "cogentcore.org/core/base/slicesx" + "cogentcore.org/core/colors/colormap" + "cogentcore.org/core/math32" +) + +// DepthNorm renders a normalized linear depth map from GPU (0-1 normalized floats) to +// given float slice, which is resized if not already appropriate size. +// if flipY then Y axis is flipped -- input is bottom-Y = 0. +// Camera params determine whether log is used, and max cutoff distance for sensitive +// range of distances -- also has Near / Far required to transform numbers into +// linearized distance values. +func DepthNorm(nd *[]float32, depth []float32, cam *Camera, flipY bool) { + sz := cam.Size + totn := sz.X * sz.Y + *nd = slicesx.SetLength(*nd, totn) + fpn := cam.Far + cam.Near + fmn := cam.Far - cam.Near + var norm float32 + if cam.LogD { + norm = 1 / math32.Log(1+cam.MaxD) + } else { + norm = 1 / cam.MaxD + } + + twonf := (2.0 * cam.Near * cam.Far) + for y := 0; y < sz.Y; y++ { + for x := 0; x < sz.X; x++ { + oi := y*sz.X + x + ii := oi + if flipY { + ii = (sz.Y-y-1)*sz.X + x + } + d := depth[ii] + z := d*2 - 1 // convert from 0..1 to -1..1 + lind := twonf / (fpn - (z * fmn)) // untransform + effd := float32(1) + if lind < cam.MaxD { + if cam.LogD { + effd = norm * math32.Log(1+lind) + } else { + effd = norm * lind + } + } + (*nd)[oi] = effd + } + } +} + +// DepthImage renders an image of linear depth map from GPU (0-1 normalized floats) to +// given image, which must be of appropriate size for map, using given colormap name. +// Camera params determine whether log is used, and max cutoff distance for sensitive +// range of distances -- also has Near / Far required to transform numbers into +// linearized distance values. Y axis is always flipped. +func DepthImage(img *image.RGBA, depth []float32, cmap *colormap.Map, cam *Camera) { + if img == nil { + return + } + sz := img.Bounds().Size() + fpn := cam.Far + cam.Near + fmn := cam.Far - cam.Near + var norm float32 + if cam.LogD { + norm = 1 / math32.Log(1+cam.MaxD) + } else { + norm = 1 / cam.MaxD + } + + twonf := (2.0 * cam.Near * cam.Far) + for y := 0; y < sz.Y; y++ { + for x := 0; x < sz.X; x++ { + ii := (sz.Y-y-1)*sz.X + x // always flip for images + d := depth[ii] + z := d*2 - 1 // convert from 0..1 to -1..1 + lind := twonf / (fpn - (z * fmn)) // untransform + effd := float32(1) + if lind < cam.MaxD { + if cam.LogD { + effd = norm * math32.Log(1+lind) + } else { + effd = norm * lind + } + } + clr := cmap.Map(effd) + img.Set(x, y, clr) + } + } +} diff --git a/physics/world/nogui.go b/physics/world/nogui.go new file mode 100644 index 00000000..07be1bc7 --- /dev/null +++ b/physics/world/nogui.go @@ -0,0 +1,24 @@ +// Copyright (c) 2019, Cogent Core. All rights reserved. +// Use of this source code is governed by a BSD-style +// license that can be found in the LICENSE file. + +package world + +import ( + "image" + + "cogentcore.org/core/gpu" + "cogentcore.org/core/xyz" +) + +// NoDisplayScene returns a xyz Scene initialized and ready to use +// in NoGUI offscreen rendering mode, using given GPU and device. +// Must manually call Init3D and Style3D on the Scene prior to +// a RenderFromNode call to grab the image from a specific camera. +func NoDisplayScene(gp *gpu.GPU, dev *gpu.Device) *xyz.Scene { + sc := xyz.NewScene() + sc.MultiSample = 4 + sc.Geom.Size = image.Point{1024, 768} + sc.ConfigOffscreen(gp, dev) + return sc +} diff --git a/physics/world/typegen.go b/physics/world/typegen.go new file mode 100644 index 00000000..6b2fc78d --- /dev/null +++ b/physics/world/typegen.go @@ -0,0 +1,11 @@ +// Code generated by "core generate -add-types"; DO NOT EDIT. + +package world + +import ( + "cogentcore.org/core/types" +) + +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics/world.Camera", IDName: "camera", Doc: "Camera defines the properties of a camera needed for rendering from a node.", Fields: []types.Field{{Name: "Size", Doc: "size of image to record"}, {Name: "FOV", Doc: "field of view in degrees"}, {Name: "Near", Doc: "near plane z coordinate"}, {Name: "Far", Doc: "far plane z coordinate"}, {Name: "MaxD", Doc: "maximum distance for depth maps. Anything above is 1.\nThis is independent of Near / Far rendering (though must be < Far)\nand is for normalized depth maps."}, {Name: "LogD", Doc: "use the natural log of 1 + depth for normalized depth values in display etc."}, {Name: "MSample", Doc: "number of multi-samples to use for antialising -- 4 is best and default."}, {Name: "UpDir", Doc: "up direction for camera. Defaults to positive Y axis,\nand is reset by call to LookAt method."}}}) + +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics/world.World", IDName: "world", Doc: "World connects a Virtual World with an [xyz.Scene] to visualize the world,\nincluding ability to render offscreen.", Fields: []types.Field{{Name: "World", Doc: "World is the root Group node of the virtual world"}, {Name: "Scene", Doc: "Scene is the [xyz.Scene] object for visualizing."}, {Name: "Root", Doc: "Root is the root Group node in the Scene under which the world is rendered."}}}) diff --git a/physics/world/update.go b/physics/world/update.go new file mode 100644 index 00000000..ef693f7f --- /dev/null +++ b/physics/world/update.go @@ -0,0 +1,151 @@ +// Copyright (c) 2019, Cogent Core. All rights reserved. +// Use of this source code is governed by a BSD-style +// license that can be found in the LICENSE file. + +package world + +import ( + "cogentcore.org/core/base/errors" + "cogentcore.org/core/colors" + "cogentcore.org/core/tree" + "cogentcore.org/core/xyz" + "cogentcore.org/lab/physics" +) + +// todo: add type that manages a world, view etc -- no need to replicate that +// and change name of world package to something better -- confusing, although +// the new type will be world so actually that is ok. + +// Add adds given physics node to the [tree.Plan], using NewView +// function on the node, or default. +func Add(p *tree.Plan, nb *physics.NodeBase) { + newFunc := nb.NewView + if newFunc == nil { + if _, ok := nb.This.(*physics.Group); ok { + newFunc = func() tree.Node { + return any(tree.New[xyz.Group]()).(tree.Node) + } + } else if _, ok := nb.This.(physics.Body); ok { + newFunc = func() tree.Node { + return any(tree.New[xyz.Solid]()).(tree.Node) + } + } // todo: joint + } + p.Add(nb.Name, newFunc, func(n tree.Node) { Init(nb, n) }) +} + +// Init is the physics node initialization function, +// which calls InitView if set on the node, or the default. +func Init(nb *physics.NodeBase, n tree.Node) { + initFunc := nb.InitView + if initFunc != nil { + initFunc(n) + return + } + switch x := nb.This.(type) { + case *physics.Group: + GroupInit(x, n.(*xyz.Group)) + case *physics.Box: + BoxInit(x, n.(*xyz.Solid)) + case *physics.Cylinder: + CylinderInit(x, n.(*xyz.Solid)) + case *physics.Capsule: + CapsuleInit(x, n.(*xyz.Solid)) + case *physics.Sphere: + SphereInit(x, n.(*xyz.Solid)) + } +} + +// GroupInit is the default InitView function for groups. +func GroupInit(gp *physics.Group, vgp *xyz.Group) { + gp.View = vgp.This + vgp.Maker(func(p *tree.Plan) { + for _, c := range gp.Children { + Add(p, c.(physics.Node).AsNodeBase()) + } + }) + vgp.Updater(func() { + UpdatePose(gp.AsNodeBase(), vgp.AsNodeBase()) + }) +} + +// UpdatePose updates the view node pose from physics node state. +func UpdatePose(nd *physics.NodeBase, vn *xyz.NodeBase) { + vn.Pose.Pos = nd.Rel.Pos + vn.Pose.Quat = nd.Rel.Quat +} + +// UpdateColor updates the view color to given color. +func UpdateColor(clr string, sld *xyz.Solid) { + if clr != "" { + sld.Material.Color = errors.Log1(colors.FromString(clr)) + } +} + +// BoxInit is the default InitView function for [physics.Box]. +// Only updates Pose in Updater: if node will change size or color, +// add updaters for that. +func BoxInit(bx *physics.Box, sld *xyz.Solid) { + bx.View = sld.This + mnm := "physics.Box" + if ms, _ := sld.Scene.MeshByName(mnm); ms == nil { + xyz.NewBox(sld.Scene, mnm, 1, 1, 1) + } + sld.SetMeshName(mnm) + sld.Pose.Scale = bx.Size + UpdateColor(bx.Color, sld) + sld.Updater(func() { + UpdatePose(bx.AsNodeBase(), sld.AsNodeBase()) + }) +} + +// CylinderInit is the default InitView function for [physics.Cylinder]. +// Only updates Pose in Updater: if node will change size or color, +// add updaters for that. +func CylinderInit(cy *physics.Cylinder, sld *xyz.Solid) { + cy.View = sld.This + mnm := "physics.Cylinder" + if ms, _ := sld.Scene.MeshByName(mnm); ms == nil { + xyz.NewCylinder(sld.Scene, mnm, 1, 1, 32, 1, true, true) + } + sld.SetMeshName(mnm) + sld.Pose.Scale.Set(cy.BotRad, cy.Height, cy.BotRad) + UpdateColor(cy.Color, sld) + sld.Updater(func() { + UpdatePose(cy.AsNodeBase(), sld.AsNodeBase()) + }) +} + +// CapsuleInit is the default InitView function for [physics.Capsule]. +// Only updates Pose in Updater: if node will change size or color, +// add updaters for that. +func CapsuleInit(cp *physics.Capsule, sld *xyz.Solid) { + cp.View = sld.This + mnm := "physics.Capsule" + if ms, _ := sld.Scene.MeshByName(mnm); ms == nil { + ms = xyz.NewCapsule(sld.Scene, mnm, 1, .2, 32, 1) + } + sld.SetMeshName(mnm) + sld.Pose.Scale.Set(cp.BotRad/.2, cp.Height/1.4, cp.BotRad/.2) + UpdateColor(cp.Color, sld) + sld.Updater(func() { + UpdatePose(cp.AsNodeBase(), sld.AsNodeBase()) + }) +} + +// SphereInit is the default InitView function for [physics.Sphere]. +// Only updates Pose in Updater: if node will change size or color, +// add updaters for that. +func SphereInit(sp *physics.Sphere, sld *xyz.Solid) { + sp.View = sld.This + mnm := "physics.Sphere" + if ms, _ := sld.Scene.MeshByName(mnm); ms == nil { + ms = xyz.NewSphere(sld.Scene, mnm, 1, 32) + } + sld.SetMeshName(mnm) + sld.Pose.Scale.SetScalar(sp.Radius) + UpdateColor(sp.Color, sld) + sld.Updater(func() { + UpdatePose(sp.AsNodeBase(), sld.AsNodeBase()) + }) +} diff --git a/physics/world/world.go b/physics/world/world.go new file mode 100644 index 00000000..3812d2d7 --- /dev/null +++ b/physics/world/world.go @@ -0,0 +1,117 @@ +// Copyright (c) 2019, Cogent Core. All rights reserved. +// Use of this source code is governed by a BSD-style +// license that can be found in the LICENSE file. + +// package world implements visualization of [physics] using [xyz] +// 3D graphics. +package world + +//go:generate core generate -add-types + +import ( + "image" + + "cogentcore.org/core/core" + "cogentcore.org/core/icons" + "cogentcore.org/core/tree" + "cogentcore.org/core/xyz" + "cogentcore.org/lab/physics" +) + +// World connects a Virtual World with an [xyz.Scene] to visualize the world, +// including ability to render offscreen. +type World struct { + + // World is the root Group node of the virtual world + World *physics.Group + + // Scene is the [xyz.Scene] object for visualizing. + Scene *xyz.Scene + + // Root is the root Group node in the Scene under which the world is rendered. + Root *xyz.Group +} + +// NewWorld returns a new World that links given [physics] world +// (top level Group) with given [xyz.Scene], making a +// top-level Root group in the scene. +func NewWorld(world *physics.Group, sc *xyz.Scene) *World { + rgp := xyz.NewGroup(sc) + rgp.SetName("world") + wr := &World{World: world, Scene: sc, Root: rgp} + GroupInit(wr.World, rgp) + wr.Update() + return wr +} + +// Init initializes the physics world, e.g., at start of a new sim run. +func (wr *World) Init() { + wr.World.WorldInit() + wr.Update() +} + +// Update updates the view from current physics node state. +func (wr *World) Update() { + if wr.Scene != nil { + wr.Scene.Update() + } +} + +// RenderFromNode does an offscreen render using given node +// for the camera position and orientation, returning the render image. +// Current scene camera is saved and restored. +func (wr *World) RenderFromNode(node physics.Node, cam *Camera) image.Image { + sc := wr.Scene + camnm := "physics-view-rendernode-save" + sc.SaveCamera(camnm) + defer func() { + sc.SetCamera(camnm) + sc.UseMainFrame() + }() + + sc.Camera.FOV = cam.FOV + sc.Camera.Near = cam.Near + sc.Camera.Far = cam.Far + nb := node.AsNodeBase() + sc.Camera.Pose.Pos = nb.Abs.Pos + sc.Camera.Pose.Quat = nb.Abs.Quat + sc.Camera.Pose.Scale.Set(1, 1, 1) + + sc.UseAltFrame(cam.Size) + return sc.RenderGrabImage() +} + +// DepthImage returns the current rendered depth image +// func (vw *World) DepthImage() ([]float32, error) { +// return vw.Scene.DepthImage() +// } + +// MakeStateToolbar returns a toolbar function for physics state updates, +// calling the given updt function after making the change. +func MakeStateToolbar(ps *physics.State, updt func()) func(p *tree.Plan) { + return func(p *tree.Plan) { + tree.Add(p, func(w *core.FuncButton) { + w.SetFunc(ps.SetEulerRotation).SetAfterFunc(updt).SetIcon(icons.Rotate90DegreesCcw) + }) + tree.Add(p, func(w *core.FuncButton) { + w.SetFunc(ps.SetAxisRotation).SetAfterFunc(updt).SetIcon(icons.Rotate90DegreesCcw) + }) + tree.Add(p, func(w *core.FuncButton) { + w.SetFunc(ps.RotateEuler).SetAfterFunc(updt).SetIcon(icons.Rotate90DegreesCcw) + }) + tree.Add(p, func(w *core.FuncButton) { + w.SetFunc(ps.RotateOnAxis).SetAfterFunc(updt).SetIcon(icons.Rotate90DegreesCcw) + }) + tree.Add(p, func(w *core.FuncButton) { + w.SetFunc(ps.EulerRotation).SetAfterFunc(updt).SetShowReturn(true).SetIcon(icons.Rotate90DegreesCcw) + }) + tree.Add(p, func(w *core.Separator) {}) + tree.Add(p, func(w *core.FuncButton) { + w.SetFunc(ps.MoveOnAxis).SetAfterFunc(updt).SetIcon(icons.MoveItem) + }) + tree.Add(p, func(w *core.FuncButton) { + w.SetFunc(ps.MoveOnAxisAbs).SetAfterFunc(updt).SetIcon(icons.MoveItem) + }) + + } +} From 38af4434ca316d335179e8c307ad0c9f02a1d462 Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Fri, 12 Dec 2025 23:54:23 +0100 Subject: [PATCH 10/97] physics: new GPU design mostly in place, working on joints --- physics/README.md | 29 +- physics/body.go | 260 ++++++++++++-- physics/body.goal | 256 ++++++++++++++ physics/box.go | 40 --- physics/capsule.go | 49 --- physics/collide.go | 41 +-- physics/cylinder.go | 48 --- physics/enumgen.go | 328 ++++++++++++++++++ physics/examples/virtroom/virtroom.go | 226 +++++------- physics/gosl.go | 392 +++++++++++++++++++++ physics/group.go | 481 +++++++++++++------------- physics/joint.go | 98 ++++++ physics/joint.goal | 96 +++++ physics/node.go | 186 ---------- physics/params.go | 34 ++ physics/rigid.go | 33 -- physics/shaders/InitDynamics.wgsl | 145 ++++++++ physics/shaders/Step.wgsl | 131 +++++++ physics/shaders/StepJoints.wgsl | 170 +++++++++ physics/shapes.go | 53 +++ physics/sphere.go | 40 --- physics/state.go | 14 +- physics/step.go | 68 ++++ physics/step.goal | 66 ++++ physics/typegen.go | 133 +------ physics/vars.go | 45 +++ physics/world.go | 131 +++++++ physics/world.goal | 129 +++++++ physics/world/update.go | 151 -------- physics/world/view.go | 193 +++++++++++ physics/world/world.go | 61 ++-- 31 files changed, 3005 insertions(+), 1122 deletions(-) create mode 100644 physics/body.goal delete mode 100644 physics/box.go delete mode 100644 physics/capsule.go delete mode 100644 physics/cylinder.go create mode 100644 physics/enumgen.go create mode 100644 physics/gosl.go create mode 100644 physics/joint.go create mode 100644 physics/joint.goal delete mode 100644 physics/node.go create mode 100644 physics/params.go delete mode 100644 physics/rigid.go create mode 100644 physics/shaders/InitDynamics.wgsl create mode 100644 physics/shaders/Step.wgsl create mode 100644 physics/shaders/StepJoints.wgsl create mode 100644 physics/shapes.go delete mode 100644 physics/sphere.go create mode 100644 physics/step.go create mode 100644 physics/step.goal create mode 100644 physics/vars.go create mode 100644 physics/world.go create mode 100644 physics/world.goal delete mode 100644 physics/world/update.go create mode 100644 physics/world/view.go diff --git a/physics/README.md b/physics/README.md index c94a402b..2f50f728 100644 --- a/physics/README.md +++ b/physics/README.md @@ -1,22 +1,28 @@ # Physics engine for virtual reality -This `physics` engine is a scenegraph-based 3D physics simulator for creating virtual environments. It provides a `Body` node for rigid body physics, along with some basic geometrical shapes thereof. The `physics` scene contains just the bare physics bodies and other elements, which can be updated independent of any visualization. +The `physics` engine is a 3D physics simulator for creating virtual environments, which can run on the GPU or CPU using [gosl](https://cogentcore.org/lab/gosl). -Currently, it provides collision detection and basic forward Euler physics updating, but it does not yet compute any forces for the interactions among the bodies. Ultimately we hope to figure out how the [Bullet](https://github.com/bulletphysics/bullet3) system works and get that running here, in a clean and simple implementation. +The overall design emphasizes simplicity and robustness over exact physics precision. A simple forward Euler integration of aggregated forces is computed, with strong limits, soft constraints, and damping dynamics to prevent numerical instability even with relatively large step sizes. -Incrementally, we will start with a basic explicitly driven form of physics that is sufficient to get started, and build from there. +All interactions are mediated by `Joint` elements that connect two rigid `Body` elements. Optimized joint types enable robust implementation of specific types of interactions. + +To enable GPU computation, the data is all stored in tensor structures, with `Dynamics` and `Statics` Body tensors holding all the rigid body data, etc. -The [world](world) visualization sub-package generates an [xyz](https://cogentcore.org/xyz) 3D scenegraph based on the physics bodies, and updates this visualization efficiently as the physics is updated. +The [world](world) visualization sub-package manages a View element that links to physics bodies and generates an [xyz](https://cogentcore.org/core/xyz) 3D scenegraph based on the physics bodies, and updates this visualization efficiently as the physics is updated. + +## OLD: + +It provides a `Body` node for rigid body physics, along with some basic geometrical shapes thereof. The `physics` scene contains just the bare physics bodies and other elements, which can be updated independent of any visualization. See [virtroom example](examples/virtroom) for an implemented example that shows how to do everything. -# Organizing the World +## Organizing the World It is most efficient to create a relatively deep tree with `Group` nodes that collect nearby `Body` objects at multiple levels of spatial scale. Bounding Boxes are computed at every level of Group, and pruning is done at every level, so large chunks of the tree can be eliminated easily with this strategy. Also, Nodes must be specifically flagged as being `Dynamic` -- otherwise they are assumed to be static -- and each type should be organized into separate top-level Groups (there can be multiple of each, but don't mix Dynamic and Static). Static nodes are never collided against each-other. Ideally, all the Dynamic nodes are in separate top-level, or at least second-to-top level groups -- this eliminates redundant A vs. B and B vs. A collisions and focuses each collision on the most relevant information. -# Updating Modes +## Updating Modes There are two major modes of updating: Scripted or Physics -- scripted requires a program to control what happens on every time step, while physics uses computed forces from contacts, plus joint constraints, to update velocities (not yet supported). The update modes are just about which methods you call. @@ -30,7 +36,7 @@ The `Group` has a set of `World*` methods that should be used on the top-level w * `WorldCollide` -- returns list of potential collision contacts based on projected motion, focusing on dynamic vs. static and dynamic vs. dynamic bodies, with optimized tree filtering. This is the first pass for collision detection. -## Scripted Mode +### Scripted Mode For Scripted mode, each update step typically involves manually updating the `Rel.Pos` and `.Quat` fields on `Body` objects to update their relative positions. This field is a `State` type and has `MoveOnAxis` and `RotateOnAxis` (and a number of other rotation methods). The Move methods update the `LinVel` field to reflect any delta in movement. @@ -40,7 +46,14 @@ For collision detection, it is essential to have the `Abs.LinVel` field set to a It is up to the user to manage the list of potential collisions, e.g., by setting velocity to 0 or bouncing back etc. -## Physics Mode +### Physics Mode + + + +Currently, it provides collision detection and basic forward Euler physics updating, but it does not yet compute any forces for the interactions among the bodies. Ultimately we hope to figure out how the [Bullet](https://github.com/bulletphysics/bullet3) system works and get that running here, in a clean and simple implementation. + +Incrementally, we will start with a basic explicitly driven form of physics that is sufficient to get started, and build from there. + The good news so far is that the full physics version as in Bullet is actually not too bad. The core update step is a super simple forward Euler, intuitive update (just add velocity to position, with a step size factor). The remaining work is just in computing the forces to update those velocities. Bullet uses a hybrid approach that is clearly described in the [Mirtich thesis](https://people.eecs.berkeley.edu/~jfc/mirtich/thesis/mirtichThesis.pdf), which combines *impulses* with a particular way of handling joints, due originally to Featherstone. Impulses are really simple conceptually: when two objects collide, they bounce back off of each other in proportion to their `Bounce` (coefficient of restitution) factor -- these collision impact forces dominate everything else, and aren't that hard to compute (similar conceptually to the `marbles` example in GoGi). The joint constraint stuff is a bit more complicated but not the worst. Everything can be done incrementally. And the resulting system will avoid the brittle nature of the full constraint-based approach taken in ODE, which caused a lot of crashes and instability in `cemer`. diff --git a/physics/body.go b/physics/body.go index d2334121..d43ccf34 100644 --- a/physics/body.go +++ b/physics/body.go @@ -1,38 +1,258 @@ -// Copyright (c) 2019, Cogent Core. All rights reserved. +// Code generated by "goal build"; DO NOT EDIT. +//line body.goal:1 +// Copyright (c) 2025, Cogent Core. All rights reserved. // Use of this source code is governed by a BSD-style // license that can be found in the LICENSE file. package physics -// Body is the common interface for all body types -type Body interface { - Node +import ( + "math" - // AsBodyBase returns the body as a BodyBase - AsBodyBase() *BodyBase + "cogentcore.org/core/math32" +) + +//gosl:start + +// BodyVars are body state variables stored in tensor.Float32 +type BodyVars int32 //enums:enum + +const ( + // Shape is the shape type of the object, as a Shapes type. + Shape BodyVars = iota + + // Size is the size of the object (values depend on shape type). + SizeX + SizeY + SizeZ + + // physical properties + + // Mass is the mass of the object. + Mass + + // Bounce specifies the COR or coefficient of restitution (0..1), + // which determines how elastic the collision is, + // i.e., final velocity / initial velocity. + Bounce + + // Friction coefficient: how much friction is generated by transverse motion. + // Additive across the two surfaces. + Friction + + // 3D position of center of mass. + BodyPosX + BodyPosY + BodyPosZ + + // Quaternion rotation. + BodyRotX + BodyRotY + BodyRotZ + BodyRotW +) + +func BodyShape(idx int32) Shapes { + return Shapes(math.Float32bits(Bodies.Value(int(idx), int(Shape)))) +} + +func SetBodyShape(idx int32, shape Shapes) { + Bodies.Set(math.Float32frombits(uint32(shape)), int(idx), int(Shape)) +} + +func BodySize(idx int32) math32.Vector3 { + var size math32.Vector3 + size.X = Bodies.Value(int(idx), int(SizeX)) + size.Y = Bodies.Value(int(idx), int(SizeY)) + size.Z = Bodies.Value(int(idx), int(SizeZ)) + return size +} + +func SetBodySize(idx int32, size math32.Vector3) { + Bodies.Set(size.X, int(idx), int(SizeX)) + Bodies.Set(size.Y, int(idx), int(SizeY)) + Bodies.Set(size.Z, int(idx), int(SizeZ)) +} + +func BodyPos(idx int32) math32.Vector3 { + var pos math32.Vector3 + pos.X = Bodies.Value(int(idx), int(BodyPosX)) + pos.Y = Bodies.Value(int(idx), int(BodyPosY)) + pos.Z = Bodies.Value(int(idx), int(BodyPosZ)) + return pos +} + +func SetBodyPos(idx int32, pos math32.Vector3) { + Bodies.Set(pos.X, int(idx), int(BodyPosX)) + Bodies.Set(pos.Y, int(idx), int(BodyPosY)) + Bodies.Set(pos.Z, int(idx), int(BodyPosZ)) +} + +func BodyRot(idx int32) math32.Quat { + var rot math32.Quat + rot.X = Bodies.Value(int(idx), int(BodyRotX)) + rot.Y = Bodies.Value(int(idx), int(BodyRotY)) + rot.Z = Bodies.Value(int(idx), int(BodyRotZ)) + rot.W = Bodies.Value(int(idx), int(BodyRotW)) + return rot +} + +func SetBodyRot(idx int32, rot math32.Quat) { + Bodies.Set(rot.X, int(idx), int(BodyRotX)) + Bodies.Set(rot.Y, int(idx), int(BodyRotY)) + Bodies.Set(rot.Z, int(idx), int(BodyRotZ)) + Bodies.Set(rot.W, int(idx), int(BodyRotW)) +} + +// DynamicVars are dynamic body variables stored in tensor.Float32. +type DynamicVars int32 //enums:enum + +const ( + // Index of body in list of bodies. + Index DynamicVars = iota + + // 3D position of center of mass. + PosX + PosY + PosZ + + // Quaternion rotation. + RotX + RotY + RotZ + RotW + + VelX + VelY + VelZ + + // Linear acceleration. + AccX + AccY + AccZ + + // Linear force driving linear acceleration. + ForceX + ForceY + ForceZ + + // Angular velocity. + AngVelX + AngVelY + AngVelZ + + // Angular acceleration due to applied torques. + AngAccX + AngAccY + AngAccZ +) + +func SetDynamicIndex(idx, bodyIdx int32) { + Dynamics.Set(math.Float32frombits(uint32(bodyIdx)), int(idx), int(Index)) +} + +func DynamicIndex(idx int32) int32 { + return int32(math.Float32bits(Dynamics.Value(int(idx), int(Index)))) +} + +func DynamicPos(idx int32) math32.Vector3 { + var pos math32.Vector3 + pos.X = Dynamics.Value(int(idx), int(PosX)) + pos.Y = Dynamics.Value(int(idx), int(PosY)) + pos.Z = Dynamics.Value(int(idx), int(PosZ)) + return pos +} + +func SetDynamicPos(idx int32, pos math32.Vector3) { + Dynamics.Set(pos.X, int(idx), int(PosX)) + Dynamics.Set(pos.Y, int(idx), int(PosY)) + Dynamics.Set(pos.Z, int(idx), int(PosZ)) } -// BodyBase is the base type for all specific Body types -type BodyBase struct { - NodeBase +func DynamicRot(idx int32) math32.Quat { + var rot math32.Quat + rot.X = Dynamics.Value(int(idx), int(RotX)) + rot.Y = Dynamics.Value(int(idx), int(RotY)) + rot.Z = Dynamics.Value(int(idx), int(RotZ)) + rot.W = Dynamics.Value(int(idx), int(RotW)) + return rot +} + +func SetDynamicRot(idx int32, rot math32.Quat) { + Dynamics.Set(rot.X, int(idx), int(RotX)) + Dynamics.Set(rot.Y, int(idx), int(RotY)) + Dynamics.Set(rot.Z, int(idx), int(RotZ)) + Dynamics.Set(rot.W, int(idx), int(RotW)) +} + +func DynamicVel(idx int32) math32.Vector3 { + var vel math32.Vector3 + vel.X = Dynamics.Value(int(idx), int(VelX)) + vel.Y = Dynamics.Value(int(idx), int(VelY)) + vel.Z = Dynamics.Value(int(idx), int(VelZ)) + return vel +} + +func SetDynamicVel(idx int32, vel math32.Vector3) { + Dynamics.Set(vel.X, int(idx), int(VelX)) + Dynamics.Set(vel.Y, int(idx), int(VelY)) + Dynamics.Set(vel.Z, int(idx), int(VelZ)) +} - // rigid body properties, including mass, bounce, friction etc. - Rigid Rigid +func DynamicAcc(idx int32) math32.Vector3 { + var acc math32.Vector3 + acc.X = Dynamics.Value(int(idx), int(AccX)) + acc.Y = Dynamics.Value(int(idx), int(AccY)) + acc.Z = Dynamics.Value(int(idx), int(AccZ)) + return acc +} + +func SetDynamicAcc(idx int32, acc math32.Vector3) { + Dynamics.Set(acc.X, int(idx), int(AccX)) + Dynamics.Set(acc.Y, int(idx), int(AccY)) + Dynamics.Set(acc.Z, int(idx), int(AccZ)) +} - // default color of body. - Color string +func DynamicForce(idx int32) math32.Vector3 { + var force math32.Vector3 + force.X = Dynamics.Value(int(idx), int(ForceX)) + force.Y = Dynamics.Value(int(idx), int(ForceY)) + force.Z = Dynamics.Value(int(idx), int(ForceZ)) + return force } -func (bb *BodyBase) AsBody() Body { - return bb.This.(Body) +func SetDynamicForce(idx int32, force math32.Vector3) { + Dynamics.Set(force.X, int(idx), int(ForceX)) + Dynamics.Set(force.Y, int(idx), int(ForceY)) + Dynamics.Set(force.Z, int(idx), int(ForceZ)) } -func (bb *BodyBase) AsBodyBase() *BodyBase { - return bb +func DynamicAngVel(idx int32) math32.Vector3 { + var angVel math32.Vector3 + angVel.X = Dynamics.Value(int(idx), int(AngVelX)) + angVel.Y = Dynamics.Value(int(idx), int(AngVelY)) + angVel.Z = Dynamics.Value(int(idx), int(AngVelZ)) + return angVel } -func (bb *BodyBase) GroupBBox() {} +func SetDynamicAngVel(idx int32, angVel math32.Vector3) { + Dynamics.Set(angVel.X, int(idx), int(AngVelX)) + Dynamics.Set(angVel.Y, int(idx), int(AngVelY)) + Dynamics.Set(angVel.Z, int(idx), int(AngVelZ)) +} -func (bb *BodyBase) Init() { - // not calling Updater(UpdateFromMake) here -- no children +func DynamicAngAcc(idx int32) math32.Vector3 { + var angAcc math32.Vector3 + angAcc.X = Dynamics.Value(int(idx), int(AngAccX)) + angAcc.Y = Dynamics.Value(int(idx), int(AngAccY)) + angAcc.Z = Dynamics.Value(int(idx), int(AngAccZ)) + return angAcc } + +func SetDynamicAngAcc(idx int32, angAcc math32.Vector3) { + Dynamics.Set(angAcc.X, int(idx), int(AngAccX)) + Dynamics.Set(angAcc.Y, int(idx), int(AngAccY)) + Dynamics.Set(angAcc.Z, int(idx), int(AngAccZ)) +} + +//gosl:end diff --git a/physics/body.goal b/physics/body.goal new file mode 100644 index 00000000..8a2d3197 --- /dev/null +++ b/physics/body.goal @@ -0,0 +1,256 @@ +// Copyright (c) 2025, Cogent Core. All rights reserved. +// Use of this source code is governed by a BSD-style +// license that can be found in the LICENSE file. + +package physics + +import ( + "math" + + "cogentcore.org/core/math32" +) + +//gosl:start + +// BodyVars are body state variables stored in tensor.Float32 +type BodyVars int32 //enums:enum + +const ( + // Shape is the shape type of the object, as a Shapes type. + Shape BodyVars = iota + + // Size is the size of the object (values depend on shape type). + SizeX + SizeY + SizeZ + + // physical properties + + // Mass is the mass of the object. + Mass + + // Bounce specifies the COR or coefficient of restitution (0..1), + // which determines how elastic the collision is, + // i.e., final velocity / initial velocity. + Bounce + + // Friction coefficient: how much friction is generated by transverse motion. + // Additive across the two surfaces. + Friction + + // 3D position of center of mass. + BodyPosX + BodyPosY + BodyPosZ + + // Quaternion rotation. + BodyRotX + BodyRotY + BodyRotZ + BodyRotW +) + +func BodyShape(idx int32) Shapes { + return Shapes(math.Float32bits(Bodies[idx, Shape])) +} + +func SetBodyShape(idx int32, shape Shapes) { + Bodies[idx, Shape] = math.Float32frombits(uint32(shape)) +} + +func BodySize(idx int32) math32.Vector3 { + var size math32.Vector3 + size.X = Bodies[idx, SizeX] + size.Y = Bodies[idx, SizeY] + size.Z = Bodies[idx, SizeZ] + return size +} + +func SetBodySize(idx int32, size math32.Vector3) { + Bodies[idx, SizeX] = size.X + Bodies[idx, SizeY] = size.Y + Bodies[idx, SizeZ] = size.Z +} + +func BodyPos(idx int32) math32.Vector3 { + var pos math32.Vector3 + pos.X = Bodies[idx, BodyPosX] + pos.Y = Bodies[idx, BodyPosY] + pos.Z = Bodies[idx, BodyPosZ] + return pos +} + +func SetBodyPos(idx int32, pos math32.Vector3) { + Bodies[idx, BodyPosX] = pos.X + Bodies[idx, BodyPosY] = pos.Y + Bodies[idx, BodyPosZ] = pos.Z +} + +func BodyRot(idx int32) math32.Quat { + var rot math32.Quat + rot.X = Bodies[idx, BodyRotX] + rot.Y = Bodies[idx, BodyRotY] + rot.Z = Bodies[idx, BodyRotZ] + rot.W = Bodies[idx, BodyRotW] + return rot +} + +func SetBodyRot(idx int32, rot math32.Quat) { + Bodies[idx, BodyRotX] = rot.X + Bodies[idx, BodyRotY] = rot.Y + Bodies[idx, BodyRotZ] = rot.Z + Bodies[idx, BodyRotW] = rot.W +} + +// DynamicVars are dynamic body variables stored in tensor.Float32. +type DynamicVars int32 //enums:enum + +const ( + // Index of body in list of bodies. + Index DynamicVars = iota + + // 3D position of center of mass. + PosX + PosY + PosZ + + // Quaternion rotation. + RotX + RotY + RotZ + RotW + + VelX + VelY + VelZ + + // Linear acceleration. + AccX + AccY + AccZ + + // Linear force driving linear acceleration. + ForceX + ForceY + ForceZ + + // Angular velocity. + AngVelX + AngVelY + AngVelZ + + // Angular acceleration due to applied torques. + AngAccX + AngAccY + AngAccZ +) + +func SetDynamicIndex(idx, bodyIdx int32) { + Dynamics[idx, Index] = math.Float32frombits(uint32(bodyIdx)) +} + +func DynamicIndex(idx int32) int32 { + return int32(math.Float32bits(Dynamics[idx, Index])) +} + +func DynamicPos(idx int32) math32.Vector3 { + var pos math32.Vector3 + pos.X = Dynamics[idx, PosX] + pos.Y = Dynamics[idx, PosY] + pos.Z = Dynamics[idx, PosZ] + return pos +} + +func SetDynamicPos(idx int32, pos math32.Vector3) { + Dynamics[idx, PosX] = pos.X + Dynamics[idx, PosY] = pos.Y + Dynamics[idx, PosZ] = pos.Z +} + +func DynamicRot(idx int32) math32.Quat { + var rot math32.Quat + rot.X = Dynamics[idx, RotX] + rot.Y = Dynamics[idx, RotY] + rot.Z = Dynamics[idx, RotZ] + rot.W = Dynamics[idx, RotW] + return rot +} + +func SetDynamicRot(idx int32, rot math32.Quat) { + Dynamics[idx, RotX] = rot.X + Dynamics[idx, RotY] = rot.Y + Dynamics[idx, RotZ] = rot.Z + Dynamics[idx, RotW] = rot.W +} + +func DynamicVel(idx int32) math32.Vector3 { + var vel math32.Vector3 + vel.X = Dynamics[idx, VelX] + vel.Y = Dynamics[idx, VelY] + vel.Z = Dynamics[idx, VelZ] + return vel +} + +func SetDynamicVel(idx int32, vel math32.Vector3) { + Dynamics[idx, VelX] = vel.X + Dynamics[idx, VelY] = vel.Y + Dynamics[idx, VelZ] = vel.Z +} + +func DynamicAcc(idx int32) math32.Vector3 { + var acc math32.Vector3 + acc.X = Dynamics[idx, AccX] + acc.Y = Dynamics[idx, AccY] + acc.Z = Dynamics[idx, AccZ] + return acc +} + +func SetDynamicAcc(idx int32, acc math32.Vector3) { + Dynamics[idx, AccX] = acc.X + Dynamics[idx, AccY] = acc.Y + Dynamics[idx, AccZ] = acc.Z +} + +func DynamicForce(idx int32) math32.Vector3 { + var force math32.Vector3 + force.X = Dynamics[idx, ForceX] + force.Y = Dynamics[idx, ForceY] + force.Z = Dynamics[idx, ForceZ] + return force +} + +func SetDynamicForce(idx int32, force math32.Vector3) { + Dynamics[idx, ForceX] = force.X + Dynamics[idx, ForceY] = force.Y + Dynamics[idx, ForceZ] = force.Z +} + +func DynamicAngVel(idx int32) math32.Vector3 { + var angVel math32.Vector3 + angVel.X = Dynamics[idx, AngVelX] + angVel.Y = Dynamics[idx, AngVelY] + angVel.Z = Dynamics[idx, AngVelZ] + return angVel +} + +func SetDynamicAngVel(idx int32, angVel math32.Vector3) { + Dynamics[idx, AngVelX] = angVel.X + Dynamics[idx, AngVelY] = angVel.Y + Dynamics[idx, AngVelZ] = angVel.Z +} + +func DynamicAngAcc(idx int32) math32.Vector3 { + var angAcc math32.Vector3 + angAcc.X = Dynamics[idx, AngAccX] + angAcc.Y = Dynamics[idx, AngAccY] + angAcc.Z = Dynamics[idx, AngAccZ] + return angAcc +} + +func SetDynamicAngAcc(idx int32, angAcc math32.Vector3) { + Dynamics[idx, AngAccX] = angAcc.X + Dynamics[idx, AngAccY] = angAcc.Y + Dynamics[idx, AngAccZ] = angAcc.Z +} + +//gosl:end diff --git a/physics/box.go b/physics/box.go deleted file mode 100644 index 3832f518..00000000 --- a/physics/box.go +++ /dev/null @@ -1,40 +0,0 @@ -// Copyright (c) 2019, Cogent Core. All rights reserved. -// Use of this source code is governed by a BSD-style -// license that can be found in the LICENSE file. - -package physics - -import ( - "cogentcore.org/core/math32" -) - -// Box is a box body shape -type Box struct { - BodyBase - - // size of box in each dimension (units arbitrary, as long as they are all consistent -- meters is typical) - Size math32.Vector3 -} - -func (bx *Box) SetBBox() { - bx.BBox.SetBounds(bx.Size.MulScalar(-.5), bx.Size.MulScalar(.5)) - bx.BBox.XForm(bx.Abs.Quat, bx.Abs.Pos) -} - -func (bx *Box) InitAbs(par *NodeBase) { - bx.InitAbsBase(par) - bx.SetBBox() - bx.BBox.VelNilProject() -} - -func (bx *Box) RelToAbs(par *NodeBase) { - bx.RelToAbsBase(par) - bx.SetBBox() - bx.BBox.VelProject(bx.Abs.LinVel, 1) -} - -func (bx *Box) Step(step float32) { - bx.StepBase(step) - bx.SetBBox() - bx.BBox.VelProject(bx.Abs.LinVel, step) -} diff --git a/physics/capsule.go b/physics/capsule.go deleted file mode 100644 index cbd4b372..00000000 --- a/physics/capsule.go +++ /dev/null @@ -1,49 +0,0 @@ -// Copyright (c) 2019, Cogent Core. All rights reserved. -// Use of this source code is governed by a BSD-style -// license that can be found in the LICENSE file. - -package physics - -import ( - "cogentcore.org/core/math32" -) - -// Capsule is a generalized cylinder body shape, with hemispheres at each end, -// with separate radii for top and bottom. -type Capsule struct { - BodyBase - - // height of the cylinder portion of the capsule - Height float32 - - // radius of the top hemisphere - TopRad float32 - - // radius of the bottom hemisphere - BotRad float32 -} - -func (cp *Capsule) SetBBox() { - th := cp.Height + cp.TopRad + cp.BotRad - h2 := th / 2 - cp.BBox.SetBounds(math32.Vec3(-cp.BotRad, -h2, -cp.BotRad), math32.Vec3(cp.TopRad, h2, cp.TopRad)) - cp.BBox.XForm(cp.Abs.Quat, cp.Abs.Pos) -} - -func (cp *Capsule) InitAbs(par *NodeBase) { - cp.InitAbsBase(par) - cp.SetBBox() - cp.BBox.VelNilProject() -} - -func (cp *Capsule) RelToAbs(par *NodeBase) { - cp.RelToAbsBase(par) - cp.SetBBox() - cp.BBox.VelProject(cp.Abs.LinVel, 1) -} - -func (cp *Capsule) Step(step float32) { - cp.StepBase(step) - cp.SetBBox() - cp.BBox.VelProject(cp.Abs.LinVel, step) -} diff --git a/physics/collide.go b/physics/collide.go index ecf271ad..f24944ad 100644 --- a/physics/collide.go +++ b/physics/collide.go @@ -4,39 +4,38 @@ package physics -import ( - "cogentcore.org/core/math32" - "cogentcore.org/core/tree" -) +//gosl:start // Contact is one pairwise point of contact between two bodies. // Contacts are represented in spherical terms relative to the // spherical BBox of A and B. -type Contact struct { +type ContactVars int32 //enums:enum - // one body - A Body +const ( + // first body + ContactA ContactVars = iota // the other body - B Body + ContactB // normal pointing from center of B to center of A - NormB math32.Vector3 + ContactNormX + ContactNormY + ContactNormZ // point on spherical shell of B where A is contacting - PtB math32.Vector3 - - // distance from PtB along NormB to contact point on spherical shell of A - Dist float32 -} + ContactPointX + ContactPointY + ContactPointZ -// UpdateDist updates the distance information for the contact -func (c *Contact) UpdateDist() { + // ContactDist is the distance from PtB along NormB to contact + // point on spherical shell of A. + ContactDist +) -} +//gosl:end -// Contacts is a slice list of contacts -type Contacts []*Contact +/* // New adds a new contact to the list func (cs *Contacts) New(a, b Body) *Contact { @@ -47,7 +46,7 @@ func (cs *Contacts) New(a, b Body) *Contact { // BodyVelBBoxIntersects returns the list of potential contact nodes between a and b // (could be the same or different groups) that have intersecting velocity-projected -// bounding boxes. In general a should be dynamic bodies and b either dynamic or static. +// bounding boxes. In general a should be dynamic bodies and b either dynamic or static. // This is the broad first-pass filtering. func BodyVelBBoxIntersects(a, b Node) Contacts { var cts Contacts @@ -81,3 +80,5 @@ func BodyVelBBoxIntersects(a, b Node) Contacts { }) return cts } + +*/ diff --git a/physics/cylinder.go b/physics/cylinder.go deleted file mode 100644 index 8a76182c..00000000 --- a/physics/cylinder.go +++ /dev/null @@ -1,48 +0,0 @@ -// Copyright (c) 2019, Cogent Core. All rights reserved. -// Use of this source code is governed by a BSD-style -// license that can be found in the LICENSE file. - -package physics - -import ( - "cogentcore.org/core/math32" -) - -// Cylinder is a generalized cylinder body shape, with separate radii for top and bottom. -// A cone has a zero radius at one end. -type Cylinder struct { - BodyBase - - // height of the cylinder - Height float32 - - // radius of the top -- set to 0 for a cone - TopRad float32 - - // radius of the bottom - BotRad float32 -} - -func (cy *Cylinder) SetBBox() { - h2 := cy.Height / 2 - cy.BBox.SetBounds(math32.Vec3(-cy.BotRad, -h2, -cy.BotRad), math32.Vec3(cy.TopRad, h2, cy.TopRad)) - cy.BBox.XForm(cy.Abs.Quat, cy.Abs.Pos) -} - -func (cy *Cylinder) InitAbs(par *NodeBase) { - cy.InitAbsBase(par) - cy.SetBBox() - cy.BBox.VelNilProject() -} - -func (cy *Cylinder) RelToAbs(par *NodeBase) { - cy.RelToAbsBase(par) - cy.SetBBox() - cy.BBox.VelProject(cy.Abs.LinVel, 1) -} - -func (cy *Cylinder) Step(step float32) { - cy.StepBase(step) - cy.SetBBox() - cy.BBox.VelProject(cy.Abs.LinVel, step) -} diff --git a/physics/enumgen.go b/physics/enumgen.go new file mode 100644 index 00000000..baee9b10 --- /dev/null +++ b/physics/enumgen.go @@ -0,0 +1,328 @@ +// Code generated by "core generate -add-types -gosl"; DO NOT EDIT. + +package physics + +import ( + "cogentcore.org/core/enums" +) + +var _BodyVarsValues = []BodyVars{0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13} + +// BodyVarsN is the highest valid value for type BodyVars, plus one. +// +//gosl:start +const BodyVarsN BodyVars = 14 + +//gosl:end + +var _BodyVarsValueMap = map[string]BodyVars{`Shape`: 0, `SizeX`: 1, `SizeY`: 2, `SizeZ`: 3, `Mass`: 4, `Bounce`: 5, `Friction`: 6, `BodyPosX`: 7, `BodyPosY`: 8, `BodyPosZ`: 9, `BodyRotX`: 10, `BodyRotY`: 11, `BodyRotZ`: 12, `BodyRotW`: 13} + +var _BodyVarsDescMap = map[BodyVars]string{0: `Shape is the shape type of the object, as a Shapes type.`, 1: `Size is the size of the object (values depend on shape type).`, 2: ``, 3: ``, 4: `Mass is the mass of the object.`, 5: `Bounce specifies the COR or coefficient of restitution (0..1), which determines how elastic the collision is, i.e., final velocity / initial velocity.`, 6: `Friction coefficient: how much friction is generated by transverse motion. Additive across the two surfaces.`, 7: `3D position of center of mass.`, 8: ``, 9: ``, 10: `Quaternion rotation.`, 11: ``, 12: ``, 13: ``} + +var _BodyVarsMap = map[BodyVars]string{0: `Shape`, 1: `SizeX`, 2: `SizeY`, 3: `SizeZ`, 4: `Mass`, 5: `Bounce`, 6: `Friction`, 7: `BodyPosX`, 8: `BodyPosY`, 9: `BodyPosZ`, 10: `BodyRotX`, 11: `BodyRotY`, 12: `BodyRotZ`, 13: `BodyRotW`} + +// String returns the string representation of this BodyVars value. +func (i BodyVars) String() string { return enums.String(i, _BodyVarsMap) } + +// SetString sets the BodyVars value from its string representation, +// and returns an error if the string is invalid. +func (i *BodyVars) SetString(s string) error { + return enums.SetString(i, s, _BodyVarsValueMap, "BodyVars") +} + +// Int64 returns the BodyVars value as an int64. +func (i BodyVars) Int64() int64 { return int64(i) } + +// SetInt64 sets the BodyVars value from an int64. +func (i *BodyVars) SetInt64(in int64) { *i = BodyVars(in) } + +// Desc returns the description of the BodyVars value. +func (i BodyVars) Desc() string { return enums.Desc(i, _BodyVarsDescMap) } + +// BodyVarsValues returns all possible values for the type BodyVars. +func BodyVarsValues() []BodyVars { return _BodyVarsValues } + +// Values returns all possible values for the type BodyVars. +func (i BodyVars) Values() []enums.Enum { return enums.Values(_BodyVarsValues) } + +// MarshalText implements the [encoding.TextMarshaler] interface. +func (i BodyVars) MarshalText() ([]byte, error) { return []byte(i.String()), nil } + +// UnmarshalText implements the [encoding.TextUnmarshaler] interface. +func (i *BodyVars) UnmarshalText(text []byte) error { return enums.UnmarshalText(i, text, "BodyVars") } + +var _DynamicVarsValues = []DynamicVars{0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22} + +// DynamicVarsN is the highest valid value for type DynamicVars, plus one. +// +//gosl:start +const DynamicVarsN DynamicVars = 23 + +//gosl:end + +var _DynamicVarsValueMap = map[string]DynamicVars{`Index`: 0, `PosX`: 1, `PosY`: 2, `PosZ`: 3, `RotX`: 4, `RotY`: 5, `RotZ`: 6, `RotW`: 7, `VelX`: 8, `VelY`: 9, `VelZ`: 10, `AccX`: 11, `AccY`: 12, `AccZ`: 13, `ForceX`: 14, `ForceY`: 15, `ForceZ`: 16, `AngVelX`: 17, `AngVelY`: 18, `AngVelZ`: 19, `AngAccX`: 20, `AngAccY`: 21, `AngAccZ`: 22} + +var _DynamicVarsDescMap = map[DynamicVars]string{0: `Index of body in list of bodies.`, 1: `3D position of center of mass.`, 2: ``, 3: ``, 4: `Quaternion rotation.`, 5: ``, 6: ``, 7: ``, 8: ``, 9: ``, 10: ``, 11: `Linear acceleration.`, 12: ``, 13: ``, 14: `Linear force driving linear acceleration.`, 15: ``, 16: ``, 17: `Angular velocity.`, 18: ``, 19: ``, 20: `Angular acceleration due to applied torques.`, 21: ``, 22: ``} + +var _DynamicVarsMap = map[DynamicVars]string{0: `Index`, 1: `PosX`, 2: `PosY`, 3: `PosZ`, 4: `RotX`, 5: `RotY`, 6: `RotZ`, 7: `RotW`, 8: `VelX`, 9: `VelY`, 10: `VelZ`, 11: `AccX`, 12: `AccY`, 13: `AccZ`, 14: `ForceX`, 15: `ForceY`, 16: `ForceZ`, 17: `AngVelX`, 18: `AngVelY`, 19: `AngVelZ`, 20: `AngAccX`, 21: `AngAccY`, 22: `AngAccZ`} + +// String returns the string representation of this DynamicVars value. +func (i DynamicVars) String() string { return enums.String(i, _DynamicVarsMap) } + +// SetString sets the DynamicVars value from its string representation, +// and returns an error if the string is invalid. +func (i *DynamicVars) SetString(s string) error { + return enums.SetString(i, s, _DynamicVarsValueMap, "DynamicVars") +} + +// Int64 returns the DynamicVars value as an int64. +func (i DynamicVars) Int64() int64 { return int64(i) } + +// SetInt64 sets the DynamicVars value from an int64. +func (i *DynamicVars) SetInt64(in int64) { *i = DynamicVars(in) } + +// Desc returns the description of the DynamicVars value. +func (i DynamicVars) Desc() string { return enums.Desc(i, _DynamicVarsDescMap) } + +// DynamicVarsValues returns all possible values for the type DynamicVars. +func DynamicVarsValues() []DynamicVars { return _DynamicVarsValues } + +// Values returns all possible values for the type DynamicVars. +func (i DynamicVars) Values() []enums.Enum { return enums.Values(_DynamicVarsValues) } + +// MarshalText implements the [encoding.TextMarshaler] interface. +func (i DynamicVars) MarshalText() ([]byte, error) { return []byte(i.String()), nil } + +// UnmarshalText implements the [encoding.TextUnmarshaler] interface. +func (i *DynamicVars) UnmarshalText(text []byte) error { + return enums.UnmarshalText(i, text, "DynamicVars") +} + +var _ContactVarsValues = []ContactVars{0, 1, 2, 3, 4, 5, 6, 7, 8} + +// ContactVarsN is the highest valid value for type ContactVars, plus one. +// +//gosl:start +const ContactVarsN ContactVars = 9 + +//gosl:end + +var _ContactVarsValueMap = map[string]ContactVars{`ContactA`: 0, `ContactB`: 1, `ContactNormX`: 2, `ContactNormY`: 3, `ContactNormZ`: 4, `ContactPointX`: 5, `ContactPointY`: 6, `ContactPointZ`: 7, `ContactDist`: 8} + +var _ContactVarsDescMap = map[ContactVars]string{0: `first body`, 1: `the other body`, 2: `normal pointing from center of B to center of A`, 3: ``, 4: ``, 5: `point on spherical shell of B where A is contacting`, 6: ``, 7: ``, 8: `ContactDist is the distance from PtB along NormB to contact point on spherical shell of A.`} + +var _ContactVarsMap = map[ContactVars]string{0: `ContactA`, 1: `ContactB`, 2: `ContactNormX`, 3: `ContactNormY`, 4: `ContactNormZ`, 5: `ContactPointX`, 6: `ContactPointY`, 7: `ContactPointZ`, 8: `ContactDist`} + +// String returns the string representation of this ContactVars value. +func (i ContactVars) String() string { return enums.String(i, _ContactVarsMap) } + +// SetString sets the ContactVars value from its string representation, +// and returns an error if the string is invalid. +func (i *ContactVars) SetString(s string) error { + return enums.SetString(i, s, _ContactVarsValueMap, "ContactVars") +} + +// Int64 returns the ContactVars value as an int64. +func (i ContactVars) Int64() int64 { return int64(i) } + +// SetInt64 sets the ContactVars value from an int64. +func (i *ContactVars) SetInt64(in int64) { *i = ContactVars(in) } + +// Desc returns the description of the ContactVars value. +func (i ContactVars) Desc() string { return enums.Desc(i, _ContactVarsDescMap) } + +// ContactVarsValues returns all possible values for the type ContactVars. +func ContactVarsValues() []ContactVars { return _ContactVarsValues } + +// Values returns all possible values for the type ContactVars. +func (i ContactVars) Values() []enums.Enum { return enums.Values(_ContactVarsValues) } + +// MarshalText implements the [encoding.TextMarshaler] interface. +func (i ContactVars) MarshalText() ([]byte, error) { return []byte(i.String()), nil } + +// UnmarshalText implements the [encoding.TextUnmarshaler] interface. +func (i *ContactVars) UnmarshalText(text []byte) error { + return enums.UnmarshalText(i, text, "ContactVars") +} + +var _GPUVarsValues = []GPUVars{0, 1, 2, 3, 4} + +// GPUVarsN is the highest valid value for type GPUVars, plus one. +// +//gosl:start +const GPUVarsN GPUVars = 5 + +//gosl:end + +var _GPUVarsValueMap = map[string]GPUVars{`ParamsVar`: 0, `BodiesVar`: 1, `DynamicsVar`: 2, `JointsVar`: 3, `ContactsVar`: 4} + +var _GPUVarsDescMap = map[GPUVars]string{0: ``, 1: ``, 2: ``, 3: ``, 4: ``} + +var _GPUVarsMap = map[GPUVars]string{0: `ParamsVar`, 1: `BodiesVar`, 2: `DynamicsVar`, 3: `JointsVar`, 4: `ContactsVar`} + +// String returns the string representation of this GPUVars value. +func (i GPUVars) String() string { return enums.String(i, _GPUVarsMap) } + +// SetString sets the GPUVars value from its string representation, +// and returns an error if the string is invalid. +func (i *GPUVars) SetString(s string) error { + return enums.SetString(i, s, _GPUVarsValueMap, "GPUVars") +} + +// Int64 returns the GPUVars value as an int64. +func (i GPUVars) Int64() int64 { return int64(i) } + +// SetInt64 sets the GPUVars value from an int64. +func (i *GPUVars) SetInt64(in int64) { *i = GPUVars(in) } + +// Desc returns the description of the GPUVars value. +func (i GPUVars) Desc() string { return enums.Desc(i, _GPUVarsDescMap) } + +// GPUVarsValues returns all possible values for the type GPUVars. +func GPUVarsValues() []GPUVars { return _GPUVarsValues } + +// Values returns all possible values for the type GPUVars. +func (i GPUVars) Values() []enums.Enum { return enums.Values(_GPUVarsValues) } + +// MarshalText implements the [encoding.TextMarshaler] interface. +func (i GPUVars) MarshalText() ([]byte, error) { return []byte(i.String()), nil } + +// UnmarshalText implements the [encoding.TextUnmarshaler] interface. +func (i *GPUVars) UnmarshalText(text []byte) error { return enums.UnmarshalText(i, text, "GPUVars") } + +var _JointVarsValues = []JointVars{0, 1, 2, 3, 4, 5, 6, 7} + +// JointVarsN is the highest valid value for type JointVars, plus one. +// +//gosl:start +const JointVarsN JointVars = 8 + +//gosl:end + +var _JointVarsValueMap = map[string]JointVars{`JointType`: 0, `JointA`: 1, `JointB`: 2, `JointPosX`: 3, `JointPosY`: 4, `JointPosZ`: 5, `JointParamA`: 6, `JointParamB`: 7} + +var _JointVarsDescMap = map[JointVars]string{0: `JointType (as an int32 from bits).`, 1: `JointA is the dynamic body index for source, anchor, A body.`, 2: `JointB is the dynamic body index for target, follower, B body.`, 3: `position of joint.`, 4: ``, 5: ``, 6: `joint parameters, specific to each joint type.`, 7: ``} + +var _JointVarsMap = map[JointVars]string{0: `JointType`, 1: `JointA`, 2: `JointB`, 3: `JointPosX`, 4: `JointPosY`, 5: `JointPosZ`, 6: `JointParamA`, 7: `JointParamB`} + +// String returns the string representation of this JointVars value. +func (i JointVars) String() string { return enums.String(i, _JointVarsMap) } + +// SetString sets the JointVars value from its string representation, +// and returns an error if the string is invalid. +func (i *JointVars) SetString(s string) error { + return enums.SetString(i, s, _JointVarsValueMap, "JointVars") +} + +// Int64 returns the JointVars value as an int64. +func (i JointVars) Int64() int64 { return int64(i) } + +// SetInt64 sets the JointVars value from an int64. +func (i *JointVars) SetInt64(in int64) { *i = JointVars(in) } + +// Desc returns the description of the JointVars value. +func (i JointVars) Desc() string { return enums.Desc(i, _JointVarsDescMap) } + +// JointVarsValues returns all possible values for the type JointVars. +func JointVarsValues() []JointVars { return _JointVarsValues } + +// Values returns all possible values for the type JointVars. +func (i JointVars) Values() []enums.Enum { return enums.Values(_JointVarsValues) } + +// MarshalText implements the [encoding.TextMarshaler] interface. +func (i JointVars) MarshalText() ([]byte, error) { return []byte(i.String()), nil } + +// UnmarshalText implements the [encoding.TextUnmarshaler] interface. +func (i *JointVars) UnmarshalText(text []byte) error { + return enums.UnmarshalText(i, text, "JointVars") +} + +var _JointTypesValues = []JointTypes{0, 1} + +// JointTypesN is the highest valid value for type JointTypes, plus one. +// +//gosl:start +const JointTypesN JointTypes = 2 + +//gosl:end + +var _JointTypesValueMap = map[string]JointTypes{`Glue`: 0, `Ball`: 1} + +var _JointTypesDescMap = map[JointTypes]string{0: `Glue is a completely rigid joint that supercedes all others, where body A drives motion of body B. JointA is the parent, supporting body, JointB is the child supported.`, 1: ``} + +var _JointTypesMap = map[JointTypes]string{0: `Glue`, 1: `Ball`} + +// String returns the string representation of this JointTypes value. +func (i JointTypes) String() string { return enums.String(i, _JointTypesMap) } + +// SetString sets the JointTypes value from its string representation, +// and returns an error if the string is invalid. +func (i *JointTypes) SetString(s string) error { + return enums.SetString(i, s, _JointTypesValueMap, "JointTypes") +} + +// Int64 returns the JointTypes value as an int64. +func (i JointTypes) Int64() int64 { return int64(i) } + +// SetInt64 sets the JointTypes value from an int64. +func (i *JointTypes) SetInt64(in int64) { *i = JointTypes(in) } + +// Desc returns the description of the JointTypes value. +func (i JointTypes) Desc() string { return enums.Desc(i, _JointTypesDescMap) } + +// JointTypesValues returns all possible values for the type JointTypes. +func JointTypesValues() []JointTypes { return _JointTypesValues } + +// Values returns all possible values for the type JointTypes. +func (i JointTypes) Values() []enums.Enum { return enums.Values(_JointTypesValues) } + +// MarshalText implements the [encoding.TextMarshaler] interface. +func (i JointTypes) MarshalText() ([]byte, error) { return []byte(i.String()), nil } + +// UnmarshalText implements the [encoding.TextUnmarshaler] interface. +func (i *JointTypes) UnmarshalText(text []byte) error { + return enums.UnmarshalText(i, text, "JointTypes") +} + +var _ShapesValues = []Shapes{0, 1, 2, 3} + +// ShapesN is the highest valid value for type Shapes, plus one. +// +//gosl:start +const ShapesN Shapes = 4 + +//gosl:end + +var _ShapesValueMap = map[string]Shapes{`Box`: 0, `Sphere`: 1, `Cylinder`: 2, `Capsule`: 3} + +var _ShapesDescMap = map[Shapes]string{0: `Box is a 3D rectalinear shape.`, 1: `Sphere. SizeX is the radius.`, 2: `Cylinder, natively oriented vertically along the Y axis. If one radius is 0, then it is a cone. SizeX = bottom radius, SizeY = height in Y axis, SizeZ = top radius.`, 3: `Capsule, which is a cylinder with half-spheres on the ends. Natively oriented vertically along the Y axis. SizeX = bottom radius, SizeY = height, SizeZ = top radius.`} + +var _ShapesMap = map[Shapes]string{0: `Box`, 1: `Sphere`, 2: `Cylinder`, 3: `Capsule`} + +// String returns the string representation of this Shapes value. +func (i Shapes) String() string { return enums.String(i, _ShapesMap) } + +// SetString sets the Shapes value from its string representation, +// and returns an error if the string is invalid. +func (i *Shapes) SetString(s string) error { return enums.SetString(i, s, _ShapesValueMap, "Shapes") } + +// Int64 returns the Shapes value as an int64. +func (i Shapes) Int64() int64 { return int64(i) } + +// SetInt64 sets the Shapes value from an int64. +func (i *Shapes) SetInt64(in int64) { *i = Shapes(in) } + +// Desc returns the description of the Shapes value. +func (i Shapes) Desc() string { return enums.Desc(i, _ShapesDescMap) } + +// ShapesValues returns all possible values for the type Shapes. +func ShapesValues() []Shapes { return _ShapesValues } + +// Values returns all possible values for the type Shapes. +func (i Shapes) Values() []enums.Enum { return enums.Values(_ShapesValues) } + +// MarshalText implements the [encoding.TextMarshaler] interface. +func (i Shapes) MarshalText() ([]byte, error) { return []byte(i.String()), nil } + +// UnmarshalText implements the [encoding.TextUnmarshaler] interface. +func (i *Shapes) UnmarshalText(text []byte) error { return enums.UnmarshalText(i, text, "Shapes") } diff --git a/physics/examples/virtroom/virtroom.go b/physics/examples/virtroom/virtroom.go index 03a748b4..c060313f 100644 --- a/physics/examples/virtroom/virtroom.go +++ b/physics/examples/virtroom/virtroom.go @@ -7,9 +7,7 @@ package main //go:generate core generate import ( - "fmt" "image" - "math/rand" "os" "cogentcore.org/core/base/iox/imagex" @@ -82,20 +80,20 @@ type Env struct { //types:add // color map to use for rendering depth map DepthMap core.ColorMapName - // The whole physics World, including visualization. + // The [physics World]. + Physics *physics.World + + // Visualization of the physics world. World *world.World // 3D visualization of the Scene SceneEditor *xyzcore.SceneEditor // emer object - Emer *physics.Group `display:"-"` + Emer *world.View `display:"-"` // Right eye of emer - EyeR physics.Body `display:"-"` - - // contacts from last step, for body - Contacts physics.Contacts `display:"-"` + EyeR *world.View `display:"-"` // snapshot image EyeRImg *core.Image `display:"-"` @@ -117,31 +115,24 @@ func (ev *Env) Defaults() { ev.Camera.FOV = 90 } -// MakePhysicsWorld constructs a new virtual physics world. -func (ev *Env) MakePhysicsWorld() *physics.Group { - pw := physics.NewGroup() - pw.SetName("RoomWorld") - - ev.MakeRoom(pw, "room1", ev.Width, ev.Depth, ev.Height, ev.Thick) - ev.MakeEmer(pw, "emer", ev.EmerHt) - pw.WorldInit() - return pw -} - func (ev *Env) MakeWorld(sc *xyz.Scene) { - pw := ev.MakePhysicsWorld() + ev.Physics = physics.NewWorld() sc.Background = colors.Scheme.Select.Container xyz.NewAmbient(sc, "ambient", 0.3, xyz.DirectSun) dir := xyz.NewDirectional(sc, "dir", 1, xyz.DirectSun) dir.Pos.Set(0, 2, 1) // default: 0,1,1 = above and behind us (we are at 0,0,X) - ev.World = world.NewWorld(pw, sc) + ev.World = world.NewWorld(sc) + ev.MakeRoom("room1", ev.Width, ev.Depth, ev.Height, ev.Thick) + ev.MakeEmer("emer", ev.EmerHt) + ev.World.Init(ev.Physics) + ev.World.Update() } // InitWorld does init on world. func (ev *Env) WorldInit() { //types:add - ev.World.Init() + // ev.World.Init() } // ConfigView3D makes the 3D view @@ -186,28 +177,28 @@ func (ev *Env) UpdateView() { // WorldStep does one step of the world func (ev *Env) WorldStep() { - pw := ev.World.World - pw.Update() // only need to call if there are updaters added to world - pw.WorldRelToAbs() - cts := pw.WorldCollide(physics.DynsTopGps) - ev.Contacts = nil - for _, cl := range cts { - if len(cl) > 1 { - for _, c := range cl { - if c.A.AsTree().Name == "body" { - ev.Contacts = cl - } - fmt.Printf("A: %v B: %v\n", c.A.AsTree().Name, c.B.AsTree().Name) - } - } - } + // pw := ev.Physics + // pw.Update() // only need to call if there are updaters added to world + // pw.WorldRelToAbs() + // cts := pw.WorldCollide(physics.DynsTopGps) + // ev.Contacts = nil + // for _, cl := range cts { + // if len(cl) > 1 { + // for _, c := range cl { + // if c.A.AsTree().Name == "body" { + // ev.Contacts = cl + // } + // fmt.Printf("A: %v B: %v\n", c.A.AsTree().Name, c.B.AsTree().Name) + // } + // } + // } ev.EmerAngry = false - if len(ev.Contacts) > 1 { // turn around - ev.EmerAngry = true - fmt.Printf("hit wall: turn around!\n") - rot := 100.0 + 90.0*rand.Float32() - ev.Emer.Rel.RotateOnAxis(0, 1, 0, rot) - } + // if len(ev.Contacts) > 1 { // turn around + // ev.EmerAngry = true + // fmt.Printf("hit wall: turn around!\n") + // rot := 100.0 + 90.0*rand.Float32() + // ev.Emer.Rel.RotateOnAxis(0, 1, 0, rot) + // } ev.World.Update() ev.GrabEyeImg() ev.UpdateView() @@ -215,121 +206,94 @@ func (ev *Env) WorldStep() { // StepForward moves Emer forward in current facing direction one step, and takes GrabEyeImg func (ev *Env) StepForward() { //types:add - ev.Emer.Rel.MoveOnAxis(0, 0, 1, -ev.MoveStep) + // ev.Emer.Rel.MoveOnAxis(0, 0, 1, -ev.MoveStep) ev.WorldStep() } // StepBackward moves Emer backward in current facing direction one step, and takes GrabEyeImg func (ev *Env) StepBackward() { //types:add - ev.Emer.Rel.MoveOnAxis(0, 0, 1, ev.MoveStep) + // ev.Emer.Rel.MoveOnAxis(0, 0, 1, ev.MoveStep) ev.WorldStep() } // RotBodyLeft rotates emer left and takes GrabEyeImg func (ev *Env) RotBodyLeft() { //types:add - ev.Emer.Rel.RotateOnAxis(0, 1, 0, ev.RotStep) + // ev.Emer.Rel.RotateOnAxis(0, 1, 0, ev.RotStep) ev.WorldStep() } // RotBodyRight rotates emer right and takes GrabEyeImg func (ev *Env) RotBodyRight() { //types:add - ev.Emer.Rel.RotateOnAxis(0, 1, 0, -ev.RotStep) + // ev.Emer.Rel.RotateOnAxis(0, 1, 0, -ev.RotStep) ev.WorldStep() } // RotHeadLeft rotates emer left and takes GrabEyeImg func (ev *Env) RotHeadLeft() { //types:add - hd := ev.Emer.ChildByName("head", 1).(*physics.Group) - hd.Rel.RotateOnAxis(0, 1, 0, ev.RotStep) + // hd := ev.Emer.ChildByName("head", 1).(*physics.Group) + // hd.Rel.RotateOnAxis(0, 1, 0, ev.RotStep) ev.WorldStep() } // RotHeadRight rotates emer right and takes GrabEyeImg func (ev *Env) RotHeadRight() { //types:add - hd := ev.Emer.ChildByName("head", 1).(*physics.Group) - hd.Rel.RotateOnAxis(0, 1, 0, -ev.RotStep) + // hd := ev.Emer.ChildByName("head", 1).(*physics.Group) + // hd.Rel.RotateOnAxis(0, 1, 0, -ev.RotStep) ev.WorldStep() } -// MakeRoom constructs a new room in given parent group with given params -func (ev *Env) MakeRoom(par *physics.Group, name string, width, depth, height, thick float32) { - tree.AddChildAt(par, name, func(rm *physics.Group) { - rm.Maker(func(p *tree.Plan) { - tree.AddAt(p, "floor", func(n *physics.Box) { - n.SetSize(math32.Vec3(width, thick, depth)). - SetColor("grey").SetInitPos(math32.Vec3(0, -thick/2, 0)) - }) - tree.AddAt(p, "back-wall", func(n *physics.Box) { - n.SetSize(math32.Vec3(width, height, thick)). - SetColor("blue").SetInitPos(math32.Vec3(0, height/2, -depth/2)) - }) - tree.AddAt(p, "left-wall", func(n *physics.Box) { - n.SetSize(math32.Vec3(thick, height, depth)). - SetColor("red").SetInitPos(math32.Vec3(-width/2, height/2, 0)) - }) - tree.AddAt(p, "right-wall", func(n *physics.Box) { - n.SetSize(math32.Vec3(thick, height, depth)). - SetColor("green").SetInitPos(math32.Vec3(width/2, height/2, 0)) - }) - tree.AddAt(p, "front-wall", func(n *physics.Box) { - n.SetSize(math32.Vec3(width, height, thick)). - SetColor("yellow").SetInitPos(math32.Vec3(0, height/2, depth/2)) - }) - }) - }) +// MakeRoom constructs a new room with given params +func (ev *Env) MakeRoom(name string, width, depth, height, thick float32) { + wr := ev.World + wl := ev.Physics + rot := math32.NewQuat(0, 0, 0, 1) + wr.NewBody(wl, name+"_floor", physics.Box, "grey", math32.Vec3(width, thick, depth), + math32.Vec3(0, -thick/2, 0), rot) + wr.NewBody(wl, name+"_back-wall", physics.Box, "blue", math32.Vec3(width, height, thick), + math32.Vec3(0, height/2, -depth/2), rot) + wr.NewBody(wl, name+"_left-wall", physics.Box, "red", math32.Vec3(thick, height, depth), + math32.Vec3(-width/2, height/2, 0), rot) + wr.NewBody(wl, name+"_right-wall", physics.Box, "green", math32.Vec3(thick, height, depth), + math32.Vec3(width/2, height/2, 0), rot) + wr.NewBody(wl, name+"_front-wall", physics.Box, "yellow", math32.Vec3(width, height, thick), + math32.Vec3(0, height/2, depth/2), rot) } // MakeEmer constructs a new Emer virtual robot of given height (e.g., 1). -func (ev *Env) MakeEmer(par *physics.Group, name string, height float32) { - tree.AddChildAt(par, name, func(emr *physics.Group) { - ev.Emer = emr - emr.Maker(func(p *tree.Plan) { - width := height * .4 - depth := height * .15 - tree.AddAt(p, "body", func(n *physics.Box) { - n.SetSize(math32.Vec3(width, height, depth)). - SetColor("purple").SetDynamic(true). - SetInitPos(math32.Vec3(0, height/2, 0)) - }) - // body := physics.NewCapsule(emr, "body", math32.Vec3(0, height / 2, 0), height, width/2) - // body := physics.NewCylinder(emr, "body", math32.Vec3(0, height / 2, 0), height, width/2) - - headsz := depth * 1.5 - eyesz := headsz * .2 - hhsz := .5 * headsz - tree.AddAt(p, "head", func(n *physics.Group) { - n.SetInitPos(math32.Vec3(0, height+hhsz, 0)) - n.Maker(func(p *tree.Plan) { - tree.AddAt(p, "head", func(n *physics.Box) { - n.SetSize(math32.Vec3(headsz, headsz, headsz)). - SetColor("tan").SetDynamic(true).SetInitPos(math32.Vec3(0, 0, 0)) - n.InitView = func(vn tree.Node) { - sld := vn.(*xyz.Solid) - world.BoxInit(n, sld) - sld.Updater(func() { - clr := n.Color - if ev.EmerAngry { - clr = "pink" - } - world.UpdateColor(clr, n.View.(*xyz.Solid)) - }) - } - }) - tree.AddAt(p, "eye-l", func(n *physics.Box) { - n.SetSize(math32.Vec3(eyesz, eyesz*.5, eyesz*.2)). - SetColor("green").SetDynamic(true). - SetInitPos(math32.Vec3(-hhsz*.6, headsz*.1, -(hhsz + eyesz*.3))) - }) - tree.AddAt(p, "eye-r", func(n *physics.Box) { - ev.EyeR = n - n.SetSize(math32.Vec3(eyesz, eyesz*.5, eyesz*.2)). - SetColor("green").SetDynamic(true). - SetInitPos(math32.Vec3(hhsz*.6, headsz*.1, -(hhsz + eyesz*.3))) - }) - }) - }) +func (ev *Env) MakeEmer(name string, height float32) { + wr := ev.World + wl := ev.Physics + width := height * .4 + depth := height * .15 + headsz := depth * 1.5 + eyesz := headsz * .2 + hhsz := .5 * headsz + rot := math32.NewQuat(0, 0, 0, 1) + ev.Emer = wr.NewBody(wl, name+"_body", physics.Box, "purple", math32.Vec3(width, height, depth), + math32.Vec3(0, height/2, 0), rot) + // body := physics.NewCapsule(emr, "body", math32.Vec3(0, height / 2, 0), height, width/2) + // body := physics.NewCylinder(emr, "body", math32.Vec3(0, height / 2, 0), height, width/2) + + headPos := math32.Vec3(0, height+hhsz, 0) + vw := wr.NewBody(wl, name+"_head", physics.Box, "tan", math32.Vec3(headsz, headsz, headsz), + headPos, rot) + vw.InitView = func(sld *xyz.Solid) { + vw.BoxInit(sld) + sld.Updater(func() { + clr := vw.Color + if ev.EmerAngry { + clr = "pink" + } + vw.UpdateColor(clr, sld) }) - }) + } + wl.NewJoint(physics.Glue, ev.Emer.Index, vw.Index, vw.Pos) + vw = wr.NewBody(wl, name+"_eye-l", physics.Box, "green", math32.Vec3(eyesz, eyesz*.5, eyesz*.2), + headPos.Add(math32.Vec3(-hhsz*.6, headsz*.1, -(hhsz+eyesz*.3))), rot) + wl.NewJoint(physics.Glue, ev.Emer.Index, vw.Index, vw.Pos) + ev.EyeR = wr.NewBody(wl, name+"_eye-r", physics.Box, "green", math32.Vec3(eyesz, eyesz*.5, eyesz*.2), + headPos.Add(math32.Vec3(hhsz*.6, headsz*.1, -(hhsz+eyesz*.3))), rot) + wl.NewJoint(physics.Glue, ev.Emer.Index, ev.EyeR.Index, ev.EyeR.Pos) } func (ev *Env) ConfigGUI() *core.Body { @@ -355,17 +319,17 @@ func (ev *Env) ConfigGUI() *core.Body { //////// 3D Scene etb := core.NewToolbar(scfr) + _ = etb ev.SceneEditor = xyzcore.NewSceneEditor(scfr) ev.SceneEditor.UpdateWidget() sc := ev.SceneEditor.SceneXYZ() ev.MakeWorld(sc) - tv.SyncTree(ev.World.World) // local toolbar for manipulating emer - etb.Maker(world.MakeStateToolbar(&ev.Emer.Rel, func() { - ev.World.Update() - ev.SceneEditor.NeedsRender() - })) + // etb.Maker(world.MakeStateToolbar(&ev.Emer.Rel, func() { + // ev.World.Update() + // ev.SceneEditor.NeedsRender() + // })) sc.Camera.Pose.Pos = math32.Vec3(0, 40, 3.5) sc.Camera.LookAt(math32.Vec3(0, 5, 0), math32.Vec3(0, 1, 0)) diff --git a/physics/gosl.go b/physics/gosl.go new file mode 100644 index 00000000..67e2f98a --- /dev/null +++ b/physics/gosl.go @@ -0,0 +1,392 @@ +// Code generated by "gosl"; DO NOT EDIT + +package physics + +import ( + "embed" + "fmt" + "math" + "unsafe" + "cogentcore.org/core/gpu" + "cogentcore.org/lab/tensor" +) + +//go:embed shaders/*.wgsl +var shaders embed.FS + +var ( + // GPUInitialized is true once the GPU system has been initialized. + // Prevents multiple initializations. + GPUInitialized bool + + // ComputeGPU is the compute gpu device. + // Set this prior to calling GPUInit() to use an existing device. + ComputeGPU *gpu.GPU + + // BorrowedGPU is true if our ComputeGPU is set externally, + // versus created specifically for this system. If external, + // we don't release it. + BorrowedGPU bool + + // UseGPU indicates whether to use GPU vs. CPU. + UseGPU bool +) +// GPUSystem is a GPU compute System with kernels operating on the +// same set of data variables. +var GPUSystem *gpu.ComputeSystem + +// GPUVars is an enum for GPU variables, for specifying what to sync. +type GPUVars int32 //enums:enum + +const ( + ParamsVar GPUVars = 0 + BodiesVar GPUVars = 1 + DynamicsVar GPUVars = 2 + JointsVar GPUVars = 3 + ContactsVar GPUVars = 4 +) + +// Tensor stride variables +var TensorStrides tensor.Uint32 + +// GPUInit initializes the GPU compute system, +// configuring system(s), variables and kernels. +// It is safe to call multiple times: detects if already run. +func GPUInit() { + if GPUInitialized { + return + } + GPUInitialized = true + if ComputeGPU == nil { // set prior to this call to use an external + ComputeGPU = gpu.NewComputeGPU() + } else { + BorrowedGPU = true + } + gp := ComputeGPU + + _ = fmt.Sprintf("%g",math.NaN()) // keep imports happy + { + sy := gpu.NewComputeSystem(gp, "Default") + GPUSystem = sy + vars := sy.Vars() + { + sgp := vars.AddGroup(gpu.Storage, "Params") + var vr *gpu.Var + _ = vr + vr = sgp.Add("TensorStrides", gpu.Uint32, 1, gpu.ComputeShader) + vr.ReadOnly = true + vr = sgp.AddStruct("Params", int(unsafe.Sizeof(PhysParams{})), 1, gpu.ComputeShader) + vr.ReadOnly = true + sgp.SetNValues(1) + } + { + sgp := vars.AddGroup(gpu.Storage, "Bodies") + var vr *gpu.Var + _ = vr + vr = sgp.Add("Bodies", gpu.Float32, 1, gpu.ComputeShader) + vr = sgp.Add("Dynamics", gpu.Float32, 1, gpu.ComputeShader) + vr = sgp.Add("Joints", gpu.Float32, 1, gpu.ComputeShader) + sgp.SetNValues(1) + } + { + sgp := vars.AddGroup(gpu.Storage, "Contacts") + var vr *gpu.Var + _ = vr + vr = sgp.Add("Contacts", gpu.Float32, 1, gpu.ComputeShader) + sgp.SetNValues(1) + } + var pl *gpu.ComputePipeline + pl = gpu.NewComputePipelineShaderFS(shaders, "shaders/InitDynamics.wgsl", sy) + pl.AddVarUsed(0, "TensorStrides") + pl.AddVarUsed(1, "Bodies") + pl.AddVarUsed(1, "Dynamics") + pl.AddVarUsed(0, "Params") + pl = gpu.NewComputePipelineShaderFS(shaders, "shaders/Step.wgsl", sy) + pl.AddVarUsed(0, "TensorStrides") + pl.AddVarUsed(1, "Dynamics") + pl.AddVarUsed(0, "Params") + pl = gpu.NewComputePipelineShaderFS(shaders, "shaders/StepJoints.wgsl", sy) + pl.AddVarUsed(0, "TensorStrides") + pl.AddVarUsed(1, "Dynamics") + pl.AddVarUsed(1, "Joints") + pl.AddVarUsed(0, "Params") + sy.Config() + } +} + +// GPURelease releases the GPU compute system resources. +// Call this at program exit. +func GPURelease() { + if GPUSystem != nil { + GPUSystem.Release() + GPUSystem = nil + } + + if !BorrowedGPU && ComputeGPU != nil { + ComputeGPU.Release() + + } + ComputeGPU = nil +} + +// RunInitDynamics runs the InitDynamics kernel with given number of elements, +// on either the CPU or GPU depending on the UseGPU variable. +// Can call multiple Run* kernels in a row, which are then all launched +// in the same command submission on the GPU, which is by far the most efficient. +// MUST call RunDone (with optional vars to sync) after all Run calls. +// Alternatively, a single-shot RunOneInitDynamics call does Run and Done for a +// single run-and-sync case. +func RunInitDynamics(n int) { + if UseGPU { + RunInitDynamicsGPU(n) + } else { + RunInitDynamicsCPU(n) + } +} + +// RunInitDynamicsGPU runs the InitDynamics kernel on the GPU. See [RunInitDynamics] for more info. +func RunInitDynamicsGPU(n int) { + sy := GPUSystem + pl := sy.ComputePipelines["InitDynamics"] + ce, _ := sy.BeginComputePass() + pl.Dispatch1D(ce, n, 64) +} + +// RunInitDynamicsCPU runs the InitDynamics kernel on the CPU. +func RunInitDynamicsCPU(n int) { + gpu.VectorizeFunc(0, n, InitDynamics) +} + +// RunOneInitDynamics runs the InitDynamics kernel with given number of elements, +// on either the CPU or GPU depending on the UseGPU variable. +// This version then calls RunDone with the given variables to sync +// after the Run, for a single-shot Run-and-Done call. If multiple kernels +// can be run in sequence, it is much more efficient to do multiple Run* +// calls followed by a RunDone call. +func RunOneInitDynamics(n int, syncVars ...GPUVars) { + if UseGPU { + RunInitDynamicsGPU(n) + RunDone(syncVars...) + } else { + RunInitDynamicsCPU(n) + } +} +// RunStep runs the Step kernel with given number of elements, +// on either the CPU or GPU depending on the UseGPU variable. +// Can call multiple Run* kernels in a row, which are then all launched +// in the same command submission on the GPU, which is by far the most efficient. +// MUST call RunDone (with optional vars to sync) after all Run calls. +// Alternatively, a single-shot RunOneStep call does Run and Done for a +// single run-and-sync case. +func RunStep(n int) { + if UseGPU { + RunStepGPU(n) + } else { + RunStepCPU(n) + } +} + +// RunStepGPU runs the Step kernel on the GPU. See [RunStep] for more info. +func RunStepGPU(n int) { + sy := GPUSystem + pl := sy.ComputePipelines["Step"] + ce, _ := sy.BeginComputePass() + pl.Dispatch1D(ce, n, 64) +} + +// RunStepCPU runs the Step kernel on the CPU. +func RunStepCPU(n int) { + gpu.VectorizeFunc(0, n, Step) +} + +// RunOneStep runs the Step kernel with given number of elements, +// on either the CPU or GPU depending on the UseGPU variable. +// This version then calls RunDone with the given variables to sync +// after the Run, for a single-shot Run-and-Done call. If multiple kernels +// can be run in sequence, it is much more efficient to do multiple Run* +// calls followed by a RunDone call. +func RunOneStep(n int, syncVars ...GPUVars) { + if UseGPU { + RunStepGPU(n) + RunDone(syncVars...) + } else { + RunStepCPU(n) + } +} +// RunStepJoints runs the StepJoints kernel with given number of elements, +// on either the CPU or GPU depending on the UseGPU variable. +// Can call multiple Run* kernels in a row, which are then all launched +// in the same command submission on the GPU, which is by far the most efficient. +// MUST call RunDone (with optional vars to sync) after all Run calls. +// Alternatively, a single-shot RunOneStepJoints call does Run and Done for a +// single run-and-sync case. +func RunStepJoints(n int) { + if UseGPU { + RunStepJointsGPU(n) + } else { + RunStepJointsCPU(n) + } +} + +// RunStepJointsGPU runs the StepJoints kernel on the GPU. See [RunStepJoints] for more info. +func RunStepJointsGPU(n int) { + sy := GPUSystem + pl := sy.ComputePipelines["StepJoints"] + ce, _ := sy.BeginComputePass() + pl.Dispatch1D(ce, n, 64) +} + +// RunStepJointsCPU runs the StepJoints kernel on the CPU. +func RunStepJointsCPU(n int) { + gpu.VectorizeFunc(0, n, StepJoints) +} + +// RunOneStepJoints runs the StepJoints kernel with given number of elements, +// on either the CPU or GPU depending on the UseGPU variable. +// This version then calls RunDone with the given variables to sync +// after the Run, for a single-shot Run-and-Done call. If multiple kernels +// can be run in sequence, it is much more efficient to do multiple Run* +// calls followed by a RunDone call. +func RunOneStepJoints(n int, syncVars ...GPUVars) { + if UseGPU { + RunStepJointsGPU(n) + RunDone(syncVars...) + } else { + RunStepJointsCPU(n) + } +} +// RunDone must be called after Run* calls to start compute kernels. +// This actually submits the kernel jobs to the GPU, and adds commands +// to synchronize the given variables back from the GPU to the CPU. +// After this function completes, the GPU results will be available in +// the specified variables. +func RunDone(syncVars ...GPUVars) { + if !UseGPU { + return + } + sy := GPUSystem + sy.ComputeEncoder.End() + ReadFromGPU(syncVars...) + sy.EndComputePass() + SyncFromGPU(syncVars...) +} + +// ToGPU copies given variables to the GPU for the system. +func ToGPU(vars ...GPUVars) { + if !UseGPU { + return + } + sy := GPUSystem + syVars := sy.Vars() + for _, vr := range vars { + switch vr { + case ParamsVar: + v, _ := syVars.ValueByIndex(0, "Params", 0) + gpu.SetValueFrom(v, Params) + case BodiesVar: + v, _ := syVars.ValueByIndex(1, "Bodies", 0) + gpu.SetValueFrom(v, Bodies.Values) + case DynamicsVar: + v, _ := syVars.ValueByIndex(1, "Dynamics", 0) + gpu.SetValueFrom(v, Dynamics.Values) + case JointsVar: + v, _ := syVars.ValueByIndex(1, "Joints", 0) + gpu.SetValueFrom(v, Joints.Values) + case ContactsVar: + v, _ := syVars.ValueByIndex(2, "Contacts", 0) + gpu.SetValueFrom(v, Contacts.Values) + } + } +} +// RunGPUSync can be called to synchronize data between CPU and GPU. +// Any prior ToGPU* calls will execute to send data to the GPU, +// and any subsequent RunDone* calls will copy data back from the GPU. +func RunGPUSync() { + if !UseGPU { + return + } + sy := GPUSystem + sy.BeginComputePass() +} + +// ToGPUTensorStrides gets tensor strides and starts copying to the GPU. +func ToGPUTensorStrides() { + if !UseGPU { + return + } + sy := GPUSystem + syVars := sy.Vars() + TensorStrides.SetShapeSizes(40) + TensorStrides.SetInt1D(Bodies.Shape().Strides[0], 0) + TensorStrides.SetInt1D(Bodies.Shape().Strides[1], 1) + TensorStrides.SetInt1D(Dynamics.Shape().Strides[0], 10) + TensorStrides.SetInt1D(Dynamics.Shape().Strides[1], 11) + TensorStrides.SetInt1D(Joints.Shape().Strides[0], 20) + TensorStrides.SetInt1D(Joints.Shape().Strides[1], 21) + TensorStrides.SetInt1D(Contacts.Shape().Strides[0], 30) + TensorStrides.SetInt1D(Contacts.Shape().Strides[1], 31) + v, _ := syVars.ValueByIndex(0, "TensorStrides", 0) + gpu.SetValueFrom(v, TensorStrides.Values) +} + +// ReadFromGPU starts the process of copying vars to the GPU. +func ReadFromGPU(vars ...GPUVars) { + sy := GPUSystem + syVars := sy.Vars() + for _, vr := range vars { + switch vr { + case ParamsVar: + v, _ := syVars.ValueByIndex(0, "Params", 0) + v.GPUToRead(sy.CommandEncoder) + case BodiesVar: + v, _ := syVars.ValueByIndex(1, "Bodies", 0) + v.GPUToRead(sy.CommandEncoder) + case DynamicsVar: + v, _ := syVars.ValueByIndex(1, "Dynamics", 0) + v.GPUToRead(sy.CommandEncoder) + case JointsVar: + v, _ := syVars.ValueByIndex(1, "Joints", 0) + v.GPUToRead(sy.CommandEncoder) + case ContactsVar: + v, _ := syVars.ValueByIndex(2, "Contacts", 0) + v.GPUToRead(sy.CommandEncoder) + } + } +} + +// SyncFromGPU synchronizes vars from the GPU to the actual variable. +func SyncFromGPU(vars ...GPUVars) { + sy := GPUSystem + syVars := sy.Vars() + for _, vr := range vars { + switch vr { + case ParamsVar: + v, _ := syVars.ValueByIndex(0, "Params", 0) + v.ReadSync() + gpu.ReadToBytes(v, Params) + case BodiesVar: + v, _ := syVars.ValueByIndex(1, "Bodies", 0) + v.ReadSync() + gpu.ReadToBytes(v, Bodies.Values) + case DynamicsVar: + v, _ := syVars.ValueByIndex(1, "Dynamics", 0) + v.ReadSync() + gpu.ReadToBytes(v, Dynamics.Values) + case JointsVar: + v, _ := syVars.ValueByIndex(1, "Joints", 0) + v.ReadSync() + gpu.ReadToBytes(v, Joints.Values) + case ContactsVar: + v, _ := syVars.ValueByIndex(2, "Contacts", 0) + v.ReadSync() + gpu.ReadToBytes(v, Contacts.Values) + } + } +} + +// GetParams returns a pointer to the given global variable: +// [Params] []PhysParams at given index. This directly processed in the GPU code, +// so this function call is an equivalent for the CPU. +func GetParams(idx uint32) *PhysParams { + return &Params[idx] +} diff --git a/physics/group.go b/physics/group.go index bc1791d0..11a2df13 100644 --- a/physics/group.go +++ b/physics/group.go @@ -4,248 +4,241 @@ package physics -import ( - "sort" - - "cogentcore.org/core/math32" - "cogentcore.org/core/tree" -) - -// Group is a container of bodies, joints, or other groups -// it should be used strategically to partition the space +// Group is a container of bodies or other groups. +// It should be used strategically to partition the space // and its BBox is used to optimize tree-based collision detection. -// Use a group for the top-level World node as well. -type Group struct { - NodeBase -} - -func (gp *Group) Update() { - gp.RunUpdaters() - gp.WalkDown(func(n tree.Node) bool { - if n.AsTree().This == gp.This { - return tree.Continue - } - n.(Node).Update() - return tree.Continue - }) -} - -func (gp *Group) InitAbs(par *NodeBase) { - gp.InitAbsBase(par) -} - -func (gp *Group) RelToAbs(par *NodeBase) { - gp.RelToAbsBase(par) // yes we can move groups -} - -func (gp *Group) Step(step float32) { - // groups do NOT update physics -} - -func (gp *Group) GroupBBox() { - hasDyn := false - gp.BBox.BBox.SetEmpty() - gp.BBox.VelBBox.SetEmpty() - for _, kid := range gp.Children { - n, nb := AsNode(kid) - if n == nil { - continue - } - gp.BBox.BBox.ExpandByBox(nb.BBox.BBox) - gp.BBox.VelBBox.ExpandByBox(nb.BBox.VelBBox) - if nb.Dynamic { - hasDyn = true - } - } - gp.SetDynamic(hasDyn) -} - -// WorldDynGroupBBox does a GroupBBox on all dynamic nodes -func (gp *Group) WorldDynGroupBBox() { - gp.WalkDownPost(func(tn tree.Node) bool { - n, nb := AsNode(tn) - if n == nil { - return false - } - if !nb.Dynamic { - return false - } - return true - }, func(tn tree.Node) bool { - n, nb := AsNode(tn) - if n == nil { - return false - } - if !nb.Dynamic { - return false - } - n.GroupBBox() - return true - }) -} - -// WorldInit does the full tree InitAbs and GroupBBox updates -func (gp *Group) WorldInit() { - gp.Update() - gp.WalkDown(func(tn tree.Node) bool { - n, _ := AsNode(tn) - if n == nil { - return false - } - _, pi := AsNode(tn.AsTree().Parent) - n.InitAbs(pi) - return true - }) - - gp.WalkDownPost(func(tn tree.Node) bool { - n, _ := AsNode(tn) - if n == nil { - return false - } - return true - }, func(tn tree.Node) bool { - n, _ := AsNode(tn) - if n == nil { - return false - } - n.GroupBBox() - return true - }) - -} - -// WorldRelToAbs does a full RelToAbs update for all Dynamic groups, for -// Scripted mode updates with manual updating of Rel values. -func (gp *Group) WorldRelToAbs() { - gp.WalkDown(func(tn tree.Node) bool { - n, nb := AsNode(tn) - if n == nil { - return false // going into a different type of thing, bail - } - if !nb.Dynamic { - return false - } - _, pi := AsNode(tn.AsTree().Parent) - n.RelToAbs(pi) - return true - }) - - gp.WorldDynGroupBBox() -} - -// WorldStep does a full Step update for all Dynamic nodes, for -// either physics or scripted mode, based on current velocities. -func (gp *Group) WorldStep(step float32) { - gp.WalkDown(func(tn tree.Node) bool { - n, nb := AsNode(tn) - if n == nil { - return false // going into a different type of thing, bail - } - if !nb.Dynamic { - return false - } - n.Step(step) - return true - }) - - gp.WorldDynGroupBBox() -} - -const ( - // DynsTopGps is passed to WorldCollide when all dynamic objects are in separate top groups - DynsTopGps = true - - // DynsSubGps is passed to WorldCollide when all dynamic objects are in separate groups under top - // level (i.e., one level deeper) - DynsSubGps -) - -// WorldCollide does first pass filtering step of collision detection -// based on separate dynamic vs. dynamic and dynamic vs. static groups. -// If dynTop is true, then each Dynamic group is separate at the top level -- -// otherwise they are organized at the next group level. -// Contacts are organized by dynamic group, when non-nil, for easier -// processing. -func (gp *Group) WorldCollide(dynTop bool) []Contacts { - var stats []Node - var dyns []Node - for _, kid := range gp.Children { - n, nb := AsNode(kid) - if n == nil { - continue - } - if nb.Dynamic { - dyns = append(dyns, n) - } else { - stats = append(stats, n) - } - } - - var sdyns []Node - if !dynTop { - for _, d := range dyns { - for _, dk := range d.AsTree().Children { - nii, _ := AsNode(dk) - if nii == nil { - continue - } - sdyns = append(sdyns, nii) - } - } - dyns = sdyns - } - - var cts []Contacts - for i, d := range dyns { - var dct Contacts - for _, s := range stats { - cc := BodyVelBBoxIntersects(d, s) - dct = append(dct, cc...) - } - for di := 0; di < i; di++ { - od := dyns[di] - cc := BodyVelBBoxIntersects(d, od) - dct = append(dct, cc...) - } - if len(dct) > 0 { - cts = append(cts, dct) - } - } - return cts -} - -// BodyPoint contains a Body and a Point on that body -type BodyPoint struct { - Body Body - Point math32.Vector3 -} - -// RayBodyIntersections returns a list of bodies whose bounding box intersects -// with the given ray, with the point of intersection -func (gp *Group) RayBodyIntersections(ray math32.Ray) []*BodyPoint { - var bs []*BodyPoint - gp.WalkDown(func(k tree.Node) bool { - nii, ni := AsNode(k) - if nii == nil { - return false // going into a different type of thing, bail - } - pt, has := ray.IntersectBox(ni.BBox.BBox) - if !has { - return false - } - bd := nii.AsBody() - if bd == nil { - return true - } - bs = append(bs, &BodyPoint{bd, pt}) - return false - }) - - sort.Slice(bs, func(i, j int) bool { - di := bs[i].Point.DistanceTo(ray.Origin) - dj := bs[j].Point.DistanceTo(ray.Origin) - return di < dj - }) - - return bs -} +// Use a group for the top-level World node. +// type Group struct { +// NodeBase +// } +// +// func (gp *Group) Update() { +// gp.RunUpdaters() +// gp.WalkDown(func(n tree.Node) bool { +// if n.AsTree().This == gp.This { +// return tree.Continue +// } +// n.(Node).Update() +// return tree.Continue +// }) +// } +// +// func (gp *Group) InitAbs(par *NodeBase) { +// gp.InitAbsBase(par) +// } +// +// func (gp *Group) RelToAbs(par *NodeBase) { +// gp.RelToAbsBase(par) // yes we can move groups +// } +// +// func (gp *Group) Step(step float32) { +// // groups do NOT update physics +// } +// +// func (gp *Group) GroupBBox() { +// hasDyn := false +// gp.BBox.BBox.SetEmpty() +// gp.BBox.VelBBox.SetEmpty() +// for _, kid := range gp.Children { +// n, nb := AsNode(kid) +// if n == nil { +// continue +// } +// gp.BBox.BBox.ExpandByBox(nb.BBox.BBox) +// gp.BBox.VelBBox.ExpandByBox(nb.BBox.VelBBox) +// if nb.Dynamic { +// hasDyn = true +// } +// } +// gp.SetDynamic(hasDyn) +// } +// +// // WorldDynGroupBBox does a GroupBBox on all dynamic nodes +// func (gp *Group) WorldDynGroupBBox() { +// gp.WalkDownPost(func(tn tree.Node) bool { +// n, nb := AsNode(tn) +// if n == nil { +// return false +// } +// if !nb.Dynamic { +// return false +// } +// return true +// }, func(tn tree.Node) bool { +// n, nb := AsNode(tn) +// if n == nil { +// return false +// } +// if !nb.Dynamic { +// return false +// } +// n.GroupBBox() +// return true +// }) +// } +// +// // WorldInit does the full tree InitAbs and GroupBBox updates +// func (gp *Group) WorldInit() { +// gp.Update() +// gp.WalkDown(func(tn tree.Node) bool { +// n, _ := AsNode(tn) +// if n == nil { +// return false +// } +// _, pi := AsNode(tn.AsTree().Parent) +// n.InitAbs(pi) +// return true +// }) +// +// gp.WalkDownPost(func(tn tree.Node) bool { +// n, _ := AsNode(tn) +// if n == nil { +// return false +// } +// return true +// }, func(tn tree.Node) bool { +// n, _ := AsNode(tn) +// if n == nil { +// return false +// } +// n.GroupBBox() +// return true +// }) +// +// } +// +// // WorldRelToAbs does a full RelToAbs update for all Dynamic groups, for +// // Scripted mode updates with manual updating of Rel values. +// func (gp *Group) WorldRelToAbs() { +// gp.WalkDown(func(tn tree.Node) bool { +// n, nb := AsNode(tn) +// if n == nil { +// return false // going into a different type of thing, bail +// } +// if !nb.Dynamic { +// return false +// } +// _, pi := AsNode(tn.AsTree().Parent) +// n.RelToAbs(pi) +// return true +// }) +// +// gp.WorldDynGroupBBox() +// } +// +// // WorldStep does a full Step update for all Dynamic nodes, for +// // either physics or scripted mode, based on current velocities. +// func (gp *Group) WorldStep(step float32) { +// gp.WalkDown(func(tn tree.Node) bool { +// n, nb := AsNode(tn) +// if n == nil { +// return false // going into a different type of thing, bail +// } +// if !nb.Dynamic { +// return false +// } +// n.Step(step) +// return true +// }) +// +// gp.WorldDynGroupBBox() +// } +// +// const ( +// // DynsTopGps is passed to WorldCollide when all dynamic objects are in separate top groups +// DynsTopGps = true +// +// // DynsSubGps is passed to WorldCollide when all dynamic objects are in separate groups under top +// // level (i.e., one level deeper) +// DynsSubGps +// ) +// +// // WorldCollide does first pass filtering step of collision detection +// // based on separate dynamic vs. dynamic and dynamic vs. static groups. +// // If dynTop is true, then each Dynamic group is separate at the top level -- +// // otherwise they are organized at the next group level. +// // Contacts are organized by dynamic group, when non-nil, for easier +// // processing. +// func (gp *Group) WorldCollide(dynTop bool) []Contacts { +// var stats []Node +// var dyns []Node +// for _, kid := range gp.Children { +// n, nb := AsNode(kid) +// if n == nil { +// continue +// } +// if nb.Dynamic { +// dyns = append(dyns, n) +// } else { +// stats = append(stats, n) +// } +// } +// +// var sdyns []Node +// if !dynTop { +// for _, d := range dyns { +// for _, dk := range d.AsTree().Children { +// nii, _ := AsNode(dk) +// if nii == nil { +// continue +// } +// sdyns = append(sdyns, nii) +// } +// } +// dyns = sdyns +// } +// +// var cts []Contacts +// for i, d := range dyns { +// var dct Contacts +// for _, s := range stats { +// cc := BodyVelBBoxIntersects(d, s) +// dct = append(dct, cc...) +// } +// for di := 0; di < i; di++ { +// od := dyns[di] +// cc := BodyVelBBoxIntersects(d, od) +// dct = append(dct, cc...) +// } +// if len(dct) > 0 { +// cts = append(cts, dct) +// } +// } +// return cts +// } +// +// // BodyPoint contains a Body and a Point on that body +// type BodyPoint struct { +// Body Body +// Point math32.Vector3 +// } +// +// // RayBodyIntersections returns a list of bodies whose bounding box intersects +// // with the given ray, with the point of intersection +// func (gp *Group) RayBodyIntersections(ray math32.Ray) []*BodyPoint { +// var bs []*BodyPoint +// gp.WalkDown(func(k tree.Node) bool { +// nii, ni := AsNode(k) +// if nii == nil { +// return false // going into a different type of thing, bail +// } +// pt, has := ray.IntersectBox(ni.BBox.BBox) +// if !has { +// return false +// } +// bd := nii.AsBody() +// if bd == nil { +// return true +// } +// bs = append(bs, &BodyPoint{bd, pt}) +// return false +// }) +// +// sort.Slice(bs, func(i, j int) bool { +// di := bs[i].Point.DistanceTo(ray.Origin) +// dj := bs[j].Point.DistanceTo(ray.Origin) +// return di < dj +// }) +// +// return bs +//} diff --git a/physics/joint.go b/physics/joint.go new file mode 100644 index 00000000..b9fe4c0a --- /dev/null +++ b/physics/joint.go @@ -0,0 +1,98 @@ +// Code generated by "goal build"; DO NOT EDIT. +//line joint.goal:1 +// Copyright (c) 2025, Cogent Core. All rights reserved. +// Use of this source code is governed by a BSD-style +// license that can be found in the LICENSE file. + +package physics + +import ( + "math" + + "cogentcore.org/core/math32" +) + +//gosl:start + +// JointVars are joint state variables stored in tensor.Float32 +type JointVars int32 //enums:enum + +const ( + // JointType (as an int32 from bits). + JointType JointVars = iota + + // JointA is the dynamic body index for source, anchor, A body. + JointA + + // JointB is the dynamic body index for target, follower, B body. + JointB + + // position of joint. + JointPosX + JointPosY + JointPosZ + + // joint parameters, specific to each joint type. + JointParamA + JointParamB +) + +func GetJointType(idx int32) JointTypes { + return JointTypes(math.Float32bits(Joints.Value(int(idx), int(JointType)))) +} + +func SetJointType(idx int32, typ JointTypes) { + Joints.Set(math.Float32frombits(uint32(typ)), int(idx), int(JointType)) +} + +func SetJointA(idx, bodyIdx int32) { + Joints.Set(math.Float32frombits(uint32(bodyIdx)), int(idx), int(JointA)) +} + +func JointAIndex(idx int32) int32 { + return int32(math.Float32bits(Joints.Value(int(idx), int(JointA)))) +} + +func SetJointB(idx, bodyIdx int32) { + Joints.Set(math.Float32frombits(uint32(bodyIdx)), int(idx), int(JointB)) +} + +func JointBIndex(idx int32) int32 { + return int32(math.Float32bits(Joints.Value(int(idx), int(JointB)))) +} + +func JointPos(idx int32) math32.Vector3 { + var pos math32.Vector3 + pos.X = Joints.Value(int(idx), int(JointPosX)) + pos.Y = Joints.Value(int(idx), int(JointPosY)) + pos.Z = Joints.Value(int(idx), int(JointPosZ)) + return pos +} + +func SetJointPos(idx int32, pos math32.Vector3) { + Joints.Set(pos.X, int(idx), int(JointPosX)) + Joints.Set(pos.Y, int(idx), int(JointPosY)) + Joints.Set(pos.Z, int(idx), int(JointPosZ)) +} + +// JointTypes are joint types that determine nature of interaction. +type JointTypes int32 //enums:enum + +const ( + // Glue is a completely rigid joint that supercedes all others, + // where body A drives motion of body B. + // JointA is the parent, supporting body, JointB is the child supported. + Glue JointTypes = iota + + Ball +) + +// GlueStep does glue joint processing +func GlueStep(ji, ba, bb int32) { + pos := JointPos(ji) + bap := DynamicPos(ba) + bbp := pos.Add(bap) + SetDynamicPos(bb, bbp) +} + +//gosl:end diff --git a/physics/joint.goal b/physics/joint.goal new file mode 100644 index 00000000..dc72e4e0 --- /dev/null +++ b/physics/joint.goal @@ -0,0 +1,96 @@ +// Copyright (c) 2025, Cogent Core. All rights reserved. +// Use of this source code is governed by a BSD-style +// license that can be found in the LICENSE file. + +package physics + +import ( + "math" + + "cogentcore.org/core/math32" +) + +//gosl:start + +// JointVars are joint state variables stored in tensor.Float32 +type JointVars int32 //enums:enum + +const ( + // JointType (as an int32 from bits). + JointType JointVars = iota + + // JointA is the dynamic body index for source, anchor, A body. + JointA + + // JointB is the dynamic body index for target, follower, B body. + JointB + + // position of joint. + JointPosX + JointPosY + JointPosZ + + // joint parameters, specific to each joint type. + JointParamA + JointParamB +) + +func GetJointType(idx int32) JointTypes { + return JointTypes(math.Float32bits(Joints[idx, JointType])) +} + +func SetJointType(idx int32, typ JointTypes) { + Joints[idx, JointType] = math.Float32frombits(uint32(typ)) +} + +func SetJointA(idx, bodyIdx int32) { + Joints[idx, JointA] = math.Float32frombits(uint32(bodyIdx)) +} + +func JointAIndex(idx int32) int32 { + return int32(math.Float32bits(Joints[idx, JointA])) +} + +func SetJointB(idx, bodyIdx int32) { + Joints[idx, JointB] = math.Float32frombits(uint32(bodyIdx)) +} + +func JointBIndex(idx int32) int32 { + return int32(math.Float32bits(Joints[idx, JointB])) +} + +func JointPos(idx int32) math32.Vector3 { + var pos math32.Vector3 + pos.X = Joints[idx, JointPosX] + pos.Y = Joints[idx, JointPosY] + pos.Z = Joints[idx, JointPosZ] + return pos +} + +func SetJointPos(idx int32, pos math32.Vector3) { + Joints[idx, JointPosX] = pos.X + Joints[idx, JointPosY] = pos.Y + Joints[idx, JointPosZ] = pos.Z +} + +// JointTypes are joint types that determine nature of interaction. +type JointTypes int32 //enums:enum + +const ( + // Glue is a completely rigid joint that supercedes all others, + // where body A drives motion of body B. + // JointA is the parent, supporting body, JointB is the child supported. + Glue JointTypes = iota + + Ball +) + +// GlueStep does glue joint processing +func GlueStep(ji, ba, bb int32) { + pos := JointPos(ji) + bap := DynamicPos(ba) + bbp := pos.Add(bap) + SetDynamicPos(bb, bbp) +} + +//gosl:end diff --git a/physics/node.go b/physics/node.go deleted file mode 100644 index 578ff137..00000000 --- a/physics/node.go +++ /dev/null @@ -1,186 +0,0 @@ -// Copyright (c) 2019, Cogent Core. All rights reserved. -// Use of this source code is governed by a BSD-style -// license that can be found in the LICENSE file. - -package physics - -//go:generate core generate -add-types - -import ( - "cogentcore.org/core/math32" - "cogentcore.org/core/tree" -) - -// Node is the common interface for all nodes. -type Node interface { - tree.Node - - // AsNodeBase returns a generic NodeBase for our node -- gives generic - // access to all the base-level data structures without needing interface methods. - AsNodeBase() *NodeBase - - // AsBody returns a generic Body interface for our node -- nil if not a Body - AsBody() Body - - // GroupBBox sets bounding boxes for groups based on groups or bodies. - // called in a FuncDownMeLast traversal. - GroupBBox() - - // InitAbs sets current Abs physical state parameters from Initial values - // which are local, relative to parent -- is passed the parent (nil = top). - // Body nodes should also set their bounding boxes. - // Called in a FuncDownMeFirst traversal. - InitAbs(par *NodeBase) - - // RelToAbs updates current world Abs physical state parameters - // based on Rel values added to updated Abs values at higher levels. - // Abs.LinVel is updated from the resulting change from prior position. - // This is useful for manual updating of relative positions (scripted movement). - // It is passed the parent (nil = top). - // Body nodes should also update their bounding boxes. - // Called in a FuncDownMeFirst traversal. - RelToAbs(par *NodeBase) - - // Step computes one update of the world Abs physical state parameters, - // using *current* velocities -- add forces prior to calling. - // Use this for physics-based state updates. - // Body nodes should also update their bounding boxes. - Step(step float32) - - // Update does [tree] updating to dynamically update nodes / tree config. - Update() -} - -// NodeBase is the basic node, which has position, rotation, velocity -// and computed bounding boxes, etc. -// There are only three different kinds of Nodes: Group, Body, and Joint -type NodeBase struct { - tree.NodeBase - - // Dynamic is whether this node can move. If it is false, then this is a Static node. - // Any top-level group that is not Dynamic is immediately pruned from further consideration, - // so top-level groups should be separated into Dynamic and Static nodes at the start. - Dynamic bool - - // initial position, orientation, velocity in *local* coordinates (relative to parent) - Initial State `display:"inline"` - - // current relative (local) position, orientation, velocity -- only change these values, as abs values are computed therefrom - Rel State `display:"inline"` - - // current absolute (world) position, orientation, velocity - Abs State `set:"-" edit:"-" display:"inline"` - - // bounding box in world coordinates (aggregated for groups) - BBox BBox `set:"-"` - - // NewView is a function that returns a new [xyz.Node] - // to represent this node. If nil, Groups make Groups, - // and bodies make corresponding Solid shape. - NewView func() tree.Node - - // InitView is a function that initializes a new [xyz.Node] - // that represents this physics node. If nil, Groups make Group children, - // and bodies configure corresponding Solid shape. - InitView func(n tree.Node) - - // View is the current view node for this node, set when made. - View tree.Node `set:"-"` -} - -func (nb *NodeBase) Init() { - nb.Updater(nb.UpdateFromMake) -} - -func (nb *NodeBase) AsNodeBase() *NodeBase { - return nb -} - -func (nb *NodeBase) AsBody() Body { - return nil -} - -// SetInitPos sets the initial position -func (nb *NodeBase) SetInitPos(pos math32.Vector3) *NodeBase { - nb.Initial.Pos = pos - return nb -} - -// SetInitQuat sets the initial rotation as a Quaternion -func (nb *NodeBase) SetInitQuat(quat math32.Quat) *NodeBase { - nb.Initial.Quat = quat - return nb -} - -// SetInitLinVel sets the initial linear velocity -func (nb *NodeBase) SetInitLinVel(vel math32.Vector3) *NodeBase { - nb.Initial.LinVel = vel - return nb -} - -// SetInitAngVel sets the initial angular velocity -func (nb *NodeBase) SetInitAngVel(vel math32.Vector3) *NodeBase { - nb.Initial.AngVel = vel - return nb -} - -// InitAbsBase is the base-level version of InitAbs -- most nodes call this. -// InitAbs sets current Abs physical state parameters from Initial values -// which are local, relative to parent -- is passed the parent (nil = top). -// Body nodes should also set their bounding boxes. -// Called in a FuncDownMeFirst traversal. -func (nb *NodeBase) InitAbsBase(par *NodeBase) { - if nb.Initial.Quat.IsNil() { - nb.Initial.Quat.SetIdentity() - } - nb.Rel = nb.Initial - if par != nil { - nb.Abs.FromRel(&nb.Initial, &par.Abs) - } else { - nb.Abs = nb.Initial - } -} - -// RelToAbsBase is the base-level version of RelToAbs -- most nodes call this. -// note: Group WorldRelToAbs ensures only called on Dynamic nodes. -// RelToAbs updates current world Abs physical state parameters -// based on Rel values added to updated Abs values at higher levels. -// Abs.LinVel is updated from the resulting change from prior position. -// This is useful for manual updating of relative positions (scripted movement). -// It is passed the parent (nil = top). -// Body nodes should also update their bounding boxes. -// Called in a FuncDownMeFirst traversal. -func (nb *NodeBase) RelToAbsBase(par *NodeBase) { - ppos := nb.Abs.Pos - if par != nil { - nb.Abs.FromRel(&nb.Rel, &par.Abs) - } else { - nb.Abs = nb.Rel - } - nb.Abs.LinVel = nb.Abs.Pos.Sub(ppos) // needed for VelBBox projection -} - -// StepBase is base-level version of Step -- most nodes call this. -// note: Group WorldRelToAbs ensures only called on Dynamic nodes. -// Computes one update of the world Abs physical state parameters, -// using *current* velocities -- add forces prior to calling. -// Use this for physics-based state updates. -// Body nodes should also update their bounding boxes. -func (nb *NodeBase) StepBase(step float32) { - nb.Abs.StepByAngVel(step) - nb.Abs.StepByLinVel(step) -} - -func (nb *NodeBase) Update() { - nb.RunUpdaters() -} - -// AsNode converts a [tree.Node] to a [Node] interface and a [Node3DBase] object, -// or nil if not possible. -func AsNode(n tree.Node) (Node, *NodeBase) { - nii, ok := n.(Node) - if ok { - return nii, nii.AsNodeBase() - } - return nil, nil -} diff --git a/physics/params.go b/physics/params.go new file mode 100644 index 00000000..150ded50 --- /dev/null +++ b/physics/params.go @@ -0,0 +1,34 @@ +// Copyright (c) 2025, Cogent Core. All rights reserved. +// Use of this source code is governed by a BSD-style +// license that can be found in the LICENSE file. + +package physics + +import "cogentcore.org/core/math32" + +//gosl:start + +// PhysParams are the physics parameters +type PhysParams struct { + // DynamicsN is number of dynamics bodies. + DynamicsN int32 + + // JointsN is number of joints. + JointsN int32 + + // Step is the global stepsize for numerical integration. + Step float32 `default:"0.01"` + + // Gravity is the force of gravity. + Gravity float32 + + // GravityDir is the direction of Gravity. + GravityDir math32.Vector4 +} + +func (pr *PhysParams) Defaults() { + pr.Step = 0.01 + pr.GravityDir.Set(0, -1, 0, 0) +} + +//gosl:end diff --git a/physics/rigid.go b/physics/rigid.go deleted file mode 100644 index 7cde5331..00000000 --- a/physics/rigid.go +++ /dev/null @@ -1,33 +0,0 @@ -// Copyright (c) 2019, Cogent Core. All rights reserved. -// Use of this source code is governed by a BSD-style -// license that can be found in the LICENSE file. - -package physics - -import ( - "cogentcore.org/core/math32" -) - -// Rigid contains the full specification of a given object's basic physics -// properties including position, orientation, velocity. These -type Rigid struct { - - // 1/mass -- 0 for no mass - InvMass float32 - - // COR or coefficient of restitution -- how elastic is the collision i.e., final velocity / initial velocity - Bounce float32 `min:"0" max:"1"` - - // friction coefficient -- how much friction is generated by transverse motion - Friction float32 - - // record of computed force vector from last iteration - Force math32.Vector3 - - // Last calculated rotational inertia matrix in local coords - RotInertia math32.Matrix3 -} - -// Defaults sets defaults only if current values are nil -func (ps *Rigid) Defaults() { -} diff --git a/physics/shaders/InitDynamics.wgsl b/physics/shaders/InitDynamics.wgsl new file mode 100644 index 00000000..c6d0dfd4 --- /dev/null +++ b/physics/shaders/InitDynamics.wgsl @@ -0,0 +1,145 @@ +// Code generated by "gosl"; DO NOT EDIT +// kernel: InitDynamics + +// // Params are global parameters. +@group(0) @binding(0) +var TensorStrides: array; +@group(0) @binding(1) +var Params: array; +// // Bodies are the rigid body elements (dynamic and static), // specifying the constant, non-dynamic properties, // which is initial state for dynamics. // [body][BodyVarsN] +@group(1) @binding(0) +var Bodies: array; +@group(1) @binding(1) +var Dynamics: array; +// // Contacts are points of contact between bodies. // [contact][ContactVarsN] + +alias GPUVars = i32; + +@compute @workgroup_size(64, 1, 1) +fn main(@builtin(workgroup_id) wgid: vec3, @builtin(num_workgroups) nwg: vec3, @builtin(local_invocation_index) loci: u32) { + let idx = loci + (wgid.x + wgid.y * nwg.x + wgid.z * nwg.x * nwg.y) * 64; + InitDynamics(idx); +} + +fn Index2D(s0: u32, s1: u32, i0: u32, i1: u32) -> u32 { + return s0 * i0 + s1 * i1; +} + + +//////// import: "vars.go" + +//////// import: "body.go" +alias BodyVars = i32; //enums:enum +const Shape: BodyVars = 0; +const SizeX: BodyVars = 1; +const SizeY: BodyVars = 2; +const SizeZ: BodyVars = 3; +const Mass: BodyVars = 4; +const Bounce: BodyVars = 5; +const Friction: BodyVars = 6; +const BodyPosX: BodyVars = 7; +const BodyPosY: BodyVars = 8; +const BodyPosZ: BodyVars = 9; +const BodyRotX: BodyVars = 10; +const BodyRotY: BodyVars = 11; +const BodyRotZ: BodyVars = 12; +const BodyRotW: BodyVars = 13; +alias DynamicVars = i32; //enums:enum +const Index: DynamicVars = 0; +const PosX: DynamicVars = 1; +const PosY: DynamicVars = 2; +const PosZ: DynamicVars = 3; +const RotX: DynamicVars = 4; +const RotY: DynamicVars = 5; +const RotZ: DynamicVars = 6; +const RotW: DynamicVars = 7; +const VelX: DynamicVars = 8; +const VelY: DynamicVars = 9; +const VelZ: DynamicVars = 10; +const AccX: DynamicVars = 11; +const AccY: DynamicVars = 12; +const AccZ: DynamicVars = 13; +const ForceX: DynamicVars = 14; +const ForceY: DynamicVars = 15; +const ForceZ: DynamicVars = 16; +const AngVelX: DynamicVars = 17; +const AngVelY: DynamicVars = 18; +const AngVelZ: DynamicVars = 19; +const AngAccX: DynamicVars = 20; +const AngAccY: DynamicVars = 21; +const AngAccZ: DynamicVars = 22; +fn DynamicIndex(idx: i32) -> i32 { + return i32(bitcast(Dynamics[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(Index))])); +} + +//////// import: "collide.go" +alias ContactVars = i32; //enums:enum +const ContactA: ContactVars = 0; +const ContactB: ContactVars = 1; +const ContactNormX: ContactVars = 2; +const ContactNormY: ContactVars = 3; +const ContactNormZ: ContactVars = 4; +const ContactPointX: ContactVars = 5; +const ContactPointY: ContactVars = 6; +const ContactPointZ: ContactVars = 7; +const ContactDist: ContactVars = 8; + +//////// import: "enumgen.go" +const BodyVarsN: BodyVars = 14; +const DynamicVarsN: DynamicVars = 23; +const ContactVarsN: ContactVars = 9; +const GPUVarsN: GPUVars = 5; +const JointVarsN: JointVars = 8; +const JointTypesN: JointTypes = 2; +const ShapesN: Shapes = 4; + +//////// import: "joint.go" +alias JointVars = i32; //enums:enum +const JointType: JointVars = 0; +const JointA: JointVars = 1; +const JointB: JointVars = 2; +const JointPosX: JointVars = 3; +const JointPosY: JointVars = 4; +const JointPosZ: JointVars = 5; +const JointParamA: JointVars = 6; +const JointParamB: JointVars = 7; +alias JointTypes = i32; //enums:enum +const Glue: JointTypes = 0; +const Ball: JointTypes = 1; + +//////// import: "params.go" +struct PhysParams { + DynamicsN: i32, + JointsN: i32, + Step: f32, + Gravity: f32, + GravityDir: vec4, +} + +//////// import: "shapes.go" +alias Shapes = i32; //enums:enum +const Box: Shapes = 0; +const Sphere: Shapes = 1; +const Cylinder: Shapes = 2; +const Capsule: Shapes = 3; + +//////// import: "step.go" +fn InitDynamics(i: u32) { //gosl:kernel + let pars = Params[0]; + var ii = i32(i); + if (ii >= pars.DynamicsN) { + return; + } + var bi = DynamicIndex(ii); + Dynamics[Index2D(TensorStrides[10], TensorStrides[11], u32(ii), u32(PosX))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyPosX))]; + Dynamics[Index2D(TensorStrides[10], TensorStrides[11], u32(ii), u32(PosY))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyPosY))]; + Dynamics[Index2D(TensorStrides[10], TensorStrides[11], u32(ii), u32(PosZ))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyPosZ))]; + Dynamics[Index2D(TensorStrides[10], TensorStrides[11], u32(ii), u32(RotX))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyRotX))]; + Dynamics[Index2D(TensorStrides[10], TensorStrides[11], u32(ii), u32(RotY))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyRotY))]; + Dynamics[Index2D(TensorStrides[10], TensorStrides[11], u32(ii), u32(RotZ))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyRotZ))]; + Dynamics[Index2D(TensorStrides[10], TensorStrides[11], u32(ii), u32(RotW))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyRotW))]; + for (var v = VelX; v < DynamicVarsN; v++) { + Dynamics[Index2D(TensorStrides[10], TensorStrides[11], + u32(ii), u32(v))] = 0.0; + } +} \ No newline at end of file diff --git a/physics/shaders/Step.wgsl b/physics/shaders/Step.wgsl new file mode 100644 index 00000000..7f679a6e --- /dev/null +++ b/physics/shaders/Step.wgsl @@ -0,0 +1,131 @@ +// Code generated by "gosl"; DO NOT EDIT +// kernel: Step + +// // Params are global parameters. +@group(0) @binding(0) +var TensorStrides: array; +@group(0) @binding(1) +var Params: array; +// // Bodies are the rigid body elements (dynamic and static), // specifying the constant, non-dynamic properties, // which is initial state for dynamics. // [body][BodyVarsN] +@group(1) @binding(1) +var Dynamics: array; +// // Contacts are points of contact between bodies. // [contact][ContactVarsN] + +alias GPUVars = i32; + +@compute @workgroup_size(64, 1, 1) +fn main(@builtin(workgroup_id) wgid: vec3, @builtin(num_workgroups) nwg: vec3, @builtin(local_invocation_index) loci: u32) { + let idx = loci + (wgid.x + wgid.y * nwg.x + wgid.z * nwg.x * nwg.y) * 64; + Step(idx); +} + +fn Index2D(s0: u32, s1: u32, i0: u32, i1: u32) -> u32 { + return s0 * i0 + s1 * i1; +} + + +//////// import: "vars.go" + +//////// import: "body.go" +alias BodyVars = i32; //enums:enum +const Shape: BodyVars = 0; +const SizeX: BodyVars = 1; +const SizeY: BodyVars = 2; +const SizeZ: BodyVars = 3; +const Mass: BodyVars = 4; +const Bounce: BodyVars = 5; +const Friction: BodyVars = 6; +const BodyPosX: BodyVars = 7; +const BodyPosY: BodyVars = 8; +const BodyPosZ: BodyVars = 9; +const BodyRotX: BodyVars = 10; +const BodyRotY: BodyVars = 11; +const BodyRotZ: BodyVars = 12; +const BodyRotW: BodyVars = 13; +alias DynamicVars = i32; //enums:enum +const Index: DynamicVars = 0; +const PosX: DynamicVars = 1; +const PosY: DynamicVars = 2; +const PosZ: DynamicVars = 3; +const RotX: DynamicVars = 4; +const RotY: DynamicVars = 5; +const RotZ: DynamicVars = 6; +const RotW: DynamicVars = 7; +const VelX: DynamicVars = 8; +const VelY: DynamicVars = 9; +const VelZ: DynamicVars = 10; +const AccX: DynamicVars = 11; +const AccY: DynamicVars = 12; +const AccZ: DynamicVars = 13; +const ForceX: DynamicVars = 14; +const ForceY: DynamicVars = 15; +const ForceZ: DynamicVars = 16; +const AngVelX: DynamicVars = 17; +const AngVelY: DynamicVars = 18; +const AngVelZ: DynamicVars = 19; +const AngAccX: DynamicVars = 20; +const AngAccY: DynamicVars = 21; +const AngAccZ: DynamicVars = 22; + +//////// import: "collide.go" +alias ContactVars = i32; //enums:enum +const ContactA: ContactVars = 0; +const ContactB: ContactVars = 1; +const ContactNormX: ContactVars = 2; +const ContactNormY: ContactVars = 3; +const ContactNormZ: ContactVars = 4; +const ContactPointX: ContactVars = 5; +const ContactPointY: ContactVars = 6; +const ContactPointZ: ContactVars = 7; +const ContactDist: ContactVars = 8; + +//////// import: "enumgen.go" +const BodyVarsN: BodyVars = 14; +const DynamicVarsN: DynamicVars = 23; +const ContactVarsN: ContactVars = 9; +const GPUVarsN: GPUVars = 5; +const JointVarsN: JointVars = 8; +const JointTypesN: JointTypes = 2; +const ShapesN: Shapes = 4; + +//////// import: "joint.go" +alias JointVars = i32; //enums:enum +const JointType: JointVars = 0; +const JointA: JointVars = 1; +const JointB: JointVars = 2; +const JointPosX: JointVars = 3; +const JointPosY: JointVars = 4; +const JointPosZ: JointVars = 5; +const JointParamA: JointVars = 6; +const JointParamB: JointVars = 7; +alias JointTypes = i32; //enums:enum +const Glue: JointTypes = 0; +const Ball: JointTypes = 1; + +//////// import: "params.go" +struct PhysParams { + DynamicsN: i32, + JointsN: i32, + Step: f32, + Gravity: f32, + GravityDir: vec4, +} + +//////// import: "shapes.go" +alias Shapes = i32; //enums:enum +const Box: Shapes = 0; +const Sphere: Shapes = 1; +const Cylinder: Shapes = 2; +const Capsule: Shapes = 3; + +//////// import: "step.go" +fn Step(i: u32) { //gosl:kernel + let pars = Params[0]; + var ii = i32(i); + if (ii >= pars.DynamicsN) { + return; + } + Dynamics[Index2D(TensorStrides[10], TensorStrides[11], u32(ii), u32(PosX))] += pars.Step * Dynamics[Index2D(TensorStrides[10], TensorStrides[11], u32(ii), u32(VelX))]; + Dynamics[Index2D(TensorStrides[10], TensorStrides[11], u32(ii), u32(PosY))] += pars.Step * Dynamics[Index2D(TensorStrides[10], TensorStrides[11], u32(ii), u32(VelY))]; + Dynamics[Index2D(TensorStrides[10], TensorStrides[11], u32(ii), u32(PosZ))] += pars.Step * Dynamics[Index2D(TensorStrides[10], TensorStrides[11], u32(ii), u32(VelZ))]; +} \ No newline at end of file diff --git a/physics/shaders/StepJoints.wgsl b/physics/shaders/StepJoints.wgsl new file mode 100644 index 00000000..a5cd56ca --- /dev/null +++ b/physics/shaders/StepJoints.wgsl @@ -0,0 +1,170 @@ +// Code generated by "gosl"; DO NOT EDIT +// kernel: StepJoints + +// // Params are global parameters. +@group(0) @binding(0) +var TensorStrides: array; +@group(0) @binding(1) +var Params: array; +// // Bodies are the rigid body elements (dynamic and static), // specifying the constant, non-dynamic properties, // which is initial state for dynamics. // [body][BodyVarsN] +@group(1) @binding(1) +var Dynamics: array; +@group(1) @binding(2) +var Joints: array; +// // Contacts are points of contact between bodies. // [contact][ContactVarsN] + +alias GPUVars = i32; + +@compute @workgroup_size(64, 1, 1) +fn main(@builtin(workgroup_id) wgid: vec3, @builtin(num_workgroups) nwg: vec3, @builtin(local_invocation_index) loci: u32) { + let idx = loci + (wgid.x + wgid.y * nwg.x + wgid.z * nwg.x * nwg.y) * 64; + StepJoints(idx); +} + +fn Index2D(s0: u32, s1: u32, i0: u32, i1: u32) -> u32 { + return s0 * i0 + s1 * i1; +} + + +//////// import: "vars.go" + +//////// import: "body.go" +alias BodyVars = i32; //enums:enum +const Shape: BodyVars = 0; +const SizeX: BodyVars = 1; +const SizeY: BodyVars = 2; +const SizeZ: BodyVars = 3; +const Mass: BodyVars = 4; +const Bounce: BodyVars = 5; +const Friction: BodyVars = 6; +const BodyPosX: BodyVars = 7; +const BodyPosY: BodyVars = 8; +const BodyPosZ: BodyVars = 9; +const BodyRotX: BodyVars = 10; +const BodyRotY: BodyVars = 11; +const BodyRotZ: BodyVars = 12; +const BodyRotW: BodyVars = 13; +alias DynamicVars = i32; //enums:enum +const Index: DynamicVars = 0; +const PosX: DynamicVars = 1; +const PosY: DynamicVars = 2; +const PosZ: DynamicVars = 3; +const RotX: DynamicVars = 4; +const RotY: DynamicVars = 5; +const RotZ: DynamicVars = 6; +const RotW: DynamicVars = 7; +const VelX: DynamicVars = 8; +const VelY: DynamicVars = 9; +const VelZ: DynamicVars = 10; +const AccX: DynamicVars = 11; +const AccY: DynamicVars = 12; +const AccZ: DynamicVars = 13; +const ForceX: DynamicVars = 14; +const ForceY: DynamicVars = 15; +const ForceZ: DynamicVars = 16; +const AngVelX: DynamicVars = 17; +const AngVelY: DynamicVars = 18; +const AngVelZ: DynamicVars = 19; +const AngAccX: DynamicVars = 20; +const AngAccY: DynamicVars = 21; +const AngAccZ: DynamicVars = 22; +fn DynamicPos(idx: i32) -> vec3 { + var pos: vec3; + pos.x = Dynamics[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(PosX))]; + pos.y = Dynamics[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(PosY))]; + pos.z = Dynamics[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(PosZ))];return pos; +} +fn SetDynamicPos(idx: i32, pos: vec3) { + Dynamics[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(PosX))] = pos.x; + Dynamics[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(PosY))] = pos.y; + Dynamics[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(PosZ))] = pos.z; +} + +//////// import: "collide.go" +alias ContactVars = i32; //enums:enum +const ContactA: ContactVars = 0; +const ContactB: ContactVars = 1; +const ContactNormX: ContactVars = 2; +const ContactNormY: ContactVars = 3; +const ContactNormZ: ContactVars = 4; +const ContactPointX: ContactVars = 5; +const ContactPointY: ContactVars = 6; +const ContactPointZ: ContactVars = 7; +const ContactDist: ContactVars = 8; + +//////// import: "enumgen.go" +const BodyVarsN: BodyVars = 14; +const DynamicVarsN: DynamicVars = 23; +const ContactVarsN: ContactVars = 9; +const GPUVarsN: GPUVars = 5; +const JointVarsN: JointVars = 8; +const JointTypesN: JointTypes = 2; +const ShapesN: Shapes = 4; + +//////// import: "joint.go" +alias JointVars = i32; //enums:enum +const JointType: JointVars = 0; +const JointA: JointVars = 1; +const JointB: JointVars = 2; +const JointPosX: JointVars = 3; +const JointPosY: JointVars = 4; +const JointPosZ: JointVars = 5; +const JointParamA: JointVars = 6; +const JointParamB: JointVars = 7; +fn GetJointType(idx: i32) -> JointTypes { + return JointTypes(bitcast(Joints[Index2D(TensorStrides[20], TensorStrides[21], u32(idx), u32(JointType))])); +} +fn JointAIndex(idx: i32) -> i32 { + return i32(bitcast(Joints[Index2D(TensorStrides[20], TensorStrides[21], u32(idx), u32(JointA))])); +} +fn JointBIndex(idx: i32) -> i32 { + return i32(bitcast(Joints[Index2D(TensorStrides[20], TensorStrides[21], u32(idx), u32(JointB))])); +} +fn JointPos(idx: i32) -> vec3 { + var pos: vec3; + pos.x = Joints[Index2D(TensorStrides[20], TensorStrides[21], u32(idx), u32(JointPosX))]; + pos.y = Joints[Index2D(TensorStrides[20], TensorStrides[21], u32(idx), u32(JointPosY))]; + pos.z = Joints[Index2D(TensorStrides[20], TensorStrides[21], u32(idx), u32(JointPosZ))];return pos; +} +alias JointTypes = i32; //enums:enum +const Glue: JointTypes = 0; +const Ball: JointTypes = 1; +fn GlueStep(ji: i32,ba: i32,bb: i32) { + var pos = JointPos(ji); + var bap = DynamicPos(ba); + var bbp = pos.Add(bap); + SetDynamicPos(bb, bbp); +} + +//////// import: "params.go" +struct PhysParams { + DynamicsN: i32, + JointsN: i32, + Step: f32, + Gravity: f32, + GravityDir: vec4, +} + +//////// import: "shapes.go" +alias Shapes = i32; //enums:enum +const Box: Shapes = 0; +const Sphere: Shapes = 1; +const Cylinder: Shapes = 2; +const Capsule: Shapes = 3; + +//////// import: "step.go" +fn StepJoints(i: u32) { //gosl:kernel + let pars = Params[0]; + var ji = i32(i); + if (ji >= pars.JointsN) { + return; + } + var ba = JointAIndex(ji); + var bb = JointBIndex(ji); + var jt = GetJointType(ji); + switch (jt) { + case Glue: { + GlueStep(ji, ba, bb); + } + } +} \ No newline at end of file diff --git a/physics/shapes.go b/physics/shapes.go new file mode 100644 index 00000000..5cce8684 --- /dev/null +++ b/physics/shapes.go @@ -0,0 +1,53 @@ +// Copyright (c) 2025, Cogent Core. All rights reserved. +// Use of this source code is governed by a BSD-style +// license that can be found in the LICENSE file. + +package physics + +import "cogentcore.org/core/math32" + +//gosl:start + +// Shapes are elemental shapes for rigid bodies. +type Shapes int32 //enums:enum + +const ( + // Box is a 3D rectalinear shape. + Box Shapes = iota + + // Sphere. SizeX is the radius. + Sphere + + // Cylinder, natively oriented vertically along the Y axis. + // If one radius is 0, then it is a cone. + // SizeX = bottom radius, SizeY = height in Y axis, SizeZ = top radius. + Cylinder + + // Capsule, which is a cylinder with half-spheres on the ends. + // Natively oriented vertically along the Y axis. + // SizeX = bottom radius, SizeY = height, SizeZ = top radius. + Capsule +) + +//gosl:end + +func (sh Shapes) ShapeBBox(sz math32.Vector3) math32.Box3 { + var bb math32.Box3 + + switch sh { + case Box: + bb.SetMinMax(sz.MulScalar(-.5), sz.MulScalar(.5)) + case Sphere: + bb.SetMinMax(math32.Vec3(-sz.X, -sz.X, -sz.X), math32.Vec3(sz.X, sz.X, sz.X)) + case Cylinder: + h2 := sz.Y / 2 + bb.SetMinMax(math32.Vec3(-sz.X, -h2, -sz.X), math32.Vec3(sz.Z, h2, sz.Z)) + case Capsule: + th := sz.X + sz.Y + sz.Z + h2 := th / 2 + bb.SetMinMax(math32.Vec3(-sz.X, -h2, -sz.X), math32.Vec3(sz.Z, h2, sz.Z)) + } + // bb.Area = 2*sz.X + 2*sz.Y + 2*sz.Z + // bb.Volume = sz.X * sz.Y * sz.Z + return bb +} diff --git a/physics/sphere.go b/physics/sphere.go deleted file mode 100644 index bf95b8d3..00000000 --- a/physics/sphere.go +++ /dev/null @@ -1,40 +0,0 @@ -// Copyright (c) 2019, Cogent Core. All rights reserved. -// Use of this source code is governed by a BSD-style -// license that can be found in the LICENSE file. - -package physics - -import ( - "cogentcore.org/core/math32" -) - -// Sphere is a spherical body shape. -type Sphere struct { - BodyBase - - // radius - Radius float32 -} - -func (sp *Sphere) SetBBox() { - sp.BBox.SetBounds(math32.Vec3(-sp.Radius, -sp.Radius, -sp.Radius), math32.Vec3(sp.Radius, sp.Radius, sp.Radius)) - sp.BBox.XForm(sp.Abs.Quat, sp.Abs.Pos) -} - -func (sp *Sphere) InitAbs(par *NodeBase) { - sp.InitAbsBase(par) - sp.SetBBox() - sp.BBox.VelNilProject() -} - -func (sp *Sphere) RelToAbs(par *NodeBase) { - sp.RelToAbsBase(par) - sp.SetBBox() - sp.BBox.VelProject(sp.Abs.LinVel, 1) -} - -func (sp *Sphere) Step(step float32) { - sp.StepBase(step) - sp.SetBBox() - sp.BBox.VelProject(sp.Abs.LinVel, step) -} diff --git a/physics/state.go b/physics/state.go index 5f4098dc..ff04d537 100644 --- a/physics/state.go +++ b/physics/state.go @@ -15,16 +15,16 @@ import ( // state values such as Mass should go in Rigid. type State struct { - // position of center of mass of object + // Pos is the position of center of mass of object. Pos math32.Vector3 - // rotation specified as a Quat + // Quat is the rotation specified as a quaternion. Quat math32.Quat - // linear velocity + // LinVel is the linear velocity. LinVel math32.Vector3 - // angular velocity + // AngVel is the angular velocity. AngVel math32.Vector3 } @@ -35,7 +35,7 @@ func (ps *State) Defaults() { } } -//////// State updates +//////// State updates // FromRel sets state from relative values compared to a parent state func (ps *State) FromRel(rel, par *State) { @@ -75,7 +75,7 @@ func (ps *State) StepByLinVel(step float32) { ps.Pos = ps.Pos.Add(ps.LinVel.MulScalar(step)) } -//////// Moving +//////// Moving // Move moves (translates) Pos by given amount, and sets the LinVel to the given // delta -- this can be useful for Scripted motion to track movement. @@ -102,7 +102,7 @@ func (ps *State) MoveOnAxisAbs(x, y, z, dist float32) { //types:add ps.Pos.SetAdd(ps.LinVel) } -//////// Rotating +//////// Rotating // SetEulerRotation sets the rotation in Euler angles (degrees). func (ps *State) SetEulerRotation(x, y, z float32) { //types:add diff --git a/physics/step.go b/physics/step.go new file mode 100644 index 00000000..434f526d --- /dev/null +++ b/physics/step.go @@ -0,0 +1,68 @@ +// Code generated by "goal build"; DO NOT EDIT. +//line step.goal:1 +// Copyright (c) 2025, Cogent Core. All rights reserved. +// Use of this source code is governed by a BSD-style +// license that can be found in the LICENSE file. + +package physics + +//gosl:start + +// InitDynamics copies Body initial state to dynamic state. +func InitDynamics(i uint32) { //gosl:kernel + pars := GetParams(0) + ii := int32(i) + if ii >= pars.DynamicsN { + return + } + bi := DynamicIndex(ii) + Dynamics.Set(Bodies.Value(int(bi), int(BodyPosX)), int(ii), int(PosX)) + Dynamics.Set(Bodies.Value(int(bi), int(BodyPosY)), int(ii), int(PosY)) + Dynamics.Set(Bodies.Value(int(bi), int(BodyPosZ)), int(ii), int(PosZ)) + + Dynamics.Set(Bodies.Value(int(bi), int(BodyRotX)), int(ii), int(RotX)) + Dynamics.Set(Bodies.Value(int(bi), int(BodyRotY)), int(ii), int(RotY)) + Dynamics.Set(Bodies.Value(int(bi), int(BodyRotZ)), int(ii), int(RotZ)) + Dynamics.Set(Bodies.Value(int(bi), int(BodyRotW)), int(ii), int(RotW)) + + for v := VelX; v < DynamicVarsN; v++ { + Dynamics.Set(0.0, int(ii), int(v)) + } +} + +// StepJoints does joint-based update. +func StepJoints(i uint32) { //gosl:kernel + pars := GetParams(0) + ji := int32(i) + if ji >= pars.JointsN { + return + } + ba := JointAIndex(ji) + bb := JointBIndex(ji) + jt := GetJointType(ji) + switch jt { + case Glue: + GlueStep(ji, ba, bb) + } +} + +// Step is dynamic update step kernel. i = body +func Step(i uint32) { //gosl:kernel + pars := GetParams(0) + ii := int32(i) + if ii >= pars.DynamicsN { + return + } + Dynamics.SetAdd(pars.Step*Dynamics.Value(int(ii), int(VelX)), int(ii), int(PosX)) + Dynamics.SetAdd(pars.Step*Dynamics.Value(int(ii), int(VelY)), int(ii), int(PosY)) + Dynamics.SetAdd(pars.Step*Dynamics.Value(int(ii), int(VelZ)), int(ii), int(PosZ)) + + // todo: force, integrated etc. +} + +//gosl:end + +func (wl *World) Step() { + pars := GetParams(0) + RunStep(int(pars.DynamicsN)) +} diff --git a/physics/step.goal b/physics/step.goal new file mode 100644 index 00000000..5a838231 --- /dev/null +++ b/physics/step.goal @@ -0,0 +1,66 @@ +// Copyright (c) 2025, Cogent Core. All rights reserved. +// Use of this source code is governed by a BSD-style +// license that can be found in the LICENSE file. + +package physics + +//gosl:start + +// InitDynamics copies Body initial state to dynamic state. +func InitDynamics(i uint32) { //gosl:kernel + pars := GetParams(0) + ii := int32(i) + if ii >= pars.DynamicsN { + return + } + bi := DynamicIndex(ii) + Dynamics[ii, PosX] = Bodies[bi, BodyPosX] + Dynamics[ii, PosY] = Bodies[bi, BodyPosY] + Dynamics[ii, PosZ] = Bodies[bi, BodyPosZ] + + Dynamics[ii, RotX] = Bodies[bi, BodyRotX] + Dynamics[ii, RotY] = Bodies[bi, BodyRotY] + Dynamics[ii, RotZ] = Bodies[bi, BodyRotZ] + Dynamics[ii, RotW] = Bodies[bi, BodyRotW] + + for v := VelX; v < DynamicVarsN; v++ { + Dynamics[ii, v] = 0.0 + } +} + +// StepJoints does joint-based update. +func StepJoints(i uint32) { //gosl:kernel + pars := GetParams(0) + ji := int32(i) + if ji >= pars.JointsN { + return + } + ba := JointAIndex(ji) + bb := JointBIndex(ji) + jt := GetJointType(ji) + switch jt { + case Glue: + GlueStep(ji, ba, bb) + } +} + +// Step is dynamic update step kernel. i = body +func Step(i uint32) { //gosl:kernel + pars := GetParams(0) + ii := int32(i) + if ii >= pars.DynamicsN { + return + } + Dynamics[ii, PosX] += pars.Step * Dynamics[ii, VelX] + Dynamics[ii, PosY] += pars.Step * Dynamics[ii, VelY] + Dynamics[ii, PosZ] += pars.Step * Dynamics[ii, VelZ] + + // todo: force, integrated etc. +} + +//gosl:end + +func (wl *World) Step() { + pars := GetParams(0) + RunStep(int(pars.DynamicsN)) +} diff --git a/physics/typegen.go b/physics/typegen.go index c21ef0dd..c6772a9c 100644 --- a/physics/typegen.go +++ b/physics/typegen.go @@ -1,140 +1,29 @@ -// Code generated by "core generate -add-types"; DO NOT EDIT. +// Code generated by "core generate -add-types -gosl"; DO NOT EDIT. package physics import ( - "cogentcore.org/core/math32" - "cogentcore.org/core/tree" "cogentcore.org/core/types" ) var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.BBox", IDName: "b-box", Doc: "BBox contains bounding box and other gross object properties", Fields: []types.Field{{Name: "BBox", Doc: "bounding box in world coords (Axis-Aligned Bounding Box = AABB)"}, {Name: "VelBBox", Doc: "velocity-projected bounding box in world coords: extend BBox to include future position of moving bodies -- collision must be made on this basis"}, {Name: "BSphere", Doc: "bounding sphere in local coords"}, {Name: "Area", Doc: "area"}, {Name: "Volume", Doc: "volume"}}}) -var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.Body", IDName: "body", Doc: "Body is the common interface for all body types", Methods: []types.Method{{Name: "AsBodyBase", Doc: "AsBodyBase returns the body as a BodyBase", Returns: []string{"BodyBase"}}}}) +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.BodyVars", IDName: "body-vars", Doc: "BodyVars are body state variables stored in tensor.Float32"}) -var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.BodyBase", IDName: "body-base", Doc: "BodyBase is the base type for all specific Body types", Embeds: []types.Field{{Name: "NodeBase"}}, Fields: []types.Field{{Name: "Rigid", Doc: "rigid body properties, including mass, bounce, friction etc."}, {Name: "Color", Doc: "default color of body."}}}) +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.DynamicVars", IDName: "dynamic-vars", Doc: "DynamicVars are dynamic body variables stored in tensor.Float32."}) -// NewBodyBase returns a new [BodyBase] with the given optional parent: -// BodyBase is the base type for all specific Body types -func NewBodyBase(parent ...tree.Node) *BodyBase { return tree.New[BodyBase](parent...) } +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.ContactVars", IDName: "contact-vars", Doc: "Contact is one pairwise point of contact between two bodies.\nContacts are represented in spherical terms relative to the\nspherical BBox of A and B."}) -// SetRigid sets the [BodyBase.Rigid]: -// rigid body properties, including mass, bounce, friction etc. -func (t *BodyBase) SetRigid(v Rigid) *BodyBase { t.Rigid = v; return t } +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.GPUVars", IDName: "gpu-vars", Doc: "GPUVars is an enum for GPU variables, for specifying what to sync."}) -// SetColor sets the [BodyBase.Color]: -// default color of body. -func (t *BodyBase) SetColor(v string) *BodyBase { t.Color = v; return t } +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.JointVars", IDName: "joint-vars", Doc: "JointVars are joint state variables stored in tensor.Float32"}) -var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.Box", IDName: "box", Doc: "Box is a box body shape", Embeds: []types.Field{{Name: "BodyBase"}}, Fields: []types.Field{{Name: "Size", Doc: "size of box in each dimension (units arbitrary, as long as they are all consistent -- meters is typical)"}}}) +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.JointTypes", IDName: "joint-types", Doc: "JointTypes are joint types that determine nature of interaction."}) -// NewBox returns a new [Box] with the given optional parent: -// Box is a box body shape -func NewBox(parent ...tree.Node) *Box { return tree.New[Box](parent...) } +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.PhysParams", IDName: "phys-params", Doc: "PhysParams are the physics parameters", Directives: []types.Directive{{Tool: "gosl", Directive: "start"}}, Fields: []types.Field{{Name: "DynamicsN", Doc: "DynamicsN is number of dynamics bodies."}, {Name: "JointsN", Doc: "JointsN is number of joints."}, {Name: "Step", Doc: "Step is the global stepsize for numerical integration."}, {Name: "Gravity", Doc: "Gravity is the force of gravity."}, {Name: "GravityDir", Doc: "GravityDir is the direction of Gravity."}}}) -// SetSize sets the [Box.Size]: -// size of box in each dimension (units arbitrary, as long as they are all consistent -- meters is typical) -func (t *Box) SetSize(v math32.Vector3) *Box { t.Size = v; return t } +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.Shapes", IDName: "shapes", Doc: "Shapes are elemental shapes for rigid bodies."}) -var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.Capsule", IDName: "capsule", Doc: "Capsule is a generalized cylinder body shape, with hemispheres at each end,\nwith separate radii for top and bottom.", Embeds: []types.Field{{Name: "BodyBase"}}, Fields: []types.Field{{Name: "Height", Doc: "height of the cylinder portion of the capsule"}, {Name: "TopRad", Doc: "radius of the top hemisphere"}, {Name: "BotRad", Doc: "radius of the bottom hemisphere"}}}) +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.State", IDName: "state", Doc: "State contains the basic physical state including position, orientation, velocity.\nThese are only the values that can be either relative or absolute -- other physical\nstate values such as Mass should go in Rigid.", Methods: []types.Method{{Name: "MoveOnAxis", Doc: "MoveOnAxis moves (translates) the specified distance on the specified local axis,\nrelative to the current rotation orientation.\nThe axis is normalized prior to aplying the distance factor.\nSets the LinVel to motion vector.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z", "dist"}}, {Name: "MoveOnAxisAbs", Doc: "MoveOnAxisAbs moves (translates) the specified distance on the specified local axis,\nin absolute X,Y,Z coordinates (does not apply the Quat rotation factor.\nThe axis is normalized prior to aplying the distance factor.\nSets the LinVel to motion vector.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z", "dist"}}, {Name: "SetEulerRotation", Doc: "SetEulerRotation sets the rotation in Euler angles (degrees).", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z"}}, {Name: "EulerRotation", Doc: "EulerRotation returns the current rotation in Euler angles (degrees).", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Returns: []string{"Vector3"}}, {Name: "SetAxisRotation", Doc: "SetAxisRotation sets rotation from local axis and angle in degrees.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z", "angle"}}, {Name: "RotateOnAxis", Doc: "RotateOnAxis rotates around the specified local axis the specified angle in degrees.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z", "angle"}}, {Name: "RotateEuler", Doc: "RotateEuler rotates by given Euler angles (in degrees) relative to existing rotation.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z"}}}, Fields: []types.Field{{Name: "Pos", Doc: "Pos is the position of center of mass of object."}, {Name: "Quat", Doc: "Quat is the rotation specified as a quaternion."}, {Name: "LinVel", Doc: "LinVel is the linear velocity."}, {Name: "AngVel", Doc: "AngVel is the angular velocity."}}}) -// NewCapsule returns a new [Capsule] with the given optional parent: -// Capsule is a generalized cylinder body shape, with hemispheres at each end, -// with separate radii for top and bottom. -func NewCapsule(parent ...tree.Node) *Capsule { return tree.New[Capsule](parent...) } - -// SetHeight sets the [Capsule.Height]: -// height of the cylinder portion of the capsule -func (t *Capsule) SetHeight(v float32) *Capsule { t.Height = v; return t } - -// SetTopRad sets the [Capsule.TopRad]: -// radius of the top hemisphere -func (t *Capsule) SetTopRad(v float32) *Capsule { t.TopRad = v; return t } - -// SetBotRad sets the [Capsule.BotRad]: -// radius of the bottom hemisphere -func (t *Capsule) SetBotRad(v float32) *Capsule { t.BotRad = v; return t } - -var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.Contact", IDName: "contact", Doc: "Contact is one pairwise point of contact between two bodies.\nContacts are represented in spherical terms relative to the\nspherical BBox of A and B.", Fields: []types.Field{{Name: "A", Doc: "one body"}, {Name: "B", Doc: "the other body"}, {Name: "NormB", Doc: "normal pointing from center of B to center of A"}, {Name: "PtB", Doc: "point on spherical shell of B where A is contacting"}, {Name: "Dist", Doc: "distance from PtB along NormB to contact point on spherical shell of A"}}}) - -var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.Contacts", IDName: "contacts", Doc: "Contacts is a slice list of contacts"}) - -var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.Cylinder", IDName: "cylinder", Doc: "Cylinder is a generalized cylinder body shape, with separate radii for top and bottom.\nA cone has a zero radius at one end.", Embeds: []types.Field{{Name: "BodyBase"}}, Fields: []types.Field{{Name: "Height", Doc: "height of the cylinder"}, {Name: "TopRad", Doc: "radius of the top -- set to 0 for a cone"}, {Name: "BotRad", Doc: "radius of the bottom"}}}) - -// NewCylinder returns a new [Cylinder] with the given optional parent: -// Cylinder is a generalized cylinder body shape, with separate radii for top and bottom. -// A cone has a zero radius at one end. -func NewCylinder(parent ...tree.Node) *Cylinder { return tree.New[Cylinder](parent...) } - -// SetHeight sets the [Cylinder.Height]: -// height of the cylinder -func (t *Cylinder) SetHeight(v float32) *Cylinder { t.Height = v; return t } - -// SetTopRad sets the [Cylinder.TopRad]: -// radius of the top -- set to 0 for a cone -func (t *Cylinder) SetTopRad(v float32) *Cylinder { t.TopRad = v; return t } - -// SetBotRad sets the [Cylinder.BotRad]: -// radius of the bottom -func (t *Cylinder) SetBotRad(v float32) *Cylinder { t.BotRad = v; return t } - -var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.Group", IDName: "group", Doc: "Group is a container of bodies, joints, or other groups\nit should be used strategically to partition the space\nand its BBox is used to optimize tree-based collision detection.\nUse a group for the top-level World node as well.", Embeds: []types.Field{{Name: "NodeBase"}}}) - -// NewGroup returns a new [Group] with the given optional parent: -// Group is a container of bodies, joints, or other groups -// it should be used strategically to partition the space -// and its BBox is used to optimize tree-based collision detection. -// Use a group for the top-level World node as well. -func NewGroup(parent ...tree.Node) *Group { return tree.New[Group](parent...) } - -var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.BodyPoint", IDName: "body-point", Doc: "BodyPoint contains a Body and a Point on that body", Fields: []types.Field{{Name: "Body"}, {Name: "Point"}}}) - -var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.Node", IDName: "node", Doc: "Node is the common interface for all nodes.", Methods: []types.Method{{Name: "AsNodeBase", Doc: "AsNodeBase returns a generic NodeBase for our node -- gives generic\naccess to all the base-level data structures without needing interface methods.", Returns: []string{"NodeBase"}}, {Name: "AsBody", Doc: "AsBody returns a generic Body interface for our node -- nil if not a Body", Returns: []string{"Body"}}, {Name: "GroupBBox", Doc: "GroupBBox sets bounding boxes for groups based on groups or bodies.\ncalled in a FuncDownMeLast traversal."}, {Name: "InitAbs", Doc: "InitAbs sets current Abs physical state parameters from Initial values\nwhich are local, relative to parent -- is passed the parent (nil = top).\nBody nodes should also set their bounding boxes.\nCalled in a FuncDownMeFirst traversal.", Args: []string{"par"}}, {Name: "RelToAbs", Doc: "RelToAbs updates current world Abs physical state parameters\nbased on Rel values added to updated Abs values at higher levels.\nAbs.LinVel is updated from the resulting change from prior position.\nThis is useful for manual updating of relative positions (scripted movement).\nIt is passed the parent (nil = top).\nBody nodes should also update their bounding boxes.\nCalled in a FuncDownMeFirst traversal.", Args: []string{"par"}}, {Name: "Step", Doc: "Step computes one update of the world Abs physical state parameters,\nusing *current* velocities -- add forces prior to calling.\nUse this for physics-based state updates.\nBody nodes should also update their bounding boxes.", Args: []string{"step"}}, {Name: "Update", Doc: "Update does [tree] updating to dynamically update nodes / tree config."}}}) - -var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.NodeBase", IDName: "node-base", Doc: "NodeBase is the basic node, which has position, rotation, velocity\nand computed bounding boxes, etc.\nThere are only three different kinds of Nodes: Group, Body, and Joint", Embeds: []types.Field{{Name: "NodeBase"}}, Fields: []types.Field{{Name: "Dynamic", Doc: "Dynamic is whether this node can move. If it is false, then this is a Static node.\nAny top-level group that is not Dynamic is immediately pruned from further consideration,\nso top-level groups should be separated into Dynamic and Static nodes at the start."}, {Name: "Initial", Doc: "initial position, orientation, velocity in *local* coordinates (relative to parent)"}, {Name: "Rel", Doc: "current relative (local) position, orientation, velocity -- only change these values, as abs values are computed therefrom"}, {Name: "Abs", Doc: "current absolute (world) position, orientation, velocity"}, {Name: "BBox", Doc: "bounding box in world coordinates (aggregated for groups)"}, {Name: "NewView", Doc: "NewView is a function that returns a new [xyz.Node]\nto represent this node. If nil, Groups make Groups,\nand bodies make corresponding Solid shape."}, {Name: "InitView", Doc: "InitView is a function that initializes a new [xyz.Node]\nthat represents this physics node. If nil, Groups make Group children,\nand bodies configure corresponding Solid shape."}, {Name: "View", Doc: "View is the current view node for this node, set when made."}}}) - -// NewNodeBase returns a new [NodeBase] with the given optional parent: -// NodeBase is the basic node, which has position, rotation, velocity -// and computed bounding boxes, etc. -// There are only three different kinds of Nodes: Group, Body, and Joint -func NewNodeBase(parent ...tree.Node) *NodeBase { return tree.New[NodeBase](parent...) } - -// SetDynamic sets the [NodeBase.Dynamic]: -// Dynamic is whether this node can move. If it is false, then this is a Static node. -// Any top-level group that is not Dynamic is immediately pruned from further consideration, -// so top-level groups should be separated into Dynamic and Static nodes at the start. -func (t *NodeBase) SetDynamic(v bool) *NodeBase { t.Dynamic = v; return t } - -// SetInitial sets the [NodeBase.Initial]: -// initial position, orientation, velocity in *local* coordinates (relative to parent) -func (t *NodeBase) SetInitial(v State) *NodeBase { t.Initial = v; return t } - -// SetRel sets the [NodeBase.Rel]: -// current relative (local) position, orientation, velocity -- only change these values, as abs values are computed therefrom -func (t *NodeBase) SetRel(v State) *NodeBase { t.Rel = v; return t } - -// SetNewView sets the [NodeBase.NewView]: -// NewView is a function that returns a new [xyz.Node] -// to represent this node. If nil, Groups make Groups, -// and bodies make corresponding Solid shape. -func (t *NodeBase) SetNewView(v func() tree.Node) *NodeBase { t.NewView = v; return t } - -// SetInitView sets the [NodeBase.InitView]: -// InitView is a function that initializes a new [xyz.Node] -// that represents this physics node. If nil, Groups make Group children, -// and bodies configure corresponding Solid shape. -func (t *NodeBase) SetInitView(v func(n tree.Node)) *NodeBase { t.InitView = v; return t } - -var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.Rigid", IDName: "rigid", Doc: "Rigid contains the full specification of a given object's basic physics\nproperties including position, orientation, velocity. These", Fields: []types.Field{{Name: "InvMass", Doc: "1/mass -- 0 for no mass"}, {Name: "Bounce", Doc: "COR or coefficient of restitution -- how elastic is the collision i.e., final velocity / initial velocity"}, {Name: "Friction", Doc: "friction coefficient -- how much friction is generated by transverse motion"}, {Name: "Force", Doc: "record of computed force vector from last iteration"}, {Name: "RotInertia", Doc: "Last calculated rotational inertia matrix in local coords"}}}) - -var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.Sphere", IDName: "sphere", Doc: "Sphere is a spherical body shape.", Embeds: []types.Field{{Name: "BodyBase"}}, Fields: []types.Field{{Name: "Radius", Doc: "radius"}}}) - -// NewSphere returns a new [Sphere] with the given optional parent: -// Sphere is a spherical body shape. -func NewSphere(parent ...tree.Node) *Sphere { return tree.New[Sphere](parent...) } - -// SetRadius sets the [Sphere.Radius]: -// radius -func (t *Sphere) SetRadius(v float32) *Sphere { t.Radius = v; return t } - -var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.State", IDName: "state", Doc: "State contains the basic physical state including position, orientation, velocity.\nThese are only the values that can be either relative or absolute -- other physical\nstate values such as Mass should go in Rigid.", Methods: []types.Method{{Name: "MoveOnAxis", Doc: "MoveOnAxis moves (translates) the specified distance on the specified local axis,\nrelative to the current rotation orientation.\nThe axis is normalized prior to aplying the distance factor.\nSets the LinVel to motion vector.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z", "dist"}}, {Name: "MoveOnAxisAbs", Doc: "MoveOnAxisAbs moves (translates) the specified distance on the specified local axis,\nin absolute X,Y,Z coordinates (does not apply the Quat rotation factor.\nThe axis is normalized prior to aplying the distance factor.\nSets the LinVel to motion vector.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z", "dist"}}, {Name: "SetEulerRotation", Doc: "SetEulerRotation sets the rotation in Euler angles (degrees).", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z"}}, {Name: "EulerRotation", Doc: "EulerRotation returns the current rotation in Euler angles (degrees).", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Returns: []string{"Vector3"}}, {Name: "SetAxisRotation", Doc: "SetAxisRotation sets rotation from local axis and angle in degrees.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z", "angle"}}, {Name: "RotateOnAxis", Doc: "RotateOnAxis rotates around the specified local axis the specified angle in degrees.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z", "angle"}}, {Name: "RotateEuler", Doc: "RotateEuler rotates by given Euler angles (in degrees) relative to existing rotation.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z"}}}, Fields: []types.Field{{Name: "Pos", Doc: "position of center of mass of object"}, {Name: "Quat", Doc: "rotation specified as a Quat"}, {Name: "LinVel", Doc: "linear velocity"}, {Name: "AngVel", Doc: "angular velocity"}}}) +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.World", IDName: "world", Doc: "World contains and manages all of the physics elements.", Directives: []types.Directive{{Tool: "go", Directive: "generate", Args: []string{"core", "generate", "-add-types", "-gosl"}}}, Fields: []types.Field{{Name: "GPU", Doc: "GPU determines whether to use GPU (else CPU)."}, {Name: "Params", Doc: "Params are global parameters."}, {Name: "Bodies", Doc: "Bodies are the rigid body elements (dynamic and static),\nspecifying the constant, non-dynamic properties,\nwhich is initial state for dynamics.\n[body][BodyVarsN]"}, {Name: "Dynamics", Doc: "Dynamics are the dynamic rigid body elements: these actually move.\nThe first set of variables are for initial values, and the second current.\n[body][DynamicVarsN]"}, {Name: "Joints", Doc: "Joints is a list of permanent joints connecting bodies.\n[joint][JointVarsN]"}, {Name: "Contacts", Doc: "Contacts are points of contact between bodies.\n[contact][ContactVarsN]"}}}) diff --git a/physics/vars.go b/physics/vars.go new file mode 100644 index 00000000..a7347446 --- /dev/null +++ b/physics/vars.go @@ -0,0 +1,45 @@ +// Copyright (c) 2025, Cogent Core. All rights reserved. +// Use of this source code is governed by a BSD-style +// license that can be found in the LICENSE file. + +package physics + +import "cogentcore.org/lab/tensor" + +//go:generate gosl -exclude=Update,Defaults,ShouldDisplay -max-buffer-size=2147483616 + +//gosl:start + +// vars are all the global vars for axon GPU / CPU computation. +// +//gosl:vars +var ( + // Params are global parameters. + //gosl:group Params + //gosl:read-only + Params []PhysParams + + // Bodies are the rigid body elements (dynamic and static), + // specifying the constant, non-dynamic properties, + // which is initial state for dynamics. + // [body][BodyVarsN] + //gosl:group Bodies + //gosl:dims 2 + Bodies *tensor.Float32 + + // Dynamics are the dynamic rigid body elements: these actually move. + // [body][DynamicVarsN] + //gosl:dims 2 + Dynamics *tensor.Float32 + + // Joints is a list of permanent joints connecting bodies. + // [joint][JointVars] + //gosl:dims 2 + Joints *tensor.Float32 + + // Contacts are points of contact between bodies. + // [contact][ContactVarsN] + //gosl:group Contacts + //gosl:dims 2 + Contacts *tensor.Float32 +) diff --git a/physics/world.go b/physics/world.go new file mode 100644 index 00000000..363338c8 --- /dev/null +++ b/physics/world.go @@ -0,0 +1,131 @@ +// Code generated by "goal build"; DO NOT EDIT. +//line world.goal:1 +// Copyright (c) 2025, Cogent Core. All rights reserved. +// Use of this source code is governed by a BSD-style +// license that can be found in the LICENSE file. + +package physics + +import ( + "cogentcore.org/core/math32" + "cogentcore.org/lab/tensor" +) + +//go:generate core generate -add-types -gosl + +// World contains and manages all of the physics elements. +type World struct { + // GPU determines whether to use GPU (else CPU). + GPU bool + + // Params are global parameters. + Params []PhysParams + + // Bodies are the rigid body elements (dynamic and static), + // specifying the constant, non-dynamic properties, + // which is initial state for dynamics. + // [body][BodyVarsN] + Bodies *tensor.Float32 + + // Dynamics are the dynamic rigid body elements: these actually move. + // The first set of variables are for initial values, and the second current. + // [body][DynamicVarsN] + Dynamics *tensor.Float32 + + // Joints is a list of permanent joints connecting bodies. + // [joint][JointVarsN] + Joints *tensor.Float32 + + // Contacts are points of contact between bodies. + // [contact][ContactVarsN] + Contacts *tensor.Float32 +} + +func NewWorld() *World { + wl := &World{} + wl.Init() + return wl +} + +// Init makes initial vars. +func (wl *World) Init() { + wl.Params = []PhysParams{} + wl.Bodies = tensor.NewFloat32(0, int(BodyVarsN)) + wl.Dynamics = tensor.NewFloat32(0, int(DynamicVarsN)) + wl.Joints = tensor.NewFloat32(0, int(JointVarsN)) + wl.Contacts = tensor.NewFloat32(0, int(ContactVarsN)) + wl.SetAsCurrentVars() +} + +// NewBody adds a new body with given parameters. Returns the index. +// Use this for Static elements; NewDynamic for dynamic elements. +func (wl *World) NewBody(shape Shapes, size, pos math32.Vector3, rot math32.Quat) int32 { + sizes := wl.Bodies.ShapeSizes() + n := int32(sizes[0]) + wl.Bodies.SetShapeSizes(int(n+1), int(BodyVarsN)) + SetBodyShape(n, shape) + SetBodySize(n, size) + SetBodyPos(n, pos) + SetBodyRot(n, rot) + return n +} + +// NewDynamic adds a new dynamic body with given parameters. Returns the index. +func (wl *World) NewDynamic(shape Shapes, size, pos math32.Vector3, rot math32.Quat) (bodyIdx, dynIdx int32) { + bodyIdx = wl.NewBody(shape, size, pos, rot) + sizes := wl.Dynamics.ShapeSizes() + dynIdx = int32(sizes[0]) + wl.Dynamics.SetShapeSizes(int(dynIdx+1), int(DynamicVarsN)) + SetDynamicIndex(dynIdx, bodyIdx) + wl.Params[0].DynamicsN = dynIdx + 1 + return +} + +// NewJoint adds a new joint between two objects. +func (wl *World) NewJoint(joint JointTypes, bodyA, bodyB int32, pos math32.Vector3) int32 { + sizes := wl.Joints.ShapeSizes() + idx := int32(sizes[0]) + wl.Joints.SetShapeSizes(int(idx+1), int(JointVarsN)) + SetJointA(idx, bodyA) + SetJointB(idx, bodyB) + SetJointPos(idx, pos) + wl.Params[0].JointsN = idx + 1 + return idx +} + +// SetAsCurrent sets these as the current global values that are +// processed in the code (on the GPU). If this was not the setter of +// the current variables, then the parameter variables are copied up +// to the GPU. +func (wl *World) SetAsCurrent() { + isCur := (Bodies == wl.Bodies) + wl.SetAsCurrentVars() + if GPUInitialized && !isCur { + wl.ToGPUInfra() + } +} + +// SetAsCurrentVars sets these as the current global values that are +// processed in the code (on the GPU). +func (wl *World) SetAsCurrentVars() { + Params = wl.Params + Bodies = wl.Bodies + Dynamics = wl.Dynamics + Joints = wl.Joints + Contacts = wl.Contacts +} + +// GPUInit initializes the GPU and transfers Infra. +// Should have already called SetAsCurrent (needed for CPU and GPU). +func (wl *World) GPUInit() { + GPUInit() + UseGPU = wl.GPU + wl.ToGPUInfra() +} + +// ToGPUInfra copies all the infrastructure for these filters up to +// the GPU. This is done in GPUInit, and +func (wl *World) ToGPUInfra() { + ToGPUTensorStrides() + ToGPU(ParamsVar) +} diff --git a/physics/world.goal b/physics/world.goal new file mode 100644 index 00000000..b5019d56 --- /dev/null +++ b/physics/world.goal @@ -0,0 +1,129 @@ +// Copyright (c) 2025, Cogent Core. All rights reserved. +// Use of this source code is governed by a BSD-style +// license that can be found in the LICENSE file. + +package physics + +import ( + "cogentcore.org/core/math32" + "cogentcore.org/lab/tensor" +) + +//go:generate core generate -add-types -gosl + +// World contains and manages all of the physics elements. +type World struct { + // GPU determines whether to use GPU (else CPU). + GPU bool + + // Params are global parameters. + Params []PhysParams + + // Bodies are the rigid body elements (dynamic and static), + // specifying the constant, non-dynamic properties, + // which is initial state for dynamics. + // [body][BodyVarsN] + Bodies *tensor.Float32 + + // Dynamics are the dynamic rigid body elements: these actually move. + // The first set of variables are for initial values, and the second current. + // [body][DynamicVarsN] + Dynamics *tensor.Float32 + + // Joints is a list of permanent joints connecting bodies. + // [joint][JointVarsN] + Joints *tensor.Float32 + + // Contacts are points of contact between bodies. + // [contact][ContactVarsN] + Contacts *tensor.Float32 +} + +func NewWorld() *World { + wl := &World{} + wl.Init() + return wl +} + +// Init makes initial vars. +func (wl *World) Init() { + wl.Params = []PhysParams{} + wl.Bodies = tensor.NewFloat32(0, int(BodyVarsN)) + wl.Dynamics = tensor.NewFloat32(0, int(DynamicVarsN)) + wl.Joints = tensor.NewFloat32(0, int(JointVarsN)) + wl.Contacts = tensor.NewFloat32(0, int(ContactVarsN)) + wl.SetAsCurrentVars() +} + +// NewBody adds a new body with given parameters. Returns the index. +// Use this for Static elements; NewDynamic for dynamic elements. +func (wl *World) NewBody(shape Shapes, size, pos math32.Vector3, rot math32.Quat) int32 { + sizes := wl.Bodies.ShapeSizes() + n := int32(sizes[0]) + wl.Bodies.SetShapeSizes(int(n+1), int(BodyVarsN)) + SetBodyShape(n, shape) + SetBodySize(n, size) + SetBodyPos(n, pos) + SetBodyRot(n, rot) + return n +} + +// NewDynamic adds a new dynamic body with given parameters. Returns the index. +func (wl *World) NewDynamic(shape Shapes, size, pos math32.Vector3, rot math32.Quat) (bodyIdx, dynIdx int32) { + bodyIdx = wl.NewBody(shape, size, pos, rot) + sizes := wl.Dynamics.ShapeSizes() + dynIdx = int32(sizes[0]) + wl.Dynamics.SetShapeSizes(int(dynIdx+1), int(DynamicVarsN)) + SetDynamicIndex(dynIdx, bodyIdx) + wl.Params[0].DynamicsN = dynIdx + 1 + return +} + +// NewJoint adds a new joint between two objects. +func (wl *World) NewJoint(joint JointTypes, bodyA, bodyB int32, pos math32.Vector3) int32 { + sizes := wl.Joints.ShapeSizes() + idx := int32(sizes[0]) + wl.Joints.SetShapeSizes(int(idx+1), int(JointVarsN)) + SetJointA(idx, bodyA) + SetJointB(idx, bodyB) + SetJointPos(idx, pos) + wl.Params[0].JointsN = idx + 1 + return idx +} + +// SetAsCurrent sets these as the current global values that are +// processed in the code (on the GPU). If this was not the setter of +// the current variables, then the parameter variables are copied up +// to the GPU. +func (wl *World) SetAsCurrent() { + isCur := (Bodies == wl.Bodies) + wl.SetAsCurrentVars() + if GPUInitialized && !isCur { + wl.ToGPUInfra() + } +} + +// SetAsCurrentVars sets these as the current global values that are +// processed in the code (on the GPU). +func (wl *World) SetAsCurrentVars() { + Params = wl.Params + Bodies = wl.Bodies + Dynamics = wl.Dynamics + Joints = wl.Joints + Contacts = wl.Contacts +} + +// GPUInit initializes the GPU and transfers Infra. +// Should have already called SetAsCurrent (needed for CPU and GPU). +func (wl *World) GPUInit() { + GPUInit() + UseGPU = wl.GPU + wl.ToGPUInfra() +} + +// ToGPUInfra copies all the infrastructure for these filters up to +// the GPU. This is done in GPUInit, and +func (wl *World) ToGPUInfra() { + ToGPUTensorStrides() + ToGPU(ParamsVar) +} diff --git a/physics/world/update.go b/physics/world/update.go deleted file mode 100644 index ef693f7f..00000000 --- a/physics/world/update.go +++ /dev/null @@ -1,151 +0,0 @@ -// Copyright (c) 2019, Cogent Core. All rights reserved. -// Use of this source code is governed by a BSD-style -// license that can be found in the LICENSE file. - -package world - -import ( - "cogentcore.org/core/base/errors" - "cogentcore.org/core/colors" - "cogentcore.org/core/tree" - "cogentcore.org/core/xyz" - "cogentcore.org/lab/physics" -) - -// todo: add type that manages a world, view etc -- no need to replicate that -// and change name of world package to something better -- confusing, although -// the new type will be world so actually that is ok. - -// Add adds given physics node to the [tree.Plan], using NewView -// function on the node, or default. -func Add(p *tree.Plan, nb *physics.NodeBase) { - newFunc := nb.NewView - if newFunc == nil { - if _, ok := nb.This.(*physics.Group); ok { - newFunc = func() tree.Node { - return any(tree.New[xyz.Group]()).(tree.Node) - } - } else if _, ok := nb.This.(physics.Body); ok { - newFunc = func() tree.Node { - return any(tree.New[xyz.Solid]()).(tree.Node) - } - } // todo: joint - } - p.Add(nb.Name, newFunc, func(n tree.Node) { Init(nb, n) }) -} - -// Init is the physics node initialization function, -// which calls InitView if set on the node, or the default. -func Init(nb *physics.NodeBase, n tree.Node) { - initFunc := nb.InitView - if initFunc != nil { - initFunc(n) - return - } - switch x := nb.This.(type) { - case *physics.Group: - GroupInit(x, n.(*xyz.Group)) - case *physics.Box: - BoxInit(x, n.(*xyz.Solid)) - case *physics.Cylinder: - CylinderInit(x, n.(*xyz.Solid)) - case *physics.Capsule: - CapsuleInit(x, n.(*xyz.Solid)) - case *physics.Sphere: - SphereInit(x, n.(*xyz.Solid)) - } -} - -// GroupInit is the default InitView function for groups. -func GroupInit(gp *physics.Group, vgp *xyz.Group) { - gp.View = vgp.This - vgp.Maker(func(p *tree.Plan) { - for _, c := range gp.Children { - Add(p, c.(physics.Node).AsNodeBase()) - } - }) - vgp.Updater(func() { - UpdatePose(gp.AsNodeBase(), vgp.AsNodeBase()) - }) -} - -// UpdatePose updates the view node pose from physics node state. -func UpdatePose(nd *physics.NodeBase, vn *xyz.NodeBase) { - vn.Pose.Pos = nd.Rel.Pos - vn.Pose.Quat = nd.Rel.Quat -} - -// UpdateColor updates the view color to given color. -func UpdateColor(clr string, sld *xyz.Solid) { - if clr != "" { - sld.Material.Color = errors.Log1(colors.FromString(clr)) - } -} - -// BoxInit is the default InitView function for [physics.Box]. -// Only updates Pose in Updater: if node will change size or color, -// add updaters for that. -func BoxInit(bx *physics.Box, sld *xyz.Solid) { - bx.View = sld.This - mnm := "physics.Box" - if ms, _ := sld.Scene.MeshByName(mnm); ms == nil { - xyz.NewBox(sld.Scene, mnm, 1, 1, 1) - } - sld.SetMeshName(mnm) - sld.Pose.Scale = bx.Size - UpdateColor(bx.Color, sld) - sld.Updater(func() { - UpdatePose(bx.AsNodeBase(), sld.AsNodeBase()) - }) -} - -// CylinderInit is the default InitView function for [physics.Cylinder]. -// Only updates Pose in Updater: if node will change size or color, -// add updaters for that. -func CylinderInit(cy *physics.Cylinder, sld *xyz.Solid) { - cy.View = sld.This - mnm := "physics.Cylinder" - if ms, _ := sld.Scene.MeshByName(mnm); ms == nil { - xyz.NewCylinder(sld.Scene, mnm, 1, 1, 32, 1, true, true) - } - sld.SetMeshName(mnm) - sld.Pose.Scale.Set(cy.BotRad, cy.Height, cy.BotRad) - UpdateColor(cy.Color, sld) - sld.Updater(func() { - UpdatePose(cy.AsNodeBase(), sld.AsNodeBase()) - }) -} - -// CapsuleInit is the default InitView function for [physics.Capsule]. -// Only updates Pose in Updater: if node will change size or color, -// add updaters for that. -func CapsuleInit(cp *physics.Capsule, sld *xyz.Solid) { - cp.View = sld.This - mnm := "physics.Capsule" - if ms, _ := sld.Scene.MeshByName(mnm); ms == nil { - ms = xyz.NewCapsule(sld.Scene, mnm, 1, .2, 32, 1) - } - sld.SetMeshName(mnm) - sld.Pose.Scale.Set(cp.BotRad/.2, cp.Height/1.4, cp.BotRad/.2) - UpdateColor(cp.Color, sld) - sld.Updater(func() { - UpdatePose(cp.AsNodeBase(), sld.AsNodeBase()) - }) -} - -// SphereInit is the default InitView function for [physics.Sphere]. -// Only updates Pose in Updater: if node will change size or color, -// add updaters for that. -func SphereInit(sp *physics.Sphere, sld *xyz.Solid) { - sp.View = sld.This - mnm := "physics.Sphere" - if ms, _ := sld.Scene.MeshByName(mnm); ms == nil { - ms = xyz.NewSphere(sld.Scene, mnm, 1, 32) - } - sld.SetMeshName(mnm) - sld.Pose.Scale.SetScalar(sp.Radius) - UpdateColor(sp.Color, sld) - sld.Updater(func() { - UpdatePose(sp.AsNodeBase(), sld.AsNodeBase()) - }) -} diff --git a/physics/world/view.go b/physics/world/view.go new file mode 100644 index 00000000..d736a31e --- /dev/null +++ b/physics/world/view.go @@ -0,0 +1,193 @@ +// Copyright (c) 2025, Cogent Core. All rights reserved. +// Use of this source code is governed by a BSD-style +// license that can be found in the LICENSE file. + +package world + +import ( + "strconv" + + "cogentcore.org/core/base/errors" + "cogentcore.org/core/colors" + "cogentcore.org/core/math32" + "cogentcore.org/core/tree" + "cogentcore.org/core/xyz" + "cogentcore.org/lab/physics" +) + +// View has visualization functions for physics elements. +type View struct { + // Name is a name for element (index always appended). + Name string + + // Shape is the physical shape of the element. + Shape physics.Shapes + + // Color is the color of the element. + Color string + + // Size is the size (per shape). + Size math32.Vector3 + + // Pos is the position. + Pos math32.Vector3 + + // Rot is the rotation as a quaternion. + Rot math32.Quat + + // NewView is a function that returns a new [xyz.Node] + // to represent this element. If nil, uses appropriate defaults. + NewView func() tree.Node + + // InitView is a function that initializes a new [xyz.Node] + // that represents this element. If nil, uses appropriate defaults. + InitView func(sld *xyz.Solid) + + // Index is the index of the element in a list. + Index int32 + + // DynamicIndex is the index of a dynamic element (-1 if not dynamic). + DynamicIndex int32 +} + +// NewBody adds a new body with given parameters. +// Returns the View which can then be further customized. +// Use this for Static elements; NewDynamic for dynamic elements. +func (wr *World) NewBody(wl *physics.World, name string, shape physics.Shapes, clr string, size, pos math32.Vector3, rot math32.Quat) *View { + idx := wl.NewBody(shape, size, pos, rot) + vw := &View{Name: name, Index: idx, DynamicIndex: -1, Shape: shape, Color: clr, Size: size, Pos: pos, Rot: rot} + wr.Views = append(wr.Views, vw) + return vw +} + +// NewDynamic adds a new dynamic body with given parameters. +// Returns the View which can then be further customized. +func (wr *World) NewDynamic(wl *physics.World, name string, shape physics.Shapes, clr string, size, pos math32.Vector3, rot math32.Quat) *View { + idx, dyIdx := wl.NewDynamic(shape, size, pos, rot) + vw := &View{Name: name, Index: idx, DynamicIndex: dyIdx, Shape: shape, Color: clr, Size: size, Pos: pos, Rot: rot} + wr.Views = append(wr.Views, vw) + return vw +} + +// UpdateFromPhysics updates the View from physics state. +func (vw *View) UpdateFromPhysics() { + if vw.DynamicIndex >= 0 { + ix := int32(vw.DynamicIndex) + vw.Pos = physics.DynamicPos(ix) + vw.Rot = physics.DynamicRot(ix) + } else { + ix := int32(vw.Index) + vw.Pos = physics.BodyPos(ix) + vw.Rot = physics.BodyRot(ix) + } +} + +// UpdatePose updates the xyz node pose from view. +func (vw *View) UpdatePose(sld *xyz.Solid) { + sld.Pose.Pos = vw.Pos + sld.Pose.Quat = vw.Rot +} + +// UpdateColor updates the xyz node color from view. +func (vw *View) UpdateColor(clr string, sld *xyz.Solid) { + if clr == "" { + return + } + sld.Material.Color = errors.Log1(colors.FromString(clr)) +} + +// Add adds given physics node to the [tree.Plan], using NewView +// function on the node, or default. +func (vw *View) Add(p *tree.Plan) { + nm := vw.Name + strconv.Itoa(int(vw.Index)) + newFunc := vw.NewView + if newFunc == nil { + newFunc = func() tree.Node { + return any(tree.New[xyz.Solid]()).(tree.Node) + } + } + p.Add(nm, newFunc, func(n tree.Node) { vw.Init(n.(*xyz.Solid)) }) +} + +// Init initializes xyz node using InitView function or default. +func (vw *View) Init(sld *xyz.Solid) { + initFunc := vw.InitView + if initFunc != nil { + initFunc(sld) + return + } + switch vw.Shape { + case physics.Box: + vw.BoxInit(sld) + case physics.Cylinder: + vw.CylinderInit(sld) + case physics.Capsule: + vw.CapsuleInit(sld) + case physics.Sphere: + vw.SphereInit(sld) + } +} + +// BoxInit is the default InitView function for [physics.Box]. +// Only updates Pose in Updater: if node will change size or color, +// add updaters for that. +func (vw *View) BoxInit(sld *xyz.Solid) { + mnm := "physics.Box" + if ms, _ := sld.Scene.MeshByName(mnm); ms == nil { + xyz.NewBox(sld.Scene, mnm, 1, 1, 1) + } + sld.SetMeshName(mnm) + sld.Pose.Scale = vw.Size + vw.UpdateColor(vw.Color, sld) + sld.Updater(func() { + vw.UpdatePose(sld) + }) +} + +// CylinderInit is the default InitView function for [physics.Cylinder]. +// Only updates Pose in Updater: if node will change size or color, +// add updaters for that. +func (vw *View) CylinderInit(sld *xyz.Solid) { + mnm := "physics.Cylinder" + if ms, _ := sld.Scene.MeshByName(mnm); ms == nil { + xyz.NewCylinder(sld.Scene, mnm, 1, 1, 32, 1, true, true) + } + sld.SetMeshName(mnm) + sld.Pose.Scale = vw.Size + vw.UpdateColor(vw.Color, sld) + sld.Updater(func() { + vw.UpdatePose(sld) + }) +} + +// CapsuleInit is the default InitView function for [physics.Capsule]. +// Only updates Pose in Updater: if node will change size or color, +// add updaters for that. +func (vw *View) CapsuleInit(sld *xyz.Solid) { + mnm := "physics.Capsule" + if ms, _ := sld.Scene.MeshByName(mnm); ms == nil { + ms = xyz.NewCapsule(sld.Scene, mnm, 1, .2, 32, 1) + } + sld.SetMeshName(mnm) + sld.Pose.Scale.Set(vw.Size.X/.2, vw.Size.Y/1.4, vw.Size.Z/.2) + vw.UpdateColor(vw.Color, sld) + sld.Updater(func() { + vw.UpdatePose(sld) + }) +} + +// SphereInit is the default InitView function for [physics.Sphere]. +// Only updates Pose in Updater: if node will change size or color, +// add updaters for that. +func (vw *View) SphereInit(sld *xyz.Solid) { + mnm := "physics.Sphere" + if ms, _ := sld.Scene.MeshByName(mnm); ms == nil { + ms = xyz.NewSphere(sld.Scene, mnm, 1, 32) + } + sld.SetMeshName(mnm) + sld.Pose.Scale.SetScalar(vw.Size.X) + vw.UpdateColor(vw.Color, sld) + sld.Updater(func() { + vw.UpdatePose(sld) + }) +} diff --git a/physics/world/world.go b/physics/world/world.go index 3812d2d7..b5861f68 100644 --- a/physics/world/world.go +++ b/physics/world/world.go @@ -18,49 +18,65 @@ import ( "cogentcore.org/lab/physics" ) -// World connects a Virtual World with an [xyz.Scene] to visualize the world, -// including ability to render offscreen. +// World displays a [physics.World] using a [xyz.Scene]. +// One World can be used for multiple different [physics.World]s which +// is more efficient when running multiple in parallel. +// Initial construction of the physics and visualization happens here. type World struct { - - // World is the root Group node of the virtual world - World *physics.Group - // Scene is the [xyz.Scene] object for visualizing. Scene *xyz.Scene // Root is the root Group node in the Scene under which the world is rendered. Root *xyz.Group + + // Views are the view elements for each body in [physics.World]. + Views []*View } -// NewWorld returns a new World that links given [physics] world -// (top level Group) with given [xyz.Scene], making a -// top-level Root group in the scene. -func NewWorld(world *physics.Group, sc *xyz.Scene) *World { +// NewWorld returns a new World for visualizing a [physics.World]. +// with given [xyz.Scene], making a top-level Root group in the scene. +func NewWorld(sc *xyz.Scene) *World { rgp := xyz.NewGroup(sc) rgp.SetName("world") - wr := &World{World: world, Scene: sc, Root: rgp} - GroupInit(wr.World, rgp) - wr.Update() + wr := &World{Scene: sc, Root: rgp} return wr } -// Init initializes the physics world, e.g., at start of a new sim run. -func (wr *World) Init() { - wr.World.WorldInit() - wr.Update() +// Init configures the visual world based on Views. +// Call this _once_ after making all the new Views and Bodies. +// (will return if already called). +func (wr *World) Init(wl *physics.World) { + if len(wr.Root.Makers.Normal) > 0 { + return + } + wr.Root.Maker(func(p *tree.Plan) { + for _, vw := range wr.Views { + vw.Add(p) + } + }) } -// Update updates the view from current physics node state. +// Update updates the xyz scene from current physics node state. +// (use physics.World.SetAsCurrent()). func (wr *World) Update() { + wr.UpdateFromPhysics() if wr.Scene != nil { wr.Scene.Update() } } -// RenderFromNode does an offscreen render using given node +// UpdateFromPhysics updates the World from currently active +// physics state (use physics.World.SetAsCurrent()). +func (wr *World) UpdateFromPhysics() { + for _, vw := range wr.Views { + vw.UpdateFromPhysics() + } +} + +// RenderFromView does an offscreen render using given [View] // for the camera position and orientation, returning the render image. // Current scene camera is saved and restored. -func (wr *World) RenderFromNode(node physics.Node, cam *Camera) image.Image { +func (wr *World) RenderFromNode(vw *View, cam *Camera) image.Image { sc := wr.Scene camnm := "physics-view-rendernode-save" sc.SaveCamera(camnm) @@ -72,9 +88,8 @@ func (wr *World) RenderFromNode(node physics.Node, cam *Camera) image.Image { sc.Camera.FOV = cam.FOV sc.Camera.Near = cam.Near sc.Camera.Far = cam.Far - nb := node.AsNodeBase() - sc.Camera.Pose.Pos = nb.Abs.Pos - sc.Camera.Pose.Quat = nb.Abs.Quat + sc.Camera.Pose.Pos = vw.Pos + sc.Camera.Pose.Quat = vw.Rot sc.Camera.Pose.Scale.Set(1, 1, 1) sc.UseAltFrame(cam.Size) From 19f1bd2ceea542ea03b1d25e6d02899f080fe291 Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Sat, 13 Dec 2025 08:28:37 +0100 Subject: [PATCH 11/97] physics: gosl handles math32.Vector basic math ops by converting to symbols, also V() of sltype conversion. --- gosl/gotosl/nodes.go | 46 +++++++++++++++++++++++++++++ gosl/gotosl/testdata/Compute.golden | 4 ++- gosl/gotosl/testdata/basic.go | 4 ++- gosl/gotosl/testdata/basic.goal | 4 ++- gosl/slvec/slvec.go | 33 +++++++++++++++++++-- physics/shaders/StepJoints.wgsl | 4 ++- physics/step.go | 1 + physics/step.goal | 1 + 8 files changed, 90 insertions(+), 7 deletions(-) diff --git a/gosl/gotosl/nodes.go b/gosl/gotosl/nodes.go index 0b3afc73..9d6ac465 100644 --- a/gosl/gotosl/nodes.go +++ b/gosl/gotosl/nodes.go @@ -1968,6 +1968,9 @@ func (p *printer) methodExpr(x *ast.CallExpr, depth int) { } } // fmt.Println(pathIsPackage, recvType, methName, recvPath) + if p.mathMeth(x, depth, methName, recvPath, recvType) { + return + } if pathIsPackage { if recvType == "atomic" || recvType == "atomicx" { p.curMethIsAtomic = true @@ -2016,6 +2019,49 @@ func (p *printer) methodExpr(x *ast.CallExpr, depth int) { p.assignRwArgs(rwargs) // gosl: assign temp var back to global var } +// gosl: process math methods into expressions: .Add() -> + and V() to get slvec type +func (p *printer) mathMeth(x *ast.CallExpr, depth int, methName, recvPath, recvType string) bool { + if strings.HasPrefix(recvType, "slvec.") && methName == "V" { + btyp := strings.TrimPrefix(recvType, "slvec.") + rtyp := "math32." + btyp + p.print(rtyp) + p.setPos(x.Lparen) + p.print(token.LPAREN) + switch btyp { + case "Vector2", "Vector2i": + p.print(recvPath+".x", token.COMMA, recvPath+".y") + case "Vector3": + p.print(recvPath+".x", token.COMMA, recvPath+".y", token.COMMA, recvPath+".z") + } + p.setPos(x.Rparen) + p.print(token.RPAREN) + p.curMethIsAtomic = false + return true + } + opr := token.ILLEGAL + switch methName { + case "Add": + opr = token.ADD + case "Sub": + opr = token.SUB + case "Mul": + opr = token.MUL + case "Div": + opr = token.QUO + } + if opr == token.ILLEGAL { + return false + } + p.print(recvPath, opr) + p.setPos(x.Lparen) + p.print(token.LPAREN) + p.exprList(x.Lparen, x.Args, depth, commaTerm, x.Rparen, false) + p.setPos(x.Rparen) + p.print(token.RPAREN) + p.curMethIsAtomic = false + return true +} + func (p *printer) expr0(x ast.Expr, depth int) { p.expr1(x, token.LowestPrec, depth) } diff --git a/gosl/gotosl/testdata/Compute.golden b/gosl/gotosl/testdata/Compute.golden index abc0f07d..ee73061d 100644 --- a/gosl/gotosl/testdata/Compute.golden +++ b/gosl/gotosl/testdata/Compute.golden @@ -168,10 +168,12 @@ fn ParamStruct_IntegFromRaw(ps: ParamStruct, idx: i32) -> f32 { Data[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(Integ))] = integ; Data[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(Exp))] = exp(-integ); var a = ps.VXYf.x; + var b = vec2(ps.VXYf.x,ps.VXYf.y); + var c = b*(b+(b)); // converted to direct ops let ctx = Ctx[0]; ParamStruct_AnotherMeth(ps, ctx, idx, &a); var bv = BigGet(Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(Integ))); - BigSet(bv * 2, Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(Exp)));return Data[Index2D(TensorStrides[0], TensorStrides[1], + BigSet(bv*2 + c.y, Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(Exp)));return Data[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(Exp))]; } fn ParamStruct_AnotherMeth(ps: ParamStruct, ctx: Context, idx: i32, ptrarg: ptr) { diff --git a/gosl/gotosl/testdata/basic.go b/gosl/gotosl/testdata/basic.go index 6ff432f4..7a68df5c 100644 --- a/gosl/gotosl/testdata/basic.go +++ b/gosl/gotosl/testdata/basic.go @@ -146,11 +146,13 @@ func (ps *ParamStruct) IntegFromRaw(idx int) float32 { Data.Set(math32.Exp(-integ), int(idx), int(Exp)) a := ps.VXYf.X + b := ps.VXYf.V() + c := b.Mul(b.Add(b)) // converted to direct ops ctx := GetCtx(0) ps.AnotherMeth(ctx, idx, &a) bv := Big.Value(int(idx), int(Integ)) - Big.Set(bv*2, int(idx), int(Exp)) + Big.Set(bv*2+c.Y, int(idx), int(Exp)) return Data.Value(int(idx), int(Exp)) } diff --git a/gosl/gotosl/testdata/basic.goal b/gosl/gotosl/testdata/basic.goal index b9890bed..43c63b7f 100644 --- a/gosl/gotosl/testdata/basic.goal +++ b/gosl/gotosl/testdata/basic.goal @@ -139,11 +139,13 @@ func (ps *ParamStruct) IntegFromRaw(idx int) float32 { Data[idx, Exp] = math32.Exp(-integ) a := ps.VXYf.X + b := ps.VXYf.V() + c := b.Mul(b.Add(b)) // converted to direct ops ctx := GetCtx(0) ps.AnotherMeth(ctx, idx, &a) bv := Big[idx, Integ] - Big[idx, Exp] = bv * 2 + Big[idx, Exp] = bv*2 + c.Y return Data[idx, Exp] } diff --git a/gosl/slvec/slvec.go b/gosl/slvec/slvec.go index 1171d107..26cc2ce6 100644 --- a/gosl/slvec/slvec.go +++ b/gosl/slvec/slvec.go @@ -8,11 +8,10 @@ import "cogentcore.org/core/math32" //gosl:start -// Vector2 is a 2D vector/point with X and Y components. +// Vector2 is a 2D vector/point with X and Y components, // with padding values so it works in a GPU struct. Use the // V() method to get a math32.Vector2 that supports standard -// math operations. Cannot use those math ops in gosl GPU -// code at this point, unfortunately. +// math operations, which are converted to direct ops in WGSL. type Vector2 struct { X float32 Y float32 @@ -60,4 +59,32 @@ func (v *Vector2i) SetV(mv math32.Vector2i) { v.Y = mv.Y } +// Vector3 is a 3DD vector/point with X, Y, Z components, +// with padding values so it works in a GPU struct. Use the +// V() method to get a math32.Vector3 that supports standard +// math operations, which are converted to direct ops in WGSL. +type Vector3 struct { + X float32 + Y float32 + Z float32 + + pad float32 +} + +func (v *Vector3) V() math32.Vector3 { + return math32.Vec3(v.X, v.Y, v.Z) +} + +func (v *Vector3) Set(x, y, z float32) { + v.X = x + v.Y = y + v.Z = z +} + +func (v *Vector3) SetV(mv math32.Vector3) { + v.X = mv.X + v.Y = mv.Y + v.Z = mv.Z +} + //gosl:end diff --git a/physics/shaders/StepJoints.wgsl b/physics/shaders/StepJoints.wgsl index a5cd56ca..ff1a1d47 100644 --- a/physics/shaders/StepJoints.wgsl +++ b/physics/shaders/StepJoints.wgsl @@ -132,7 +132,7 @@ const Ball: JointTypes = 1; fn GlueStep(ji: i32,ba: i32,bb: i32) { var pos = JointPos(ji); var bap = DynamicPos(ba); - var bbp = pos.Add(bap); + var bbp = pos+(bap); SetDynamicPos(bb, bbp); } @@ -166,5 +166,7 @@ fn StepJoints(i: u32) { //gosl:kernel case Glue: { GlueStep(ji, ba, bb); } + default: { + } } } \ No newline at end of file diff --git a/physics/step.go b/physics/step.go index 434f526d..85516cad 100644 --- a/physics/step.go +++ b/physics/step.go @@ -43,6 +43,7 @@ func StepJoints(i uint32) { //gosl:kernel switch jt { case Glue: GlueStep(ji, ba, bb) + default: } } diff --git a/physics/step.goal b/physics/step.goal index 5a838231..bd1a723f 100644 --- a/physics/step.goal +++ b/physics/step.goal @@ -41,6 +41,7 @@ func StepJoints(i uint32) { //gosl:kernel switch jt { case Glue: GlueStep(ji, ba, bb) + default: } } From 51a799e58b499b59ee7eeb7a5b0372687c884641 Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Sun, 14 Dec 2025 20:26:24 +0100 Subject: [PATCH 12/97] physics: gosl has slmath to define functions that operate on math32.Vector* and math32.Quat types, and it properly handles Quat types. also fixed key bug in not resetting curFunc to nil. --- docs/content/gosl.md | 6 +- gosl/README.md | 8 +- gosl/alignsl/alignsl.go | 4 +- gosl/gotosl/extract.go | 1 + gosl/gotosl/gosl_test.go | 2 +- gosl/gotosl/nodes.go | 28 ++- gosl/gotosl/sledits.go | 6 + gosl/gotosl/testdata/Compute.golden | 39 +++- gosl/gotosl/testdata/CycleUpdt.golden | 6 +- gosl/gotosl/testdata/basic.go | 27 ++- gosl/gotosl/testdata/basic.goal | 27 ++- gosl/slmath/README.md | 8 + gosl/slmath/doc.go | 10 + gosl/slmath/quaternion.go | 37 ++++ physics/README.md | 41 +++- physics/body.go | 87 +++++++- physics/body.goal | 87 +++++++- physics/collide.go | 84 ------- physics/contact.go | 82 +++++++ physics/contact.goal | 80 +++++++ physics/control.goal | 73 +++++++ physics/enumgen.go | 97 ++++++--- physics/gosl.go | 65 ++++-- physics/joint.go | 128 ++++++++--- physics/joint.goal | 128 ++++++++--- physics/params.go | 54 ++++- physics/shaders/InitDynamics.wgsl | 192 +++++++++++----- physics/shaders/Step.wgsl | 180 +++++++++++---- physics/shaders/StepJoints.wgsl | 302 ++++++++++++++++++++------ physics/step.go | 66 +++++- physics/step.goal | 66 +++++- physics/typegen.go | 8 +- physics/vars.go | 19 +- physics/world.go | 29 ++- physics/world.goal | 29 ++- 35 files changed, 1636 insertions(+), 470 deletions(-) create mode 100644 gosl/slmath/README.md create mode 100644 gosl/slmath/doc.go create mode 100644 gosl/slmath/quaternion.go delete mode 100644 physics/collide.go create mode 100644 physics/contact.go create mode 100644 physics/contact.goal create mode 100644 physics/control.goal diff --git a/docs/content/gosl.md b/docs/content/gosl.md index d80e3955..891222a7 100644 --- a/docs/content/gosl.md +++ b/docs/content/gosl.md @@ -2,9 +2,11 @@ Name = "GoSL" +++ -**GoSL** allows you to write Go programs that run on [[GPU]] hardware, by transpiling Go into the WGSL shader language used by [WebGPU](https://www.w3.org/TR/webgpu/), thereby establishing the _Go shader language_. +**GoSL** (via the `gosl` executable) allows you to write Go programs that run on [[GPU]] hardware, by transpiling Go into the WGSL shader language used by [WebGPU](https://www.w3.org/TR/webgpu/), thereby establishing the _Go shader language_. -GoSL uses the [core gpu](https://github.com/cogentcore/core/tree/main/gpu) compute shader system, and operates within the overall [[Goal]] framework of an augmented version of the Go language. +GoSL uses the [core gpu](https://github.com/cogentcore/core/tree/main/gpu) compute shader system, and can take advantage of the [[Goal]] transpiler to provide a more natural tensor indexing syntax. + +Functionally, GoSL is similar to [NVIDIA warp](https://github.com/NVIDIA/warp) -- [docs](https://nvidia.github.io/warp/basics.html), which uses python as the original source and converts it to either C++ or CUDA code. In GoSL, the original code is directly Go, so we just need to do the WGSL part. Unlike warp, WGSL runs on all GPU platforms, including the web (warp only runs on NVIDIA GPUs, on desktop). The relevant regions of Go code to be run on the GPU are tagged using the `//gosl:start` and `//gosl:end` comment directives, and this code must only use basic expressions and concrete types that will compile correctly in a GPU shader (see [[#Restrictions]] below). Method functions and pass-by-reference pointer arguments to `struct` types are supported and incur no additional compute cost due to inlining (see notes below for more detail). diff --git a/gosl/README.md b/gosl/README.md index 517b5bdf..e53f43c4 100644 --- a/gosl/README.md +++ b/gosl/README.md @@ -1,8 +1,10 @@ -# gosl: Go as a shader language +# GoSL: Go as a shader language -`gosl` implements _Go as a shader language_ for GPU compute shaders (using [WebGPU](https://www.w3.org/TR/webgpu/)), **enabling standard Go code to run on the GPU**. +**GoSL** (via the `gosl` executable) allows you to write Go programs that run on [[GPU]] hardware, by transpiling Go into the WGSL shader language used by [WebGPU](https://www.w3.org/TR/webgpu/), thereby establishing the _Go shader language_. -`gosl` converts Go code to WGSL which can then be loaded directly into a WebGPU compute shader, using the [core gpu](https://github.com/cogentcore/core/tree/main/gpu) compute shader system. It operates within the overall [Goal](../goal/README.md) framework of an augmented version of the Go language. +GoSL uses the [core gpu](https://github.com/cogentcore/core/tree/main/gpu) compute shader system, and can take advantage of the [[Goal]] transpiler to provide a more natural tensor indexing syntax. + +Functionally, GoSL is similar to [NVIDIA warp](https://github.com/NVIDIA/warp) -- [docs](https://nvidia.github.io/warp/basics.html), which uses python as the original source and converts it to either C++ or CUDA code. In GoSL, the original code is directly Go, so we just need to do the WGSL part. Unlike warp, WGSL runs on all GPU platforms, including the web (warp only runs on NVIDIA GPUs, on desktop). See [examples/basic](examples/basic) and [rand](examples/rand) for complete working examples. diff --git a/gosl/alignsl/alignsl.go b/gosl/alignsl/alignsl.go index e62247a0..d774d144 100644 --- a/gosl/alignsl/alignsl.go +++ b/gosl/alignsl/alignsl.go @@ -82,7 +82,9 @@ func CheckStruct(cx *Context, st *types.Struct, stName string) bool { ut := ft.Underlying() if bt, isBasic := ut.(*types.Basic); isBasic { kind := bt.Kind() - if !(kind == types.Uint32 || kind == types.Int32 || kind == types.Float32 || kind == types.Uint64) { + if kind == types.Invalid { + hasErr = cx.AddError(fmt.Sprintf(` %s: %s: add //gosl:import "package"`, fl.Name(), bt.String()), hasErr, stName) + } else if !(kind == types.Uint32 || kind == types.Int32 || kind == types.Float32 || kind == types.Uint64) { hasErr = cx.AddError(fmt.Sprintf(" %s: basic type != [U]Int32 or Float32: %s", fl.Name(), bt.String()), hasErr, stName) fmt.Println("kind:", kind, "ft:", ft.String()) } diff --git a/gosl/gotosl/extract.go b/gosl/gotosl/extract.go index ed6cad08..8291ad09 100644 --- a/gosl/gotosl/extract.go +++ b/gosl/gotosl/extract.go @@ -150,6 +150,7 @@ func (st *State) AppendGoHeader(lines [][]byte) [][]byte { olns = append(olns, []byte("package imports")) olns = append(olns, []byte(`import ( "math" + "cogentcore.org/core/math32" "cogentcore.org/lab/gosl/slbool" "cogentcore.org/lab/gosl/slrand" "cogentcore.org/lab/gosl/sltype" diff --git a/gosl/gotosl/gosl_test.go b/gosl/gotosl/gosl_test.go index ca223bfc..b6f97bc6 100644 --- a/gosl/gotosl/gosl_test.go +++ b/gosl/gotosl/gosl_test.go @@ -17,7 +17,7 @@ func TestTranslate(t *testing.T) { os.Chdir("testdata") opts := cli.DefaultOptions("gosl", "Go as a shader language converts Go code to WGSL WebGPU shader code, which can be run on the GPU through WebGPU.") - cfg := &Config{} + cfg := &Config{Keep: true} cli.Run(opts, cfg, Run) exSh, err := os.ReadFile("Compute.golden") diff --git a/gosl/gotosl/nodes.go b/gosl/gotosl/nodes.go index 9d6ac465..40106049 100644 --- a/gosl/gotosl/nodes.go +++ b/gosl/gotosl/nodes.go @@ -695,14 +695,26 @@ func (p *printer) derefPtrArgs(x ast.Expr, prec, depth int) { // gosl: mark pointer param types (only for non-struct), returns true if pointer func (p *printer) ptrParamType(x ast.Expr) (ast.Expr, bool) { if u, ok := x.(*ast.StarExpr); ok { - typ := p.getIdType(u.X.(*ast.Ident)) - if typ != nil { - if _, ok := typ.Underlying().(*types.Struct); ok { - return u.X, false + switch pt := u.X.(type) { + case *ast.Ident: + typ := p.getIdType(pt) + if typ != nil { + if _, ok := typ.Underlying().(*types.Struct); ok { + return u.X, false + } } + p.print("ptr")}, {[]byte("slvec.Vector2i"), []byte("vec4")}, {[]byte("slvec.Vector2"), []byte("vec4")}, + {[]byte("slvec.Vector3"), []byte("vec4")}, {[]byte("math32.Vector2i"), []byte("vec2")}, {[]byte("math32.Vector2"), []byte("vec2")}, {[]byte("math32.Vector3"), []byte("vec3")}, {[]byte("math32.Vector4"), []byte("vec4")}, + {[]byte("math32.Vec2i"), []byte("vec2")}, + {[]byte("math32.Vec2"), []byte("vec2")}, + {[]byte("math32.Vec3"), []byte("vec3")}, + {[]byte("math32.Vec4"), []byte("vec4")}, + {[]byte("math32.Quat"), []byte("vec4")}, {[]byte("float32"), []byte("f32")}, {[]byte("float64"), []byte("f64")}, // TODO: not yet supported {[]byte("uint32"), []byte("u32")}, diff --git a/gosl/gotosl/testdata/Compute.golden b/gosl/gotosl/testdata/Compute.golden index ee73061d..05ffa41e 100644 --- a/gosl/gotosl/testdata/Compute.golden +++ b/gosl/gotosl/testdata/Compute.golden @@ -126,6 +126,14 @@ const Raw: i32 = 0; const Integ: i32 = 1; const Exp: i32 = 2; const NVars: i32 = 3; +fn TransformPoint(xP: vec3, xQ: vec4, p: vec3) -> vec3 { + var dp = MulQuat(p, xQ);return dp+(xP); +} +fn MulTransforms(aP: vec3, aQ: vec4, bP: vec3, bQ: vec4, oP: ptr>, oQ: ptr>) { + var br = MulQuat(bP, aQ); + *oP = br+(aP); + *oQ = MulQuats(aQ, bQ); +} alias NeuronFlags = i32; const NeuronOff: NeuronFlags = 0x01; const NeuronHasExt: NeuronFlags = 0x02; // note: 1<<2 does NOT work @@ -156,6 +164,8 @@ struct ParamStruct { pad: f32, // comment this out to trigger alignment warning VXYf: vec4, // translates to vec4 VXYi: vec4, // translates to vec4 + Pos: vec4, + Rot: vec4, Subs: SubParamStruct, } fn ParamStruct_IntegFromRaw(ps: ParamStruct, idx: i32) -> f32 { @@ -170,10 +180,15 @@ fn ParamStruct_IntegFromRaw(ps: ParamStruct, idx: i32) -> f32 { var a = ps.VXYf.x; var b = vec2(ps.VXYf.x,ps.VXYf.y); var c = b*(b+(b)); // converted to direct ops + var op: vec3; + var oq: vec4; + MulTransforms(vec3(ps.Pos.x,ps.Pos.y,ps.Pos.z), ps.Rot, vec3(ps.Pos.x,ps.Pos.y,ps.Pos.z), ps.Rot, &op, &oq); + var d = MulQuat(op, oq); + d = TransformPoint(op, oq, d); let ctx = Ctx[0]; ParamStruct_AnotherMeth(ps, ctx, idx, &a); var bv = BigGet(Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(Integ))); - BigSet(bv*2 + c.y, Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(Exp)));return Data[Index2D(TensorStrides[0], TensorStrides[1], + BigSet(bv*2 + c.y + d.z, Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(Exp)));return Data[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(Exp))]; } fn ParamStruct_AnotherMeth(ps: ParamStruct, ctx: Context, idx: i32, ptrarg: ptr) { @@ -219,4 +234,26 @@ struct Context { fn Compute(i: u32) { //gosl:kernel let params = Params[0]; ParamStruct_IntegFromRaw(params, i32(i)); +} + +//////// import: "slmath-quaternion.go" +fn MulQuat(v: vec3, q: vec4) -> vec3 { + var qx = q.x; + var qy = q.y; + var qz = q.z; + var qw = q.w; + var ix = qw*v.x + qy*v.z - qz*v.y; + var iy = qw*v.y + qz*v.x - qx*v.z; + var iz = qw*v.z + qx*v.y - qy*v.x; + var iw = -qx*v.x - qy*v.y - qz*v.z; +return vec3(ix*qw+iw*-qx+iy*-qz-iz*-qy, + iy*qw+iw*-qy+iz*-qx-ix*-qz, + iz*qw+iw*-qz+ix*-qy-iy*-qx); +} +fn MulQuats(a: vec4,b: vec4) -> vec4 { + var q: vec4; + q.x = a.x*b.w + a.w*b.x + a.y*b.z - a.z*b.y; + q.y = a.y*b.w + a.w*b.y + a.z*b.x - a.x*b.z; + q.z = a.z*b.w + a.w*b.z + a.x*b.y - a.y*b.x; + q.w = a.w*b.w - a.x*b.x - a.y*b.y - a.z*b.z;return q; } \ No newline at end of file diff --git a/gosl/gotosl/testdata/CycleUpdt.golden b/gosl/gotosl/testdata/CycleUpdt.golden index de6861f7..cccfb6f3 100644 --- a/gosl/gotosl/testdata/CycleUpdt.golden +++ b/gosl/gotosl/testdata/CycleUpdt.golden @@ -46,6 +46,8 @@ struct ParamStruct { pad: f32, // comment this out to trigger alignment warning VXYf: vec4, // translates to vec4 VXYi: vec4, // translates to vec4 + Pos: vec4, + Rot: vec4, Subs: SubParamStruct, } struct Context { @@ -62,4 +64,6 @@ fn CycleUpdt(i: u32) { //gosl:kernel read-write:Ctx var ctx = Ctx[0]; Context_UpdtCycle(&ctx); Ctx[0] = ctx; -} \ No newline at end of file +} + +//////// import: "slmath-quaternion.go" \ No newline at end of file diff --git a/gosl/gotosl/testdata/basic.go b/gosl/gotosl/testdata/basic.go index 7a68df5c..8c94a422 100644 --- a/gosl/gotosl/testdata/basic.go +++ b/gosl/gotosl/testdata/basic.go @@ -8,11 +8,13 @@ import ( "cogentcore.org/core/math32" "cogentcore.org/lab/gosl/slbool" + "cogentcore.org/lab/gosl/slmath" "cogentcore.org/lab/gosl/slvec" "cogentcore.org/lab/tensor" ) //gosl:start +//gosl:import "cogentcore.org/lab/gosl/slmath" //gosl:vars var ( @@ -63,6 +65,21 @@ func FastExp(x float32) float32 { return math.Float32frombits(uint32(i)) } +// TransformPoint applies quat-based transform to given point +func TransformPoint(xP math32.Vector3, xQ math32.Quat, p math32.Vector3) math32.Vector3 { + dp := slmath.MulQuat(p, xQ) + return dp.Add(xP) +} + +// MulTransforms computes the equivalent of matrix multiplication for +// two quat-based transforms, o = a * b +func MulTransforms(aP math32.Vector3, aQ math32.Quat, bP math32.Vector3, bQ math32.Quat, oP *math32.Vector3, oQ *math32.Quat) { + // rotate b by a and add a + br := slmath.MulQuat(bP, aQ) + *oP = br.Add(aP) + *oQ = slmath.MulQuats(aQ, bQ) +} + // NeuronFlags are bit-flags encoding relevant binary state for neurons type NeuronFlags int32 @@ -129,6 +146,8 @@ type ParamStruct struct { VXYf slvec.Vector2 // translates to vec4 VXYi slvec.Vector2i // translates to vec4 + Pos slvec.Vector3 + Rot math32.Quat // extra parameters Subs SubParamStruct @@ -149,10 +168,16 @@ func (ps *ParamStruct) IntegFromRaw(idx int) float32 { b := ps.VXYf.V() c := b.Mul(b.Add(b)) // converted to direct ops + var op math32.Vector3 + var oq math32.Quat + MulTransforms(ps.Pos.V(), ps.Rot, ps.Pos.V(), ps.Rot, &op, &oq) + d := slmath.MulQuat(op, oq) + d = TransformPoint(op, oq, d) + ctx := GetCtx(0) ps.AnotherMeth(ctx, idx, &a) bv := Big.Value(int(idx), int(Integ)) - Big.Set(bv*2+c.Y, int(idx), int(Exp)) + Big.Set(bv*2+c.Y+d.Z, int(idx), int(Exp)) return Data.Value(int(idx), int(Exp)) } diff --git a/gosl/gotosl/testdata/basic.goal b/gosl/gotosl/testdata/basic.goal index 43c63b7f..23052256 100644 --- a/gosl/gotosl/testdata/basic.goal +++ b/gosl/gotosl/testdata/basic.goal @@ -5,11 +5,13 @@ import ( "cogentcore.org/core/math32" "cogentcore.org/lab/gosl/slbool" + "cogentcore.org/lab/gosl/slmath" "cogentcore.org/lab/gosl/slvec" "cogentcore.org/lab/tensor" ) //gosl:start +//gosl:import "cogentcore.org/lab/gosl/slmath" //gosl:vars var ( @@ -56,6 +58,21 @@ func FastExp(x float32) float32 { return math.Float32frombits(uint32(i)) } +// TransformPoint applies quat-based transform to given point +func TransformPoint(xP math32.Vector3, xQ math32.Quat, p math32.Vector3) math32.Vector3 { + dp := slmath.MulQuat(p, xQ) + return dp.Add(xP) +} + +// MulTransforms computes the equivalent of matrix multiplication for +// two quat-based transforms, o = a * b +func MulTransforms(aP math32.Vector3, aQ math32.Quat, bP math32.Vector3, bQ math32.Quat, oP *math32.Vector3, oQ *math32.Quat) { + // rotate b by a and add a + br := slmath.MulQuat(bP, aQ) + *oP = br.Add(aP) + *oQ = slmath.MulQuats(aQ, bQ) +} + // NeuronFlags are bit-flags encoding relevant binary state for neurons type NeuronFlags int32 @@ -122,6 +139,8 @@ type ParamStruct struct { VXYf slvec.Vector2 // translates to vec4 VXYi slvec.Vector2i // translates to vec4 + Pos slvec.Vector3 + Rot math32.Quat // extra parameters Subs SubParamStruct @@ -142,10 +161,16 @@ func (ps *ParamStruct) IntegFromRaw(idx int) float32 { b := ps.VXYf.V() c := b.Mul(b.Add(b)) // converted to direct ops + var op math32.Vector3 + var oq math32.Quat + MulTransforms(ps.Pos.V(), ps.Rot, ps.Pos.V(), ps.Rot, &op, &oq) + d := slmath.MulQuat(op, oq) + d = TransformPoint(op, oq, d) + ctx := GetCtx(0) ps.AnotherMeth(ctx, idx, &a) bv := Big[idx, Integ] - Big[idx, Exp] = bv*2 + c.Y + Big[idx, Exp] = bv*2 + c.Y + d.Z return Data[idx, Exp] } diff --git a/gosl/slmath/README.md b/gosl/slmath/README.md new file mode 100644 index 00000000..e3e7f363 --- /dev/null +++ b/gosl/slmath/README.md @@ -0,0 +1,8 @@ +# slmath + +`slmath` defines special math functions that operate on vector and quaternion types. These must be called as functions, not methods, and be outside of math32 itself so that the `math32.Vector3` -> `vec3` replacement operates correctly. Must explicitly import this package into gosl using: + +```go + //gosl:import "cogentcore.org/lab/gosl/slmath" +``` + diff --git a/gosl/slmath/doc.go b/gosl/slmath/doc.go new file mode 100644 index 00000000..c2d54ff9 --- /dev/null +++ b/gosl/slmath/doc.go @@ -0,0 +1,10 @@ +// Copyright (c) 2025, Cogent Core. All rights reserved. +// Use of this source code is governed by a BSD-style +// license that can be found in the LICENSE file. + +// Package slmath defines special math functions that operate on vector +// and quaternion types. These must be called as functions, not methods, +// and be outside of math32 itself so that the math32.Vector3 -> vec3 +// replacement operates correctly. Must explicitly import this package into +// gosl using: //gosl:import "cogentcore.org/lab/gosl/slmath" +package slmath diff --git a/gosl/slmath/quaternion.go b/gosl/slmath/quaternion.go new file mode 100644 index 00000000..b0e3f90c --- /dev/null +++ b/gosl/slmath/quaternion.go @@ -0,0 +1,37 @@ +// Copyright (c) 2025, Cogent Core. All rights reserved. +// Use of this source code is governed by a BSD-style +// license that can be found in the LICENSE file. + +package slmath + +import "cogentcore.org/core/math32" + +//gosl:start + +// MulQuat returns vector multiplied by specified quaternion and +// then by the quaternion inverse. +// It basically applies the rotation encoded in the quaternion to this vector. +func MulQuat(v math32.Vector3, q math32.Quat) math32.Vector3 { + // calculate quat * vector + ix := q.W*v.X + q.Y*v.Z - q.Z*v.Y + iy := q.W*v.Y + q.Z*v.X - q.X*v.Z + iz := q.W*v.Z + q.X*v.Y - q.Y*v.X + iw := -q.X*v.X - q.Y*v.Y - q.Z*v.Z + // calculate result * inverse quat + return math32.Vec3(ix*q.W+iw*-q.X+iy*-q.Z-iz*-q.Y, + iy*q.W+iw*-q.Y+iz*-q.X-ix*-q.Z, + iz*q.W+iw*-q.Z+ix*-q.Y-iy*-q.X) +} + +// MulQuats set this quaternion to the multiplication of a by b. +func MulQuats(a, b math32.Quat) math32.Quat { + // from http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/code/index.htm + var q math32.Quat + q.X = a.X*b.W + a.W*b.X + a.Y*b.Z - a.Z*b.Y + q.Y = a.Y*b.W + a.W*b.Y + a.Z*b.X - a.X*b.Z + q.Z = a.Z*b.W + a.W*b.Z + a.X*b.Y - a.Y*b.X + q.W = a.W*b.W - a.X*b.X - a.Y*b.Y - a.Z*b.Z + return q +} + +//gosl:end diff --git a/physics/README.md b/physics/README.md index 2f50f728..55d66e2e 100644 --- a/physics/README.md +++ b/physics/README.md @@ -2,13 +2,44 @@ The `physics` engine is a 3D physics simulator for creating virtual environments, which can run on the GPU or CPU using [gosl](https://cogentcore.org/lab/gosl). -The overall design emphasizes simplicity and robustness over exact physics precision. A simple forward Euler integration of aggregated forces is computed, with strong limits, soft constraints, and damping dynamics to prevent numerical instability even with relatively large step sizes. - All interactions are mediated by `Joint` elements that connect two rigid `Body` elements. Optimized joint types enable robust implementation of specific types of interactions. -To enable GPU computation, the data is all stored in tensor structures, with `Dynamics` and `Statics` Body tensors holding all the rigid body data, etc. +To enable GPU computation, the data is all stored in tensor structures, with `Dynamics` augmenting basic `Body` data for moving rigid bodies. Static elements participate in collisions but not joints. + +The [world](world) visualization sub-package manages a `View` element that links to physics bodies and generates an [xyz](https://cogentcore.org/core/xyz) 3D scenegraph based on the physics bodies, and updates this visualization efficiently as the physics is updated. + +## XPBD: Extended Position-Based Dynamics + +My intuition in confronting the physics problem was to directly update positions instead of dealing with forces, accelerations, or even velocities, because positions are more robust. Higher-order derivatives are messy and unstable. Turns out that this approach has proven quite powerful, with the MacklinMullerChentanez16 and MullerMacklinChentanezEtAl20 papers on XPBD providing some very compelling results: https://www.youtube.com/watch?v=CPq87E1vD8k + +* https://github.com/newton-physics/newton -- supports XPBD as one of several solvers, MuJoCo is default. +* https://github.com/NVIDIAGameWorks/PhysX -- older NVIDIA project -- not sure what it uses +* https://mujoco.readthedocs.io/en/stable/overview.html -- MuJoCo is widely used and is the default for Newton. +* https://github.com/bulletphysics/bullet3 -- uses Featherstone for joints and impulse-based contacts. Featherstone is incredibly complex to implement. + +* https://github.com/InteractiveComputerGraphics/PositionBasedDynamics -- another PBD impl +* https://github.com/nobuo-nakagawa/xpbd " 2016 + +So, the project is now to implement the XPBD algorithm, which is theoretically very simple, and the Newton code provides a python-based GPU-organized version. + +## Notes: + +* Update `Contact` points where bodies will touch (Dynamic on Dynamic or Static). Static is strongly grouped with hierarchical bounding boxes to optimize that process. Dynamic groups are updated as a function of motion to remain compact. + +* Contact points have priority over joints and are addressed first. No penetration is allowed. Position and velocity are updated directly. + +* Joints are then updated -- only one joint per dependent dynamic body. + + +### Scenarios + +* body -> head -> eye: this follows a clear "support" / A -> B directional dynamic -- but not parallel, as updates need to flow along the chain. + +* foot -> ankle -> leg -> body: assume that foot gets contact support, then leg, body depend on that. but what about falling over, so laying on body -- now foot is free, and dependence goes the other way. need a full constraint satisfaction solution. + +* could do multi-step constraint satisfaction within each update, so everything just propagates and an update step happens when settled? allows fully general updating. simpler. seems good. -The [world](world) visualization sub-package manages a View element that links to physics bodies and generates an [xyz](https://cogentcore.org/core/xyz) 3D scenegraph based on the physics bodies, and updates this visualization efficiently as the physics is updated. +* settle on positions, not velocities. velocity is a parameter to use as a constraint? ## OLD: @@ -50,7 +81,7 @@ It is up to the user to manage the list of potential collisions, e.g., by settin -Currently, it provides collision detection and basic forward Euler physics updating, but it does not yet compute any forces for the interactions among the bodies. Ultimately we hope to figure out how the [Bullet](https://github.com/bulletphysics/bullet3) system works and get that running here, in a clean and simple implementation. +Currently, it provides collision detection and basic forward Euler physics updating, but it does not yet compute any forces for the interactions among the bodies. Ultimately we hope to figure out how the [Bullet]) system works and get that running here, in a clean and simple implementation. Incrementally, we will start with a basic explicitly driven form of physics that is sufficient to get started, and build from there. diff --git a/physics/body.go b/physics/body.go index d43ccf34..0d230163 100644 --- a/physics/body.go +++ b/physics/body.go @@ -21,6 +21,9 @@ const ( // Shape is the shape type of the object, as a Shapes type. Shape BodyVars = iota + // WorldIndex partitions body into different worlds; Global are -1 + WorldIndex + // Size is the size of the object (values depend on shape type). SizeX SizeY @@ -31,6 +34,9 @@ const ( // Mass is the mass of the object. Mass + // InvMass is 1/mass of the object or 0 if no mass. + InvMass + // Bounce specifies the COR or coefficient of restitution (0..1), // which determines how elastic the collision is, // i.e., final velocity / initial velocity. @@ -40,16 +46,43 @@ const ( // Additive across the two surfaces. Friction - // 3D position of center of mass. + // 3D position of body (structural center). BodyPosX BodyPosY BodyPosZ - // Quaternion rotation. + // Quaternion rotation of body. BodyRotX BodyRotY BodyRotZ BodyRotW + + // Relative center-of-mass offset from 3D position of body. + BodyComX + BodyComY + BodyComZ + + // Inertia 3x3 matrix (column matrix organization, r,c labels). + InertiaXX + InertiaYX + InertiaZX + InertiaXY + InertiaYY + InertiaZY + InertiaXZ + InertiaYZ + InertiaZZ + + // InvInertia inverse inertia 3x3 matrix (column matrix organization, r,c labels). + InvInertiaXX + InvInertiaYX + InvInertiaZX + InvInertiaXY + InvInertiaYY + InvInertiaZY + InvInertiaXZ + InvInertiaYZ + InvInertiaZZ ) func BodyShape(idx int32) Shapes { @@ -104,6 +137,20 @@ func SetBodyRot(idx int32, rot math32.Quat) { Bodies.Set(rot.W, int(idx), int(BodyRotW)) } +func BodyCom(idx int32) math32.Vector3 { + var pos math32.Vector3 + pos.X = Bodies.Value(int(idx), int(BodyComX)) + pos.Y = Bodies.Value(int(idx), int(BodyComY)) + pos.Z = Bodies.Value(int(idx), int(BodyComZ)) + return pos +} + +func SetBodyCom(idx int32, pos math32.Vector3) { + Bodies.Set(pos.X, int(idx), int(BodyComX)) + Bodies.Set(pos.Y, int(idx), int(BodyComY)) + Bodies.Set(pos.Z, int(idx), int(BodyComZ)) +} + // DynamicVars are dynamic body variables stored in tensor.Float32. type DynamicVars int32 //enums:enum @@ -122,29 +169,47 @@ const ( RotZ RotW + // Linear velocity. VelX VelY VelZ + // Angular velocity. + AngVelX + AngVelY + AngVelZ + // Linear acceleration. AccX AccY AccZ + // Angular acceleration due to applied torques. + AngAccX + AngAccY + AngAccZ + // Linear force driving linear acceleration. + // joints write to this using atomic. must clear after each step. ForceX ForceY ForceZ - // Angular velocity. - AngVelX - AngVelY - AngVelZ - - // Angular acceleration due to applied torques. - AngAccX - AngAccY - AngAccZ + // Linear force driving linear acceleration. + // joints write to this using atomic. must clear after each step. + TorqueX + TorqueY + TorqueZ + + // Linear deltas. + DeltaX + DeltaY + DeltaZ + + // Angular deltas. + AngDeltaX + AngDeltaY + AngDeltaZ ) func SetDynamicIndex(idx, bodyIdx int32) { diff --git a/physics/body.goal b/physics/body.goal index 8a2d3197..f24577d4 100644 --- a/physics/body.goal +++ b/physics/body.goal @@ -19,6 +19,9 @@ const ( // Shape is the shape type of the object, as a Shapes type. Shape BodyVars = iota + // WorldIndex partitions body into different worlds; Global are -1 + WorldIndex + // Size is the size of the object (values depend on shape type). SizeX SizeY @@ -29,6 +32,9 @@ const ( // Mass is the mass of the object. Mass + // InvMass is 1/mass of the object or 0 if no mass. + InvMass + // Bounce specifies the COR or coefficient of restitution (0..1), // which determines how elastic the collision is, // i.e., final velocity / initial velocity. @@ -38,16 +44,43 @@ const ( // Additive across the two surfaces. Friction - // 3D position of center of mass. + // 3D position of body (structural center). BodyPosX BodyPosY BodyPosZ - // Quaternion rotation. + // Quaternion rotation of body. BodyRotX BodyRotY BodyRotZ BodyRotW + + // Relative center-of-mass offset from 3D position of body. + BodyComX + BodyComY + BodyComZ + + // Inertia 3x3 matrix (column matrix organization, r,c labels). + InertiaXX + InertiaYX + InertiaZX + InertiaXY + InertiaYY + InertiaZY + InertiaXZ + InertiaYZ + InertiaZZ + + // InvInertia inverse inertia 3x3 matrix (column matrix organization, r,c labels). + InvInertiaXX + InvInertiaYX + InvInertiaZX + InvInertiaXY + InvInertiaYY + InvInertiaZY + InvInertiaXZ + InvInertiaYZ + InvInertiaZZ ) func BodyShape(idx int32) Shapes { @@ -102,6 +135,20 @@ func SetBodyRot(idx int32, rot math32.Quat) { Bodies[idx, BodyRotW] = rot.W } +func BodyCom(idx int32) math32.Vector3 { + var pos math32.Vector3 + pos.X = Bodies[idx, BodyComX] + pos.Y = Bodies[idx, BodyComY] + pos.Z = Bodies[idx, BodyComZ] + return pos +} + +func SetBodyCom(idx int32, pos math32.Vector3) { + Bodies[idx, BodyComX] = pos.X + Bodies[idx, BodyComY] = pos.Y + Bodies[idx, BodyComZ] = pos.Z +} + // DynamicVars are dynamic body variables stored in tensor.Float32. type DynamicVars int32 //enums:enum @@ -120,29 +167,47 @@ const ( RotZ RotW + // Linear velocity. VelX VelY VelZ + // Angular velocity. + AngVelX + AngVelY + AngVelZ + // Linear acceleration. AccX AccY AccZ + // Angular acceleration due to applied torques. + AngAccX + AngAccY + AngAccZ + // Linear force driving linear acceleration. + // joints write to this using atomic. must clear after each step. ForceX ForceY ForceZ - // Angular velocity. - AngVelX - AngVelY - AngVelZ - - // Angular acceleration due to applied torques. - AngAccX - AngAccY - AngAccZ + // Linear force driving linear acceleration. + // joints write to this using atomic. must clear after each step. + TorqueX + TorqueY + TorqueZ + + // Linear deltas. + DeltaX + DeltaY + DeltaZ + + // Angular deltas. + AngDeltaX + AngDeltaY + AngDeltaZ ) func SetDynamicIndex(idx, bodyIdx int32) { diff --git a/physics/collide.go b/physics/collide.go deleted file mode 100644 index f24944ad..00000000 --- a/physics/collide.go +++ /dev/null @@ -1,84 +0,0 @@ -// Copyright (c) 2019, Cogent Core. All rights reserved. -// Use of this source code is governed by a BSD-style -// license that can be found in the LICENSE file. - -package physics - -//gosl:start - -// Contact is one pairwise point of contact between two bodies. -// Contacts are represented in spherical terms relative to the -// spherical BBox of A and B. -type ContactVars int32 //enums:enum - -const ( - // first body - ContactA ContactVars = iota - - // the other body - ContactB - - // normal pointing from center of B to center of A - ContactNormX - ContactNormY - ContactNormZ - - // point on spherical shell of B where A is contacting - ContactPointX - ContactPointY - ContactPointZ - - // ContactDist is the distance from PtB along NormB to contact - // point on spherical shell of A. - ContactDist -) - -//gosl:end - -/* - -// New adds a new contact to the list -func (cs *Contacts) New(a, b Body) *Contact { - c := &Contact{A: a, B: b} - *cs = append(*cs, c) - return c -} - -// BodyVelBBoxIntersects returns the list of potential contact nodes between a and b -// (could be the same or different groups) that have intersecting velocity-projected -// bounding boxes. In general a should be dynamic bodies and b either dynamic or static. -// This is the broad first-pass filtering. -func BodyVelBBoxIntersects(a, b Node) Contacts { - var cts Contacts - a.AsTree().WalkDown(func(k tree.Node) bool { - aii, ai := AsNode(k) - if aii == nil { - return false // going into a different type of thing, bail - } - abod := aii.AsBody() // only consider bodies for collision - if abod == nil { - return true - } - - b.AsTree().WalkDown(func(k tree.Node) bool { - bii, bi := AsNode(k) - if bii == nil { - return false // going into a different type of thing, bail - } - if !ai.BBox.IntersectsVelBox(&bi.BBox) { - return false // done - } - bbod := bii.AsBody() // only consider bodies for collision - if bbod == nil { - return true - } - cts.New(abod, bbod) - return false // done - }) - - return false - }) - return cts -} - -*/ diff --git a/physics/contact.go b/physics/contact.go new file mode 100644 index 00000000..c76cdd5c --- /dev/null +++ b/physics/contact.go @@ -0,0 +1,82 @@ +// Code generated by "goal build"; DO NOT EDIT. +//line contact.goal:1 +// Copyright (c) 2019, Cogent Core. All rights reserved. +// Use of this source code is governed by a BSD-style +// license that can be found in the LICENSE file. + +package physics + +//gosl:start + +// Contact is one pairwise point of contact between two bodies. +// Contacts are represented in spherical terms relative to the +// spherical BBox of A and B. +type ContactVars int32 //enums:enum + +const ( + // first body + ContactA ContactVars = iota + + // the other body + ContactB + + // normal pointing from center of B to center of A + ContactNormX + ContactNormY + ContactNormZ + + // point on spherical shell of B where A is contacting + ContactPointX + ContactPointY + ContactPointZ + + // ContactDist is the distance from PtB along NormB to contact + // point on spherical shell of A. + ContactDist +) + +//gosl:end + +// New adds a new contact to the list +// func (cs *Contacts) New(a, b Body) *Contact { +// c := &Contact{A: a, B: b} +// *cs = append(*cs, c) +// return c +// } +// +// // BodyVelBBoxIntersects returns the list of potential contact nodes between a and b +// // (could be the same or different groups) that have intersecting velocity-projected +// // bounding boxes. In general a should be dynamic bodies and b either dynamic or static. +// // This is the broad first-pass filtering. +// func BodyVelBBoxIntersects(a, b Node) Contacts { +// var cts Contacts +// a.AsTree().WalkDown(func(k tree.Node) bool { +// aii, ai := AsNode(k) +// if aii == nil { +// return false // going into a different type of thing, bail +// } +// abod := aii.AsBody() // only consider bodies for collision +// if abod == nil { +// return true +// } +// +// b.AsTree().WalkDown(func(k tree.Node) bool { +// bii, bi := AsNode(k) +// if bii == nil { +// return false // going into a different type of thing, bail +// } +// if !ai.BBox.IntersectsVelBox(&bi.BBox) { +// return false // done +// } +// bbod := bii.AsBody() // only consider bodies for collision +// if bbod == nil { +// return true +// } +// cts.New(abod, bbod) +// return false // done +// }) +// +// return false +// }) +// return cts +// } diff --git a/physics/contact.goal b/physics/contact.goal new file mode 100644 index 00000000..d007d021 --- /dev/null +++ b/physics/contact.goal @@ -0,0 +1,80 @@ +// Copyright (c) 2019, Cogent Core. All rights reserved. +// Use of this source code is governed by a BSD-style +// license that can be found in the LICENSE file. + +package physics + +//gosl:start + +// Contact is one pairwise point of contact between two bodies. +// Contacts are represented in spherical terms relative to the +// spherical BBox of A and B. +type ContactVars int32 //enums:enum + +const ( + // first body + ContactA ContactVars = iota + + // the other body + ContactB + + // normal pointing from center of B to center of A + ContactNormX + ContactNormY + ContactNormZ + + // point on spherical shell of B where A is contacting + ContactPointX + ContactPointY + ContactPointZ + + // ContactDist is the distance from PtB along NormB to contact + // point on spherical shell of A. + ContactDist +) + +//gosl:end + +// New adds a new contact to the list +// func (cs *Contacts) New(a, b Body) *Contact { +// c := &Contact{A: a, B: b} +// *cs = append(*cs, c) +// return c +// } +// +// // BodyVelBBoxIntersects returns the list of potential contact nodes between a and b +// // (could be the same or different groups) that have intersecting velocity-projected +// // bounding boxes. In general a should be dynamic bodies and b either dynamic or static. +// // This is the broad first-pass filtering. +// func BodyVelBBoxIntersects(a, b Node) Contacts { +// var cts Contacts +// a.AsTree().WalkDown(func(k tree.Node) bool { +// aii, ai := AsNode(k) +// if aii == nil { +// return false // going into a different type of thing, bail +// } +// abod := aii.AsBody() // only consider bodies for collision +// if abod == nil { +// return true +// } +// +// b.AsTree().WalkDown(func(k tree.Node) bool { +// bii, bi := AsNode(k) +// if bii == nil { +// return false // going into a different type of thing, bail +// } +// if !ai.BBox.IntersectsVelBox(&bi.BBox) { +// return false // done +// } +// bbod := bii.AsBody() // only consider bodies for collision +// if bbod == nil { +// return true +// } +// cts.New(abod, bbod) +// return false // done +// }) +// +// return false +// }) +// return cts +// } diff --git a/physics/control.goal b/physics/control.goal new file mode 100644 index 00000000..688d3439 --- /dev/null +++ b/physics/control.goal @@ -0,0 +1,73 @@ +// Copyright (c) 2025, Cogent Core. All rights reserved. +// Use of this source code is governed by a BSD-style +// license that can be found in the LICENSE file. + +package physics + +import "cogentcore.org/core/math32" + +//gosl:start + +// JointControlVars are external joint control input variables stored in tensor.Float32. +// These must be in one-to-one correspondence with the joints. +type JointControlVars int32 //enums:enum + +const ( + // Joint force and torque inputs + JointForceX JointControlVars = iota + JointForceY + JointForceZ + JointTorqueX + JointTorqueY + JointTorqueZ + + // target values (1 DoF use JointTargetPosX) + JointTargetPosX + JointTargetPosY + JointTargetPosZ + + JointTargetRotX + JointTargetRotY + JointTargetRotZ + JointTargetRotW + + // target velocity + JointTargetVelX + JointTargetVelY + JointTargetVelZ + + // target angular velocity + JointTargetAngVelX + JointTargetAngVelY + JointTargetAngVelZ +) + +func JointForce(idx int32) math32.Vector3 { + var f math32.Vector3 + f.X = Joints[idx, JointForceX] + f.Y = Joints[idx, JointForceY] + f.Z = Joints[idx, JointForceZ] + return f +} + +func SetJointForce(idx int32, f math32.Vector3) { + Joints[idx, JointForceX] = f.X + Joints[idx, JointForceY] = f.Y + Joints[idx, JointForceZ] = f.Z +} + +func JointTorque(idx int32) math32.Vector3 { + var f math32.Vector3 + f.X = Joints[idx, JointTorqueX] + f.Y = Joints[idx, JointTorqueY] + f.Z = Joints[idx, JointTorqueZ] + return f +} + +func SetJointTorque(idx int32, f math32.Vector3) { + Joints[idx, JointTorqueX] = f.X + Joints[idx, JointTorqueY] = f.Y + Joints[idx, JointTorqueZ] = f.Z +} + +//gosl:end diff --git a/physics/enumgen.go b/physics/enumgen.go index baee9b10..cafdcd98 100644 --- a/physics/enumgen.go +++ b/physics/enumgen.go @@ -6,20 +6,20 @@ import ( "cogentcore.org/core/enums" ) -var _BodyVarsValues = []BodyVars{0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13} +var _BodyVarsValues = []BodyVars{0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36} // BodyVarsN is the highest valid value for type BodyVars, plus one. // //gosl:start -const BodyVarsN BodyVars = 14 +const BodyVarsN BodyVars = 37 //gosl:end -var _BodyVarsValueMap = map[string]BodyVars{`Shape`: 0, `SizeX`: 1, `SizeY`: 2, `SizeZ`: 3, `Mass`: 4, `Bounce`: 5, `Friction`: 6, `BodyPosX`: 7, `BodyPosY`: 8, `BodyPosZ`: 9, `BodyRotX`: 10, `BodyRotY`: 11, `BodyRotZ`: 12, `BodyRotW`: 13} +var _BodyVarsValueMap = map[string]BodyVars{`Shape`: 0, `WorldIndex`: 1, `SizeX`: 2, `SizeY`: 3, `SizeZ`: 4, `Mass`: 5, `InvMass`: 6, `Bounce`: 7, `Friction`: 8, `BodyPosX`: 9, `BodyPosY`: 10, `BodyPosZ`: 11, `BodyRotX`: 12, `BodyRotY`: 13, `BodyRotZ`: 14, `BodyRotW`: 15, `BodyComX`: 16, `BodyComY`: 17, `BodyComZ`: 18, `InertiaXX`: 19, `InertiaYX`: 20, `InertiaZX`: 21, `InertiaXY`: 22, `InertiaYY`: 23, `InertiaZY`: 24, `InertiaXZ`: 25, `InertiaYZ`: 26, `InertiaZZ`: 27, `InvInertiaXX`: 28, `InvInertiaYX`: 29, `InvInertiaZX`: 30, `InvInertiaXY`: 31, `InvInertiaYY`: 32, `InvInertiaZY`: 33, `InvInertiaXZ`: 34, `InvInertiaYZ`: 35, `InvInertiaZZ`: 36} -var _BodyVarsDescMap = map[BodyVars]string{0: `Shape is the shape type of the object, as a Shapes type.`, 1: `Size is the size of the object (values depend on shape type).`, 2: ``, 3: ``, 4: `Mass is the mass of the object.`, 5: `Bounce specifies the COR or coefficient of restitution (0..1), which determines how elastic the collision is, i.e., final velocity / initial velocity.`, 6: `Friction coefficient: how much friction is generated by transverse motion. Additive across the two surfaces.`, 7: `3D position of center of mass.`, 8: ``, 9: ``, 10: `Quaternion rotation.`, 11: ``, 12: ``, 13: ``} +var _BodyVarsDescMap = map[BodyVars]string{0: `Shape is the shape type of the object, as a Shapes type.`, 1: `WorldIndex partitions body into different worlds; Global are -1`, 2: `Size is the size of the object (values depend on shape type).`, 3: ``, 4: ``, 5: `Mass is the mass of the object.`, 6: `InvMass is 1/mass of the object or 0 if no mass.`, 7: `Bounce specifies the COR or coefficient of restitution (0..1), which determines how elastic the collision is, i.e., final velocity / initial velocity.`, 8: `Friction coefficient: how much friction is generated by transverse motion. Additive across the two surfaces.`, 9: `3D position of body (structural center).`, 10: ``, 11: ``, 12: `Quaternion rotation of body.`, 13: ``, 14: ``, 15: ``, 16: `Relative center-of-mass offset from 3D position of body.`, 17: ``, 18: ``, 19: `Inertia 3x3 matrix (column matrix organization, r,c labels).`, 20: ``, 21: ``, 22: ``, 23: ``, 24: ``, 25: ``, 26: ``, 27: ``, 28: `InvInertia inverse inertia 3x3 matrix (column matrix organization, r,c labels).`, 29: ``, 30: ``, 31: ``, 32: ``, 33: ``, 34: ``, 35: ``, 36: ``} -var _BodyVarsMap = map[BodyVars]string{0: `Shape`, 1: `SizeX`, 2: `SizeY`, 3: `SizeZ`, 4: `Mass`, 5: `Bounce`, 6: `Friction`, 7: `BodyPosX`, 8: `BodyPosY`, 9: `BodyPosZ`, 10: `BodyRotX`, 11: `BodyRotY`, 12: `BodyRotZ`, 13: `BodyRotW`} +var _BodyVarsMap = map[BodyVars]string{0: `Shape`, 1: `WorldIndex`, 2: `SizeX`, 3: `SizeY`, 4: `SizeZ`, 5: `Mass`, 6: `InvMass`, 7: `Bounce`, 8: `Friction`, 9: `BodyPosX`, 10: `BodyPosY`, 11: `BodyPosZ`, 12: `BodyRotX`, 13: `BodyRotY`, 14: `BodyRotZ`, 15: `BodyRotW`, 16: `BodyComX`, 17: `BodyComY`, 18: `BodyComZ`, 19: `InertiaXX`, 20: `InertiaYX`, 21: `InertiaZX`, 22: `InertiaXY`, 23: `InertiaYY`, 24: `InertiaZY`, 25: `InertiaXZ`, 26: `InertiaYZ`, 27: `InertiaZZ`, 28: `InvInertiaXX`, 29: `InvInertiaYX`, 30: `InvInertiaZX`, 31: `InvInertiaXY`, 32: `InvInertiaYY`, 33: `InvInertiaZY`, 34: `InvInertiaXZ`, 35: `InvInertiaYZ`, 36: `InvInertiaZZ`} // String returns the string representation of this BodyVars value. func (i BodyVars) String() string { return enums.String(i, _BodyVarsMap) } @@ -51,20 +51,20 @@ func (i BodyVars) MarshalText() ([]byte, error) { return []byte(i.String()), nil // UnmarshalText implements the [encoding.TextUnmarshaler] interface. func (i *BodyVars) UnmarshalText(text []byte) error { return enums.UnmarshalText(i, text, "BodyVars") } -var _DynamicVarsValues = []DynamicVars{0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22} +var _DynamicVarsValues = []DynamicVars{0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31} // DynamicVarsN is the highest valid value for type DynamicVars, plus one. // //gosl:start -const DynamicVarsN DynamicVars = 23 +const DynamicVarsN DynamicVars = 32 //gosl:end -var _DynamicVarsValueMap = map[string]DynamicVars{`Index`: 0, `PosX`: 1, `PosY`: 2, `PosZ`: 3, `RotX`: 4, `RotY`: 5, `RotZ`: 6, `RotW`: 7, `VelX`: 8, `VelY`: 9, `VelZ`: 10, `AccX`: 11, `AccY`: 12, `AccZ`: 13, `ForceX`: 14, `ForceY`: 15, `ForceZ`: 16, `AngVelX`: 17, `AngVelY`: 18, `AngVelZ`: 19, `AngAccX`: 20, `AngAccY`: 21, `AngAccZ`: 22} +var _DynamicVarsValueMap = map[string]DynamicVars{`Index`: 0, `PosX`: 1, `PosY`: 2, `PosZ`: 3, `RotX`: 4, `RotY`: 5, `RotZ`: 6, `RotW`: 7, `VelX`: 8, `VelY`: 9, `VelZ`: 10, `AngVelX`: 11, `AngVelY`: 12, `AngVelZ`: 13, `AccX`: 14, `AccY`: 15, `AccZ`: 16, `AngAccX`: 17, `AngAccY`: 18, `AngAccZ`: 19, `ForceX`: 20, `ForceY`: 21, `ForceZ`: 22, `TorqueX`: 23, `TorqueY`: 24, `TorqueZ`: 25, `DeltaX`: 26, `DeltaY`: 27, `DeltaZ`: 28, `AngDeltaX`: 29, `AngDeltaY`: 30, `AngDeltaZ`: 31} -var _DynamicVarsDescMap = map[DynamicVars]string{0: `Index of body in list of bodies.`, 1: `3D position of center of mass.`, 2: ``, 3: ``, 4: `Quaternion rotation.`, 5: ``, 6: ``, 7: ``, 8: ``, 9: ``, 10: ``, 11: `Linear acceleration.`, 12: ``, 13: ``, 14: `Linear force driving linear acceleration.`, 15: ``, 16: ``, 17: `Angular velocity.`, 18: ``, 19: ``, 20: `Angular acceleration due to applied torques.`, 21: ``, 22: ``} +var _DynamicVarsDescMap = map[DynamicVars]string{0: `Index of body in list of bodies.`, 1: `3D position of center of mass.`, 2: ``, 3: ``, 4: `Quaternion rotation.`, 5: ``, 6: ``, 7: ``, 8: `Linear velocity.`, 9: ``, 10: ``, 11: `Angular velocity.`, 12: ``, 13: ``, 14: `Linear acceleration.`, 15: ``, 16: ``, 17: `Angular acceleration due to applied torques.`, 18: ``, 19: ``, 20: `Linear force driving linear acceleration. joints write to this using atomic. must clear after each step.`, 21: ``, 22: ``, 23: `Linear force driving linear acceleration. joints write to this using atomic. must clear after each step.`, 24: ``, 25: ``, 26: `Linear deltas.`, 27: ``, 28: ``, 29: `Angular deltas.`, 30: ``, 31: ``} -var _DynamicVarsMap = map[DynamicVars]string{0: `Index`, 1: `PosX`, 2: `PosY`, 3: `PosZ`, 4: `RotX`, 5: `RotY`, 6: `RotZ`, 7: `RotW`, 8: `VelX`, 9: `VelY`, 10: `VelZ`, 11: `AccX`, 12: `AccY`, 13: `AccZ`, 14: `ForceX`, 15: `ForceY`, 16: `ForceZ`, 17: `AngVelX`, 18: `AngVelY`, 19: `AngVelZ`, 20: `AngAccX`, 21: `AngAccY`, 22: `AngAccZ`} +var _DynamicVarsMap = map[DynamicVars]string{0: `Index`, 1: `PosX`, 2: `PosY`, 3: `PosZ`, 4: `RotX`, 5: `RotY`, 6: `RotZ`, 7: `RotW`, 8: `VelX`, 9: `VelY`, 10: `VelZ`, 11: `AngVelX`, 12: `AngVelY`, 13: `AngVelZ`, 14: `AccX`, 15: `AccY`, 16: `AccZ`, 17: `AngAccX`, 18: `AngAccY`, 19: `AngAccZ`, 20: `ForceX`, 21: `ForceY`, 22: `ForceZ`, 23: `TorqueX`, 24: `TorqueY`, 25: `TorqueZ`, 26: `DeltaX`, 27: `DeltaY`, 28: `DeltaZ`, 29: `AngDeltaX`, 30: `AngDeltaY`, 31: `AngDeltaZ`} // String returns the string representation of this DynamicVars value. func (i DynamicVars) String() string { return enums.String(i, _DynamicVarsMap) } @@ -145,20 +145,67 @@ func (i *ContactVars) UnmarshalText(text []byte) error { return enums.UnmarshalText(i, text, "ContactVars") } -var _GPUVarsValues = []GPUVars{0, 1, 2, 3, 4} +var _JointControlVarsValues = []JointControlVars{0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18} + +// JointControlVarsN is the highest valid value for type JointControlVars, plus one. +// +//gosl:start +const JointControlVarsN JointControlVars = 19 + +//gosl:end + +var _JointControlVarsValueMap = map[string]JointControlVars{`JointForceX`: 0, `JointForceY`: 1, `JointForceZ`: 2, `JointTorqueX`: 3, `JointTorqueY`: 4, `JointTorqueZ`: 5, `JointTargetPosX`: 6, `JointTargetPosY`: 7, `JointTargetPosZ`: 8, `JointTargetRotX`: 9, `JointTargetRotY`: 10, `JointTargetRotZ`: 11, `JointTargetRotW`: 12, `JointTargetVelX`: 13, `JointTargetVelY`: 14, `JointTargetVelZ`: 15, `JointTargetAngVelX`: 16, `JointTargetAngVelY`: 17, `JointTargetAngVelZ`: 18} + +var _JointControlVarsDescMap = map[JointControlVars]string{0: `Joint force and torque inputs`, 1: ``, 2: ``, 3: ``, 4: ``, 5: ``, 6: `target values (1 DoF use JointTargetPosX)`, 7: ``, 8: ``, 9: ``, 10: ``, 11: ``, 12: ``, 13: `target velocity`, 14: ``, 15: ``, 16: `target angular velocity`, 17: ``, 18: ``} + +var _JointControlVarsMap = map[JointControlVars]string{0: `JointForceX`, 1: `JointForceY`, 2: `JointForceZ`, 3: `JointTorqueX`, 4: `JointTorqueY`, 5: `JointTorqueZ`, 6: `JointTargetPosX`, 7: `JointTargetPosY`, 8: `JointTargetPosZ`, 9: `JointTargetRotX`, 10: `JointTargetRotY`, 11: `JointTargetRotZ`, 12: `JointTargetRotW`, 13: `JointTargetVelX`, 14: `JointTargetVelY`, 15: `JointTargetVelZ`, 16: `JointTargetAngVelX`, 17: `JointTargetAngVelY`, 18: `JointTargetAngVelZ`} + +// String returns the string representation of this JointControlVars value. +func (i JointControlVars) String() string { return enums.String(i, _JointControlVarsMap) } + +// SetString sets the JointControlVars value from its string representation, +// and returns an error if the string is invalid. +func (i *JointControlVars) SetString(s string) error { + return enums.SetString(i, s, _JointControlVarsValueMap, "JointControlVars") +} + +// Int64 returns the JointControlVars value as an int64. +func (i JointControlVars) Int64() int64 { return int64(i) } + +// SetInt64 sets the JointControlVars value from an int64. +func (i *JointControlVars) SetInt64(in int64) { *i = JointControlVars(in) } + +// Desc returns the description of the JointControlVars value. +func (i JointControlVars) Desc() string { return enums.Desc(i, _JointControlVarsDescMap) } + +// JointControlVarsValues returns all possible values for the type JointControlVars. +func JointControlVarsValues() []JointControlVars { return _JointControlVarsValues } + +// Values returns all possible values for the type JointControlVars. +func (i JointControlVars) Values() []enums.Enum { return enums.Values(_JointControlVarsValues) } + +// MarshalText implements the [encoding.TextMarshaler] interface. +func (i JointControlVars) MarshalText() ([]byte, error) { return []byte(i.String()), nil } + +// UnmarshalText implements the [encoding.TextUnmarshaler] interface. +func (i *JointControlVars) UnmarshalText(text []byte) error { + return enums.UnmarshalText(i, text, "JointControlVars") +} + +var _GPUVarsValues = []GPUVars{0, 1, 2, 3, 4, 5} // GPUVarsN is the highest valid value for type GPUVars, plus one. // //gosl:start -const GPUVarsN GPUVars = 5 +const GPUVarsN GPUVars = 6 //gosl:end -var _GPUVarsValueMap = map[string]GPUVars{`ParamsVar`: 0, `BodiesVar`: 1, `DynamicsVar`: 2, `JointsVar`: 3, `ContactsVar`: 4} +var _GPUVarsValueMap = map[string]GPUVars{`ParamsVar`: 0, `BodiesVar`: 1, `JointsVar`: 2, `DynamicsVar`: 3, `ContactsVar`: 4, `JointControlsVar`: 5} -var _GPUVarsDescMap = map[GPUVars]string{0: ``, 1: ``, 2: ``, 3: ``, 4: ``} +var _GPUVarsDescMap = map[GPUVars]string{0: ``, 1: ``, 2: ``, 3: ``, 4: ``, 5: ``} -var _GPUVarsMap = map[GPUVars]string{0: `ParamsVar`, 1: `BodiesVar`, 2: `DynamicsVar`, 3: `JointsVar`, 4: `ContactsVar`} +var _GPUVarsMap = map[GPUVars]string{0: `ParamsVar`, 1: `BodiesVar`, 2: `JointsVar`, 3: `DynamicsVar`, 4: `ContactsVar`, 5: `JointControlsVar`} // String returns the string representation of this GPUVars value. func (i GPUVars) String() string { return enums.String(i, _GPUVarsMap) } @@ -190,20 +237,20 @@ func (i GPUVars) MarshalText() ([]byte, error) { return []byte(i.String()), nil // UnmarshalText implements the [encoding.TextUnmarshaler] interface. func (i *GPUVars) UnmarshalText(text []byte) error { return enums.UnmarshalText(i, text, "GPUVars") } -var _JointVarsValues = []JointVars{0, 1, 2, 3, 4, 5, 6, 7} +var _JointVarsValues = []JointVars{0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20} // JointVarsN is the highest valid value for type JointVars, plus one. // //gosl:start -const JointVarsN JointVars = 8 +const JointVarsN JointVars = 21 //gosl:end -var _JointVarsValueMap = map[string]JointVars{`JointType`: 0, `JointA`: 1, `JointB`: 2, `JointPosX`: 3, `JointPosY`: 4, `JointPosZ`: 5, `JointParamA`: 6, `JointParamB`: 7} +var _JointVarsValueMap = map[string]JointVars{`JointType`: 0, `JointEnabled`: 1, `JointParent`: 2, `JointChild`: 3, `JointAncestor`: 4, `JointPPosX`: 5, `JointPPosY`: 6, `JointPPosZ`: 7, `JointPRotX`: 8, `JointPRotY`: 9, `JointPRotZ`: 10, `JointPRotW`: 11, `JointCPosX`: 12, `JointCPosY`: 13, `JointCPosZ`: 14, `JointCRotX`: 15, `JointCRotY`: 16, `JointCRotZ`: 17, `JointCRotW`: 18, `JointLimitLower`: 19, `JointLimitUpper`: 20} -var _JointVarsDescMap = map[JointVars]string{0: `JointType (as an int32 from bits).`, 1: `JointA is the dynamic body index for source, anchor, A body.`, 2: `JointB is the dynamic body index for target, follower, B body.`, 3: `position of joint.`, 4: ``, 5: ``, 6: `joint parameters, specific to each joint type.`, 7: ``} +var _JointVarsDescMap = map[JointVars]string{0: `JointType (as an int32 from bits).`, 1: `JointEnabled allows joints to be dynamically enabled.`, 2: `JointParent is the dynamic body index for parent body. Can be -1 for a fixed parent for absolute anchor.`, 3: `JointChild is the dynamic body index for child body.`, 4: `JointAncestor is the joint index where current parent is a child.`, 5: `position of joint, in parent frame.`, 6: ``, 7: ``, 8: `orientation of joint, in parent frame.`, 9: ``, 10: ``, 11: ``, 12: `position of joint, in child frame.`, 13: ``, 14: ``, 15: `orientation of joint, in child frame.`, 16: ``, 17: ``, 18: ``, 19: `joint limits`, 20: ``} -var _JointVarsMap = map[JointVars]string{0: `JointType`, 1: `JointA`, 2: `JointB`, 3: `JointPosX`, 4: `JointPosY`, 5: `JointPosZ`, 6: `JointParamA`, 7: `JointParamB`} +var _JointVarsMap = map[JointVars]string{0: `JointType`, 1: `JointEnabled`, 2: `JointParent`, 3: `JointChild`, 4: `JointAncestor`, 5: `JointPPosX`, 6: `JointPPosY`, 7: `JointPPosZ`, 8: `JointPRotX`, 9: `JointPRotY`, 10: `JointPRotZ`, 11: `JointPRotW`, 12: `JointCPosX`, 13: `JointCPosY`, 14: `JointCPosZ`, 15: `JointCRotX`, 16: `JointCRotY`, 17: `JointCRotZ`, 18: `JointCRotW`, 19: `JointLimitLower`, 20: `JointLimitUpper`} // String returns the string representation of this JointVars value. func (i JointVars) String() string { return enums.String(i, _JointVarsMap) } @@ -237,20 +284,20 @@ func (i *JointVars) UnmarshalText(text []byte) error { return enums.UnmarshalText(i, text, "JointVars") } -var _JointTypesValues = []JointTypes{0, 1} +var _JointTypesValues = []JointTypes{0, 1, 2, 3, 4, 5, 6} // JointTypesN is the highest valid value for type JointTypes, plus one. // //gosl:start -const JointTypesN JointTypes = 2 +const JointTypesN JointTypes = 7 //gosl:end -var _JointTypesValueMap = map[string]JointTypes{`Glue`: 0, `Ball`: 1} +var _JointTypesValueMap = map[string]JointTypes{`Prismatic`: 0, `Revolute`: 1, `Ball`: 2, `Fixed`: 3, `Free`: 4, `Distance`: 5, `D6`: 6} -var _JointTypesDescMap = map[JointTypes]string{0: `Glue is a completely rigid joint that supercedes all others, where body A drives motion of body B. JointA is the parent, supporting body, JointB is the child supported.`, 1: ``} +var _JointTypesDescMap = map[JointTypes]string{0: `Prismatic allows translation along a single axis (slider): 1 DoF.`, 1: `Revolute allows rotation about a single axis (axel): 1 DoF.`, 2: `Ball allows rotation about all three axes (3 DoF, quaternion).`, 3: `Fixed locks all relative motion: 0 DoF.`, 4: `Free allows full 6-DoF motion (translation and rotation).`, 5: `Distance keeps two bodies a distance within joint limits: 6 DoF.`, 6: `D6 is a generic 6-DoF joint.`} -var _JointTypesMap = map[JointTypes]string{0: `Glue`, 1: `Ball`} +var _JointTypesMap = map[JointTypes]string{0: `Prismatic`, 1: `Revolute`, 2: `Ball`, 3: `Fixed`, 4: `Free`, 5: `Distance`, 6: `D6`} // String returns the string representation of this JointTypes value. func (i JointTypes) String() string { return enums.String(i, _JointTypesMap) } diff --git a/physics/gosl.go b/physics/gosl.go index 67e2f98a..93b52ddb 100644 --- a/physics/gosl.go +++ b/physics/gosl.go @@ -41,9 +41,10 @@ type GPUVars int32 //enums:enum const ( ParamsVar GPUVars = 0 BodiesVar GPUVars = 1 - DynamicsVar GPUVars = 2 - JointsVar GPUVars = 3 + JointsVar GPUVars = 2 + DynamicsVar GPUVars = 3 ContactsVar GPUVars = 4 + JointControlsVar GPUVars = 5 ) // Tensor stride variables @@ -84,30 +85,38 @@ func GPUInit() { var vr *gpu.Var _ = vr vr = sgp.Add("Bodies", gpu.Float32, 1, gpu.ComputeShader) - vr = sgp.Add("Dynamics", gpu.Float32, 1, gpu.ComputeShader) vr = sgp.Add("Joints", gpu.Float32, 1, gpu.ComputeShader) sgp.SetNValues(1) } { - sgp := vars.AddGroup(gpu.Storage, "Contacts") + sgp := vars.AddGroup(gpu.Storage, "Bodies") var vr *gpu.Var _ = vr + vr = sgp.Add("Dynamics", gpu.Float32, 1, gpu.ComputeShader) vr = sgp.Add("Contacts", gpu.Float32, 1, gpu.ComputeShader) sgp.SetNValues(1) } + { + sgp := vars.AddGroup(gpu.Storage, "Controls") + var vr *gpu.Var + _ = vr + vr = sgp.Add("JointControls", gpu.Float32, 1, gpu.ComputeShader) + sgp.SetNValues(1) + } var pl *gpu.ComputePipeline pl = gpu.NewComputePipelineShaderFS(shaders, "shaders/InitDynamics.wgsl", sy) pl.AddVarUsed(0, "TensorStrides") pl.AddVarUsed(1, "Bodies") - pl.AddVarUsed(1, "Dynamics") + pl.AddVarUsed(2, "Dynamics") pl.AddVarUsed(0, "Params") pl = gpu.NewComputePipelineShaderFS(shaders, "shaders/Step.wgsl", sy) pl.AddVarUsed(0, "TensorStrides") - pl.AddVarUsed(1, "Dynamics") + pl.AddVarUsed(2, "Dynamics") pl.AddVarUsed(0, "Params") pl = gpu.NewComputePipelineShaderFS(shaders, "shaders/StepJoints.wgsl", sy) pl.AddVarUsed(0, "TensorStrides") - pl.AddVarUsed(1, "Dynamics") + pl.AddVarUsed(1, "Bodies") + pl.AddVarUsed(2, "Dynamics") pl.AddVarUsed(1, "Joints") pl.AddVarUsed(0, "Params") sy.Config() @@ -286,15 +295,18 @@ func ToGPU(vars ...GPUVars) { case BodiesVar: v, _ := syVars.ValueByIndex(1, "Bodies", 0) gpu.SetValueFrom(v, Bodies.Values) - case DynamicsVar: - v, _ := syVars.ValueByIndex(1, "Dynamics", 0) - gpu.SetValueFrom(v, Dynamics.Values) case JointsVar: v, _ := syVars.ValueByIndex(1, "Joints", 0) gpu.SetValueFrom(v, Joints.Values) + case DynamicsVar: + v, _ := syVars.ValueByIndex(2, "Dynamics", 0) + gpu.SetValueFrom(v, Dynamics.Values) case ContactsVar: v, _ := syVars.ValueByIndex(2, "Contacts", 0) gpu.SetValueFrom(v, Contacts.Values) + case JointControlsVar: + v, _ := syVars.ValueByIndex(3, "JointControls", 0) + gpu.SetValueFrom(v, JointControls.Values) } } } @@ -316,15 +328,17 @@ func ToGPUTensorStrides() { } sy := GPUSystem syVars := sy.Vars() - TensorStrides.SetShapeSizes(40) + TensorStrides.SetShapeSizes(50) TensorStrides.SetInt1D(Bodies.Shape().Strides[0], 0) TensorStrides.SetInt1D(Bodies.Shape().Strides[1], 1) - TensorStrides.SetInt1D(Dynamics.Shape().Strides[0], 10) - TensorStrides.SetInt1D(Dynamics.Shape().Strides[1], 11) - TensorStrides.SetInt1D(Joints.Shape().Strides[0], 20) - TensorStrides.SetInt1D(Joints.Shape().Strides[1], 21) + TensorStrides.SetInt1D(Joints.Shape().Strides[0], 10) + TensorStrides.SetInt1D(Joints.Shape().Strides[1], 11) + TensorStrides.SetInt1D(Dynamics.Shape().Strides[0], 20) + TensorStrides.SetInt1D(Dynamics.Shape().Strides[1], 21) TensorStrides.SetInt1D(Contacts.Shape().Strides[0], 30) TensorStrides.SetInt1D(Contacts.Shape().Strides[1], 31) + TensorStrides.SetInt1D(JointControls.Shape().Strides[0], 40) + TensorStrides.SetInt1D(JointControls.Shape().Strides[1], 41) v, _ := syVars.ValueByIndex(0, "TensorStrides", 0) gpu.SetValueFrom(v, TensorStrides.Values) } @@ -341,15 +355,18 @@ func ReadFromGPU(vars ...GPUVars) { case BodiesVar: v, _ := syVars.ValueByIndex(1, "Bodies", 0) v.GPUToRead(sy.CommandEncoder) - case DynamicsVar: - v, _ := syVars.ValueByIndex(1, "Dynamics", 0) - v.GPUToRead(sy.CommandEncoder) case JointsVar: v, _ := syVars.ValueByIndex(1, "Joints", 0) v.GPUToRead(sy.CommandEncoder) + case DynamicsVar: + v, _ := syVars.ValueByIndex(2, "Dynamics", 0) + v.GPUToRead(sy.CommandEncoder) case ContactsVar: v, _ := syVars.ValueByIndex(2, "Contacts", 0) v.GPUToRead(sy.CommandEncoder) + case JointControlsVar: + v, _ := syVars.ValueByIndex(3, "JointControls", 0) + v.GPUToRead(sy.CommandEncoder) } } } @@ -368,18 +385,22 @@ func SyncFromGPU(vars ...GPUVars) { v, _ := syVars.ValueByIndex(1, "Bodies", 0) v.ReadSync() gpu.ReadToBytes(v, Bodies.Values) - case DynamicsVar: - v, _ := syVars.ValueByIndex(1, "Dynamics", 0) - v.ReadSync() - gpu.ReadToBytes(v, Dynamics.Values) case JointsVar: v, _ := syVars.ValueByIndex(1, "Joints", 0) v.ReadSync() gpu.ReadToBytes(v, Joints.Values) + case DynamicsVar: + v, _ := syVars.ValueByIndex(2, "Dynamics", 0) + v.ReadSync() + gpu.ReadToBytes(v, Dynamics.Values) case ContactsVar: v, _ := syVars.ValueByIndex(2, "Contacts", 0) v.ReadSync() gpu.ReadToBytes(v, Contacts.Values) + case JointControlsVar: + v, _ := syVars.ValueByIndex(3, "JointControls", 0) + v.ReadSync() + gpu.ReadToBytes(v, JointControls.Values) } } } diff --git a/physics/joint.go b/physics/joint.go index b9fe4c0a..4e48788c 100644 --- a/physics/joint.go +++ b/physics/joint.go @@ -14,27 +14,53 @@ import ( //gosl:start -// JointVars are joint state variables stored in tensor.Float32 +// JointVars are joint state variables stored in tensor.Float32. +// These are all static joint properties; dynamic control variables +// in [JointControlVars] and [JointControls]. type JointVars int32 //enums:enum const ( // JointType (as an int32 from bits). JointType JointVars = iota - // JointA is the dynamic body index for source, anchor, A body. - JointA + // JointEnabled allows joints to be dynamically enabled. + JointEnabled - // JointB is the dynamic body index for target, follower, B body. - JointB + // JointParent is the dynamic body index for parent body. + // Can be -1 for a fixed parent for absolute anchor. + JointParent - // position of joint. - JointPosX - JointPosY - JointPosZ + // JointChild is the dynamic body index for child body. + JointChild - // joint parameters, specific to each joint type. - JointParamA - JointParamB + // JointAncestor is the joint index where current parent is a child. + JointAncestor + + // position of joint, in parent frame. + JointPPosX + JointPPosY + JointPPosZ + + // orientation of joint, in parent frame. + JointPRotX + JointPRotY + JointPRotZ + JointPRotW + + // position of joint, in child frame. + JointCPosX + JointCPosY + JointCPosZ + + // orientation of joint, in child frame. + JointCRotX + JointCRotY + JointCRotZ + JointCRotW + + // joint limits + JointLimitLower + JointLimitUpper ) func GetJointType(idx int32) JointTypes { @@ -45,52 +71,82 @@ func SetJointType(idx int32, typ JointTypes) { Joints.Set(math.Float32frombits(uint32(typ)), int(idx), int(JointType)) } -func SetJointA(idx, bodyIdx int32) { - Joints.Set(math.Float32frombits(uint32(bodyIdx)), int(idx), int(JointA)) +func SetJointParent(idx, bodyIdx int32) { + Joints.Set(math.Float32frombits(uint32(bodyIdx)), int(idx), int(JointParent)) } -func JointAIndex(idx int32) int32 { - return int32(math.Float32bits(Joints.Value(int(idx), int(JointA)))) +func JointParentIndex(idx int32) int32 { + return int32(math.Float32bits(Joints.Value(int(idx), int(JointParent)))) } -func SetJointB(idx, bodyIdx int32) { - Joints.Set(math.Float32frombits(uint32(bodyIdx)), int(idx), int(JointB)) +func SetJointChild(idx, bodyIdx int32) { + Joints.Set(math.Float32frombits(uint32(bodyIdx)), int(idx), int(JointChild)) } -func JointBIndex(idx int32) int32 { - return int32(math.Float32bits(Joints.Value(int(idx), int(JointB)))) +func JointChildIndex(idx int32) int32 { + return int32(math.Float32bits(Joints.Value(int(idx), int(JointChild)))) } -func JointPos(idx int32) math32.Vector3 { +func JointPPos(idx int32) math32.Vector3 { var pos math32.Vector3 - pos.X = Joints.Value(int(idx), int(JointPosX)) - pos.Y = Joints.Value(int(idx), int(JointPosY)) - pos.Z = Joints.Value(int(idx), int(JointPosZ)) + pos.X = Joints.Value(int(idx), int(JointPPosX)) + pos.Y = Joints.Value(int(idx), int(JointPPosY)) + pos.Z = Joints.Value(int(idx), int(JointPPosZ)) return pos } -func SetJointPos(idx int32, pos math32.Vector3) { - Joints.Set(pos.X, int(idx), int(JointPosX)) - Joints.Set(pos.Y, int(idx), int(JointPosY)) - Joints.Set(pos.Z, int(idx), int(JointPosZ)) +func SetJointPPos(idx int32, pos math32.Vector3) { + Joints.Set(pos.X, int(idx), int(JointPPosX)) + Joints.Set(pos.Y, int(idx), int(JointPPosY)) + Joints.Set(pos.Z, int(idx), int(JointPPosZ)) +} + +func JointPRot(idx int32) math32.Quat { + var rot math32.Quat + rot.X = Joints.Value(int(idx), int(JointPRotX)) + rot.Y = Joints.Value(int(idx), int(JointPRotY)) + rot.Z = Joints.Value(int(idx), int(JointPRotZ)) + rot.W = Joints.Value(int(idx), int(JointPRotW)) + return rot +} + +func SetJointPRot(idx int32, rot math32.Quat) { + Joints.Set(rot.X, int(idx), int(JointPRotX)) + Joints.Set(rot.Y, int(idx), int(JointPRotY)) + Joints.Set(rot.Z, int(idx), int(JointPRotZ)) + Joints.Set(rot.W, int(idx), int(JointPRotW)) } // JointTypes are joint types that determine nature of interaction. type JointTypes int32 //enums:enum const ( - // Glue is a completely rigid joint that supercedes all others, - // where body A drives motion of body B. - // JointA is the parent, supporting body, JointB is the child supported. - Glue JointTypes = iota + // Prismatic allows translation along a single axis (slider): 1 DoF. + Prismatic JointTypes = iota + // Revolute allows rotation about a single axis (axel): 1 DoF. + Revolute + + // Ball allows rotation about all three axes (3 DoF, quaternion). Ball + + // Fixed locks all relative motion: 0 DoF. + Fixed + + // Free allows full 6-DoF motion (translation and rotation). + Free + + // Distance keeps two bodies a distance within joint limits: 6 DoF. + Distance + + // D6 is a generic 6-DoF joint. + D6 ) -// GlueStep does glue joint processing -func GlueStep(ji, ba, bb int32) { - pos := JointPos(ji) - bap := DynamicPos(ba) +// FixedStep does fixed joint processing +func FixedStep(ji, jp, jc int32) { + pos := JointPPos(ji) + jpp := DynamicPos(jp) bbp := pos.Add(bap) SetDynamicPos(bb, bbp) } diff --git a/physics/joint.goal b/physics/joint.goal index dc72e4e0..c954eae2 100644 --- a/physics/joint.goal +++ b/physics/joint.goal @@ -12,27 +12,53 @@ import ( //gosl:start -// JointVars are joint state variables stored in tensor.Float32 +// JointVars are joint state variables stored in tensor.Float32. +// These are all static joint properties; dynamic control variables +// in [JointControlVars] and [JointControls]. type JointVars int32 //enums:enum const ( // JointType (as an int32 from bits). JointType JointVars = iota - // JointA is the dynamic body index for source, anchor, A body. - JointA + // JointEnabled allows joints to be dynamically enabled. + JointEnabled - // JointB is the dynamic body index for target, follower, B body. - JointB + // JointParent is the dynamic body index for parent body. + // Can be -1 for a fixed parent for absolute anchor. + JointParent - // position of joint. - JointPosX - JointPosY - JointPosZ + // JointChild is the dynamic body index for child body. + JointChild - // joint parameters, specific to each joint type. - JointParamA - JointParamB + // JointAncestor is the joint index where current parent is a child. + JointAncestor + + // position of joint, in parent frame. + JointPPosX + JointPPosY + JointPPosZ + + // orientation of joint, in parent frame. + JointPRotX + JointPRotY + JointPRotZ + JointPRotW + + // position of joint, in child frame. + JointCPosX + JointCPosY + JointCPosZ + + // orientation of joint, in child frame. + JointCRotX + JointCRotY + JointCRotZ + JointCRotW + + // joint limits + JointLimitLower + JointLimitUpper ) func GetJointType(idx int32) JointTypes { @@ -43,52 +69,82 @@ func SetJointType(idx int32, typ JointTypes) { Joints[idx, JointType] = math.Float32frombits(uint32(typ)) } -func SetJointA(idx, bodyIdx int32) { - Joints[idx, JointA] = math.Float32frombits(uint32(bodyIdx)) +func SetJointParent(idx, bodyIdx int32) { + Joints[idx, JointParent] = math.Float32frombits(uint32(bodyIdx)) } -func JointAIndex(idx int32) int32 { - return int32(math.Float32bits(Joints[idx, JointA])) +func JointParentIndex(idx int32) int32 { + return int32(math.Float32bits(Joints[idx, JointParent])) } -func SetJointB(idx, bodyIdx int32) { - Joints[idx, JointB] = math.Float32frombits(uint32(bodyIdx)) +func SetJointChild(idx, bodyIdx int32) { + Joints[idx, JointChild] = math.Float32frombits(uint32(bodyIdx)) } -func JointBIndex(idx int32) int32 { - return int32(math.Float32bits(Joints[idx, JointB])) +func JointChildIndex(idx int32) int32 { + return int32(math.Float32bits(Joints[idx, JointChild])) } -func JointPos(idx int32) math32.Vector3 { +func JointPPos(idx int32) math32.Vector3 { var pos math32.Vector3 - pos.X = Joints[idx, JointPosX] - pos.Y = Joints[idx, JointPosY] - pos.Z = Joints[idx, JointPosZ] + pos.X = Joints[idx, JointPPosX] + pos.Y = Joints[idx, JointPPosY] + pos.Z = Joints[idx, JointPPosZ] return pos } -func SetJointPos(idx int32, pos math32.Vector3) { - Joints[idx, JointPosX] = pos.X - Joints[idx, JointPosY] = pos.Y - Joints[idx, JointPosZ] = pos.Z +func SetJointPPos(idx int32, pos math32.Vector3) { + Joints[idx, JointPPosX] = pos.X + Joints[idx, JointPPosY] = pos.Y + Joints[idx, JointPPosZ] = pos.Z +} + +func JointPRot(idx int32) math32.Quat { + var rot math32.Quat + rot.X = Joints[idx, JointPRotX] + rot.Y = Joints[idx, JointPRotY] + rot.Z = Joints[idx, JointPRotZ] + rot.W = Joints[idx, JointPRotW] + return rot +} + +func SetJointPRot(idx int32, rot math32.Quat) { + Joints[idx, JointPRotX] = rot.X + Joints[idx, JointPRotY] = rot.Y + Joints[idx, JointPRotZ] = rot.Z + Joints[idx, JointPRotW] = rot.W } // JointTypes are joint types that determine nature of interaction. type JointTypes int32 //enums:enum const ( - // Glue is a completely rigid joint that supercedes all others, - // where body A drives motion of body B. - // JointA is the parent, supporting body, JointB is the child supported. - Glue JointTypes = iota + // Prismatic allows translation along a single axis (slider): 1 DoF. + Prismatic JointTypes = iota + // Revolute allows rotation about a single axis (axel): 1 DoF. + Revolute + + // Ball allows rotation about all three axes (3 DoF, quaternion). Ball + + // Fixed locks all relative motion: 0 DoF. + Fixed + + // Free allows full 6-DoF motion (translation and rotation). + Free + + // Distance keeps two bodies a distance within joint limits: 6 DoF. + Distance + + // D6 is a generic 6-DoF joint. + D6 ) -// GlueStep does glue joint processing -func GlueStep(ji, ba, bb int32) { - pos := JointPos(ji) - bap := DynamicPos(ba) +// FixedStep does fixed joint processing +func FixedStep(ji, jp, jc int32) { + pos := JointPPos(ji) + jpp := DynamicPos(jp) bbp := pos.Add(bap) SetDynamicPos(bb, bbp) } diff --git a/physics/params.go b/physics/params.go index 150ded50..26f44fe8 100644 --- a/physics/params.go +++ b/physics/params.go @@ -4,7 +4,10 @@ package physics -import "cogentcore.org/core/math32" +import ( + "cogentcore.org/lab/gosl/slbool" + "cogentcore.org/lab/gosl/slvec" +) //gosl:start @@ -16,19 +19,52 @@ type PhysParams struct { // JointsN is number of joints. JointsN int32 - // Step is the global stepsize for numerical integration. - Step float32 `default:"0.01"` + // Iters is the number of iterations to perform. + Iters int32 `default:"2"` - // Gravity is the force of gravity. - Gravity float32 + // SoftRelax is soft-body relaxation constant. + SoftRelax float32 `default:"0.9"` - // GravityDir is the direction of Gravity. - GravityDir math32.Vector4 + // JointLinearRelax is joint linear relaxation constant. + JointLinearRelax float32 `default:"0.7"` + + // JointAngularRelax is joint angular relaxation constant. + JointAngularRelax float32 `default:"0.4"` + + // JointLinearComply is joint linear compliance constant. + JointLinearComply float32 `default:"0"` + + // JointAngularComply is joint angular compliance constant. + JointAngularComply float32 `default:"0"` + + // ContactRelax is rigid contact relaxation constant. + ContactRelax float32 `default:"0.8"` + + // AngularDamping is damping of angular motion. + AngularDamping float32 `default:"0"` + + // Contact weighting + ContactWeighting slbool.Bool `default:"true"` + + // Restitution + Restitution slbool.Bool `default:"false"` + + // Gravity is the gravity acceleration function + Gravity slvec.Vector3 } func (pr *PhysParams) Defaults() { - pr.Step = 0.01 - pr.GravityDir.Set(0, -1, 0, 0) + pr.Iters = 2 + pr.Gravity.Set(0, -9.81, 0) + pr.SoftRelax = 0.9 + pr.JointLinearRelax = 0.7 + pr.JointAngularRelax = 0.4 + pr.JointLinearComply = 0 + pr.JointAngularComply = 0 + pr.ContactRelax = 0.8 + pr.AngularDamping = 0 + pr.ContactWeighting.SetBool(true) + pr.Restitution.SetBool(false) } //gosl:end diff --git a/physics/shaders/InitDynamics.wgsl b/physics/shaders/InitDynamics.wgsl index c6d0dfd4..1c304df6 100644 --- a/physics/shaders/InitDynamics.wgsl +++ b/physics/shaders/InitDynamics.wgsl @@ -9,9 +9,10 @@ var Params: array; // // Bodies are the rigid body elements (dynamic and static), // specifying the constant, non-dynamic properties, // which is initial state for dynamics. // [body][BodyVarsN] @group(1) @binding(0) var Bodies: array; -@group(1) @binding(1) +// // Dynamics are the dynamic rigid body elements: these actually move. // [body][DynamicVarsN] +@group(2) @binding(0) var Dynamics: array; -// // Contacts are points of contact between bodies. // [contact][ContactVarsN] +// // JointControls are dynamic joint control inputs. // [joint][JointControlVarsN] alias GPUVars = i32; @@ -31,19 +32,42 @@ fn Index2D(s0: u32, s1: u32, i0: u32, i1: u32) -> u32 { //////// import: "body.go" alias BodyVars = i32; //enums:enum const Shape: BodyVars = 0; -const SizeX: BodyVars = 1; -const SizeY: BodyVars = 2; -const SizeZ: BodyVars = 3; -const Mass: BodyVars = 4; -const Bounce: BodyVars = 5; -const Friction: BodyVars = 6; -const BodyPosX: BodyVars = 7; -const BodyPosY: BodyVars = 8; -const BodyPosZ: BodyVars = 9; -const BodyRotX: BodyVars = 10; -const BodyRotY: BodyVars = 11; -const BodyRotZ: BodyVars = 12; -const BodyRotW: BodyVars = 13; +const WorldIndex: BodyVars = 1; +const SizeX: BodyVars = 2; +const SizeY: BodyVars = 3; +const SizeZ: BodyVars = 4; +const Mass: BodyVars = 5; +const InvMass: BodyVars = 6; +const Bounce: BodyVars = 7; +const Friction: BodyVars = 8; +const BodyPosX: BodyVars = 9; +const BodyPosY: BodyVars = 10; +const BodyPosZ: BodyVars = 11; +const BodyRotX: BodyVars = 12; +const BodyRotY: BodyVars = 13; +const BodyRotZ: BodyVars = 14; +const BodyRotW: BodyVars = 15; +const BodyComX: BodyVars = 16; +const BodyComY: BodyVars = 17; +const BodyComZ: BodyVars = 18; +const InertiaXX: BodyVars = 19; +const InertiaYX: BodyVars = 20; +const InertiaZX: BodyVars = 21; +const InertiaXY: BodyVars = 22; +const InertiaYY: BodyVars = 23; +const InertiaZY: BodyVars = 24; +const InertiaXZ: BodyVars = 25; +const InertiaYZ: BodyVars = 26; +const InertiaZZ: BodyVars = 27; +const InvInertiaXX: BodyVars = 28; +const InvInertiaYX: BodyVars = 29; +const InvInertiaZX: BodyVars = 30; +const InvInertiaXY: BodyVars = 31; +const InvInertiaYY: BodyVars = 32; +const InvInertiaZY: BodyVars = 33; +const InvInertiaXZ: BodyVars = 34; +const InvInertiaYZ: BodyVars = 35; +const InvInertiaZZ: BodyVars = 36; alias DynamicVars = i32; //enums:enum const Index: DynamicVars = 0; const PosX: DynamicVars = 1; @@ -56,23 +80,32 @@ const RotW: DynamicVars = 7; const VelX: DynamicVars = 8; const VelY: DynamicVars = 9; const VelZ: DynamicVars = 10; -const AccX: DynamicVars = 11; -const AccY: DynamicVars = 12; -const AccZ: DynamicVars = 13; -const ForceX: DynamicVars = 14; -const ForceY: DynamicVars = 15; -const ForceZ: DynamicVars = 16; -const AngVelX: DynamicVars = 17; -const AngVelY: DynamicVars = 18; -const AngVelZ: DynamicVars = 19; -const AngAccX: DynamicVars = 20; -const AngAccY: DynamicVars = 21; -const AngAccZ: DynamicVars = 22; +const AngVelX: DynamicVars = 11; +const AngVelY: DynamicVars = 12; +const AngVelZ: DynamicVars = 13; +const AccX: DynamicVars = 14; +const AccY: DynamicVars = 15; +const AccZ: DynamicVars = 16; +const AngAccX: DynamicVars = 17; +const AngAccY: DynamicVars = 18; +const AngAccZ: DynamicVars = 19; +const ForceX: DynamicVars = 20; +const ForceY: DynamicVars = 21; +const ForceZ: DynamicVars = 22; +const TorqueX: DynamicVars = 23; +const TorqueY: DynamicVars = 24; +const TorqueZ: DynamicVars = 25; +const DeltaX: DynamicVars = 26; +const DeltaY: DynamicVars = 27; +const DeltaZ: DynamicVars = 28; +const AngDeltaX: DynamicVars = 29; +const AngDeltaY: DynamicVars = 30; +const AngDeltaZ: DynamicVars = 31; fn DynamicIndex(idx: i32) -> i32 { - return i32(bitcast(Dynamics[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(Index))])); + return i32(bitcast(Dynamics[Index2D(TensorStrides[20], TensorStrides[21], u32(idx), u32(Index))])); } -//////// import: "collide.go" +//////// import: "contact.go" alias ContactVars = i32; //enums:enum const ContactA: ContactVars = 0; const ContactB: ContactVars = 1; @@ -84,36 +117,85 @@ const ContactPointY: ContactVars = 6; const ContactPointZ: ContactVars = 7; const ContactDist: ContactVars = 8; +//////// import: "control.go" +alias JointControlVars = i32; //enums:enum +const JointForceX: JointControlVars = 0; +const JointForceY: JointControlVars = 1; +const JointForceZ: JointControlVars = 2; +const JointTorqueX: JointControlVars = 3; +const JointTorqueY: JointControlVars = 4; +const JointTorqueZ: JointControlVars = 5; +const JointTargetPosX: JointControlVars = 6; +const JointTargetPosY: JointControlVars = 7; +const JointTargetPosZ: JointControlVars = 8; +const JointTargetRotX: JointControlVars = 9; +const JointTargetRotY: JointControlVars = 10; +const JointTargetRotZ: JointControlVars = 11; +const JointTargetRotW: JointControlVars = 12; +const JointTargetVelX: JointControlVars = 13; +const JointTargetVelY: JointControlVars = 14; +const JointTargetVelZ: JointControlVars = 15; +const JointTargetAngVelX: JointControlVars = 16; +const JointTargetAngVelY: JointControlVars = 17; +const JointTargetAngVelZ: JointControlVars = 18; + //////// import: "enumgen.go" -const BodyVarsN: BodyVars = 14; -const DynamicVarsN: DynamicVars = 23; +const BodyVarsN: BodyVars = 37; +const DynamicVarsN: DynamicVars = 32; const ContactVarsN: ContactVars = 9; -const GPUVarsN: GPUVars = 5; -const JointVarsN: JointVars = 8; -const JointTypesN: JointTypes = 2; +const JointControlVarsN: JointControlVars = 19; +const GPUVarsN: GPUVars = 6; +const JointVarsN: JointVars = 21; +const JointTypesN: JointTypes = 7; const ShapesN: Shapes = 4; //////// import: "joint.go" alias JointVars = i32; //enums:enum const JointType: JointVars = 0; -const JointA: JointVars = 1; -const JointB: JointVars = 2; -const JointPosX: JointVars = 3; -const JointPosY: JointVars = 4; -const JointPosZ: JointVars = 5; -const JointParamA: JointVars = 6; -const JointParamB: JointVars = 7; +const JointEnabled: JointVars = 1; +const JointParent: JointVars = 2; +const JointChild: JointVars = 3; +const JointAncestor: JointVars = 4; +const JointPPosX: JointVars = 5; +const JointPPosY: JointVars = 6; +const JointPPosZ: JointVars = 7; +const JointPRotX: JointVars = 8; +const JointPRotY: JointVars = 9; +const JointPRotZ: JointVars = 10; +const JointPRotW: JointVars = 11; +const JointCPosX: JointVars = 12; +const JointCPosY: JointVars = 13; +const JointCPosZ: JointVars = 14; +const JointCRotX: JointVars = 15; +const JointCRotY: JointVars = 16; +const JointCRotZ: JointVars = 17; +const JointCRotW: JointVars = 18; +const JointLimitLower: JointVars = 19; +const JointLimitUpper: JointVars = 20; alias JointTypes = i32; //enums:enum -const Glue: JointTypes = 0; -const Ball: JointTypes = 1; +const Prismatic: JointTypes = 0; +const Revolute: JointTypes = 1; +const Ball: JointTypes = 2; +const Fixed: JointTypes = 3; +const Free: JointTypes = 4; +const Distance: JointTypes = 5; +const D6: JointTypes = 6; //////// import: "params.go" struct PhysParams { DynamicsN: i32, JointsN: i32, - Step: f32, - Gravity: f32, - GravityDir: vec4, + Iters: i32, + SoftRelax: f32, + JointLinearRelax: f32, + JointAngularRelax: f32, + JointLinearComply: f32, + JointAngularComply: f32, + ContactRelax: f32, + AngularDamping: f32, + ContactWeighting: i32, + Restitution: i32, + Gravity: vec4, } //////// import: "shapes.go" @@ -123,6 +205,8 @@ const Sphere: Shapes = 1; const Cylinder: Shapes = 2; const Capsule: Shapes = 3; +//////// import: "slmath-quaternion.go" + //////// import: "step.go" fn InitDynamics(i: u32) { //gosl:kernel let pars = Params[0]; @@ -131,15 +215,15 @@ fn InitDynamics(i: u32) { //gosl:kernel return; } var bi = DynamicIndex(ii); - Dynamics[Index2D(TensorStrides[10], TensorStrides[11], u32(ii), u32(PosX))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyPosX))]; - Dynamics[Index2D(TensorStrides[10], TensorStrides[11], u32(ii), u32(PosY))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyPosY))]; - Dynamics[Index2D(TensorStrides[10], TensorStrides[11], u32(ii), u32(PosZ))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyPosZ))]; - Dynamics[Index2D(TensorStrides[10], TensorStrides[11], u32(ii), u32(RotX))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyRotX))]; - Dynamics[Index2D(TensorStrides[10], TensorStrides[11], u32(ii), u32(RotY))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyRotY))]; - Dynamics[Index2D(TensorStrides[10], TensorStrides[11], u32(ii), u32(RotZ))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyRotZ))]; - Dynamics[Index2D(TensorStrides[10], TensorStrides[11], u32(ii), u32(RotW))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyRotW))]; + Dynamics[Index2D(TensorStrides[20], TensorStrides[21], u32(ii), u32(PosX))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyPosX))]; + Dynamics[Index2D(TensorStrides[20], TensorStrides[21], u32(ii), u32(PosY))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyPosY))]; + Dynamics[Index2D(TensorStrides[20], TensorStrides[21], u32(ii), u32(PosZ))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyPosZ))]; + Dynamics[Index2D(TensorStrides[20], TensorStrides[21], u32(ii), u32(RotX))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyRotX))]; + Dynamics[Index2D(TensorStrides[20], TensorStrides[21], u32(ii), u32(RotY))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyRotY))]; + Dynamics[Index2D(TensorStrides[20], TensorStrides[21], u32(ii), u32(RotZ))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyRotZ))]; + Dynamics[Index2D(TensorStrides[20], TensorStrides[21], u32(ii), u32(RotW))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyRotW))]; for (var v = VelX; v < DynamicVarsN; v++) { - Dynamics[Index2D(TensorStrides[10], TensorStrides[11], + Dynamics[Index2D(TensorStrides[20], TensorStrides[21], u32(ii), u32(v))] = 0.0; } } \ No newline at end of file diff --git a/physics/shaders/Step.wgsl b/physics/shaders/Step.wgsl index 7f679a6e..264403e8 100644 --- a/physics/shaders/Step.wgsl +++ b/physics/shaders/Step.wgsl @@ -7,9 +7,10 @@ var TensorStrides: array; @group(0) @binding(1) var Params: array; // // Bodies are the rigid body elements (dynamic and static), // specifying the constant, non-dynamic properties, // which is initial state for dynamics. // [body][BodyVarsN] -@group(1) @binding(1) +// // Dynamics are the dynamic rigid body elements: these actually move. // [body][DynamicVarsN] +@group(2) @binding(0) var Dynamics: array; -// // Contacts are points of contact between bodies. // [contact][ContactVarsN] +// // JointControls are dynamic joint control inputs. // [joint][JointControlVarsN] alias GPUVars = i32; @@ -29,19 +30,42 @@ fn Index2D(s0: u32, s1: u32, i0: u32, i1: u32) -> u32 { //////// import: "body.go" alias BodyVars = i32; //enums:enum const Shape: BodyVars = 0; -const SizeX: BodyVars = 1; -const SizeY: BodyVars = 2; -const SizeZ: BodyVars = 3; -const Mass: BodyVars = 4; -const Bounce: BodyVars = 5; -const Friction: BodyVars = 6; -const BodyPosX: BodyVars = 7; -const BodyPosY: BodyVars = 8; -const BodyPosZ: BodyVars = 9; -const BodyRotX: BodyVars = 10; -const BodyRotY: BodyVars = 11; -const BodyRotZ: BodyVars = 12; -const BodyRotW: BodyVars = 13; +const WorldIndex: BodyVars = 1; +const SizeX: BodyVars = 2; +const SizeY: BodyVars = 3; +const SizeZ: BodyVars = 4; +const Mass: BodyVars = 5; +const InvMass: BodyVars = 6; +const Bounce: BodyVars = 7; +const Friction: BodyVars = 8; +const BodyPosX: BodyVars = 9; +const BodyPosY: BodyVars = 10; +const BodyPosZ: BodyVars = 11; +const BodyRotX: BodyVars = 12; +const BodyRotY: BodyVars = 13; +const BodyRotZ: BodyVars = 14; +const BodyRotW: BodyVars = 15; +const BodyComX: BodyVars = 16; +const BodyComY: BodyVars = 17; +const BodyComZ: BodyVars = 18; +const InertiaXX: BodyVars = 19; +const InertiaYX: BodyVars = 20; +const InertiaZX: BodyVars = 21; +const InertiaXY: BodyVars = 22; +const InertiaYY: BodyVars = 23; +const InertiaZY: BodyVars = 24; +const InertiaXZ: BodyVars = 25; +const InertiaYZ: BodyVars = 26; +const InertiaZZ: BodyVars = 27; +const InvInertiaXX: BodyVars = 28; +const InvInertiaYX: BodyVars = 29; +const InvInertiaZX: BodyVars = 30; +const InvInertiaXY: BodyVars = 31; +const InvInertiaYY: BodyVars = 32; +const InvInertiaZY: BodyVars = 33; +const InvInertiaXZ: BodyVars = 34; +const InvInertiaYZ: BodyVars = 35; +const InvInertiaZZ: BodyVars = 36; alias DynamicVars = i32; //enums:enum const Index: DynamicVars = 0; const PosX: DynamicVars = 1; @@ -54,20 +78,29 @@ const RotW: DynamicVars = 7; const VelX: DynamicVars = 8; const VelY: DynamicVars = 9; const VelZ: DynamicVars = 10; -const AccX: DynamicVars = 11; -const AccY: DynamicVars = 12; -const AccZ: DynamicVars = 13; -const ForceX: DynamicVars = 14; -const ForceY: DynamicVars = 15; -const ForceZ: DynamicVars = 16; -const AngVelX: DynamicVars = 17; -const AngVelY: DynamicVars = 18; -const AngVelZ: DynamicVars = 19; -const AngAccX: DynamicVars = 20; -const AngAccY: DynamicVars = 21; -const AngAccZ: DynamicVars = 22; +const AngVelX: DynamicVars = 11; +const AngVelY: DynamicVars = 12; +const AngVelZ: DynamicVars = 13; +const AccX: DynamicVars = 14; +const AccY: DynamicVars = 15; +const AccZ: DynamicVars = 16; +const AngAccX: DynamicVars = 17; +const AngAccY: DynamicVars = 18; +const AngAccZ: DynamicVars = 19; +const ForceX: DynamicVars = 20; +const ForceY: DynamicVars = 21; +const ForceZ: DynamicVars = 22; +const TorqueX: DynamicVars = 23; +const TorqueY: DynamicVars = 24; +const TorqueZ: DynamicVars = 25; +const DeltaX: DynamicVars = 26; +const DeltaY: DynamicVars = 27; +const DeltaZ: DynamicVars = 28; +const AngDeltaX: DynamicVars = 29; +const AngDeltaY: DynamicVars = 30; +const AngDeltaZ: DynamicVars = 31; -//////// import: "collide.go" +//////// import: "contact.go" alias ContactVars = i32; //enums:enum const ContactA: ContactVars = 0; const ContactB: ContactVars = 1; @@ -79,36 +112,85 @@ const ContactPointY: ContactVars = 6; const ContactPointZ: ContactVars = 7; const ContactDist: ContactVars = 8; +//////// import: "control.go" +alias JointControlVars = i32; //enums:enum +const JointForceX: JointControlVars = 0; +const JointForceY: JointControlVars = 1; +const JointForceZ: JointControlVars = 2; +const JointTorqueX: JointControlVars = 3; +const JointTorqueY: JointControlVars = 4; +const JointTorqueZ: JointControlVars = 5; +const JointTargetPosX: JointControlVars = 6; +const JointTargetPosY: JointControlVars = 7; +const JointTargetPosZ: JointControlVars = 8; +const JointTargetRotX: JointControlVars = 9; +const JointTargetRotY: JointControlVars = 10; +const JointTargetRotZ: JointControlVars = 11; +const JointTargetRotW: JointControlVars = 12; +const JointTargetVelX: JointControlVars = 13; +const JointTargetVelY: JointControlVars = 14; +const JointTargetVelZ: JointControlVars = 15; +const JointTargetAngVelX: JointControlVars = 16; +const JointTargetAngVelY: JointControlVars = 17; +const JointTargetAngVelZ: JointControlVars = 18; + //////// import: "enumgen.go" -const BodyVarsN: BodyVars = 14; -const DynamicVarsN: DynamicVars = 23; +const BodyVarsN: BodyVars = 37; +const DynamicVarsN: DynamicVars = 32; const ContactVarsN: ContactVars = 9; -const GPUVarsN: GPUVars = 5; -const JointVarsN: JointVars = 8; -const JointTypesN: JointTypes = 2; +const JointControlVarsN: JointControlVars = 19; +const GPUVarsN: GPUVars = 6; +const JointVarsN: JointVars = 21; +const JointTypesN: JointTypes = 7; const ShapesN: Shapes = 4; //////// import: "joint.go" alias JointVars = i32; //enums:enum const JointType: JointVars = 0; -const JointA: JointVars = 1; -const JointB: JointVars = 2; -const JointPosX: JointVars = 3; -const JointPosY: JointVars = 4; -const JointPosZ: JointVars = 5; -const JointParamA: JointVars = 6; -const JointParamB: JointVars = 7; +const JointEnabled: JointVars = 1; +const JointParent: JointVars = 2; +const JointChild: JointVars = 3; +const JointAncestor: JointVars = 4; +const JointPPosX: JointVars = 5; +const JointPPosY: JointVars = 6; +const JointPPosZ: JointVars = 7; +const JointPRotX: JointVars = 8; +const JointPRotY: JointVars = 9; +const JointPRotZ: JointVars = 10; +const JointPRotW: JointVars = 11; +const JointCPosX: JointVars = 12; +const JointCPosY: JointVars = 13; +const JointCPosZ: JointVars = 14; +const JointCRotX: JointVars = 15; +const JointCRotY: JointVars = 16; +const JointCRotZ: JointVars = 17; +const JointCRotW: JointVars = 18; +const JointLimitLower: JointVars = 19; +const JointLimitUpper: JointVars = 20; alias JointTypes = i32; //enums:enum -const Glue: JointTypes = 0; -const Ball: JointTypes = 1; +const Prismatic: JointTypes = 0; +const Revolute: JointTypes = 1; +const Ball: JointTypes = 2; +const Fixed: JointTypes = 3; +const Free: JointTypes = 4; +const Distance: JointTypes = 5; +const D6: JointTypes = 6; //////// import: "params.go" struct PhysParams { DynamicsN: i32, JointsN: i32, - Step: f32, - Gravity: f32, - GravityDir: vec4, + Iters: i32, + SoftRelax: f32, + JointLinearRelax: f32, + JointAngularRelax: f32, + JointLinearComply: f32, + JointAngularComply: f32, + ContactRelax: f32, + AngularDamping: f32, + ContactWeighting: i32, + Restitution: i32, + Gravity: vec4, } //////// import: "shapes.go" @@ -118,6 +200,8 @@ const Sphere: Shapes = 1; const Cylinder: Shapes = 2; const Capsule: Shapes = 3; +//////// import: "slmath-quaternion.go" + //////// import: "step.go" fn Step(i: u32) { //gosl:kernel let pars = Params[0]; @@ -125,7 +209,7 @@ fn Step(i: u32) { //gosl:kernel if (ii >= pars.DynamicsN) { return; } - Dynamics[Index2D(TensorStrides[10], TensorStrides[11], u32(ii), u32(PosX))] += pars.Step * Dynamics[Index2D(TensorStrides[10], TensorStrides[11], u32(ii), u32(VelX))]; - Dynamics[Index2D(TensorStrides[10], TensorStrides[11], u32(ii), u32(PosY))] += pars.Step * Dynamics[Index2D(TensorStrides[10], TensorStrides[11], u32(ii), u32(VelY))]; - Dynamics[Index2D(TensorStrides[10], TensorStrides[11], u32(ii), u32(PosZ))] += pars.Step * Dynamics[Index2D(TensorStrides[10], TensorStrides[11], u32(ii), u32(VelZ))]; + Dynamics[Index2D(TensorStrides[20], TensorStrides[21], u32(ii), u32(PosX))] += pars.Step * Dynamics[Index2D(TensorStrides[20], TensorStrides[21], u32(ii), u32(VelX))]; + Dynamics[Index2D(TensorStrides[20], TensorStrides[21], u32(ii), u32(PosY))] += pars.Step * Dynamics[Index2D(TensorStrides[20], TensorStrides[21], u32(ii), u32(VelY))]; + Dynamics[Index2D(TensorStrides[20], TensorStrides[21], u32(ii), u32(PosZ))] += pars.Step * Dynamics[Index2D(TensorStrides[20], TensorStrides[21], u32(ii), u32(VelZ))]; } \ No newline at end of file diff --git a/physics/shaders/StepJoints.wgsl b/physics/shaders/StepJoints.wgsl index ff1a1d47..a3aab1a3 100644 --- a/physics/shaders/StepJoints.wgsl +++ b/physics/shaders/StepJoints.wgsl @@ -7,11 +7,14 @@ var TensorStrides: array; @group(0) @binding(1) var Params: array; // // Bodies are the rigid body elements (dynamic and static), // specifying the constant, non-dynamic properties, // which is initial state for dynamics. // [body][BodyVarsN] +@group(1) @binding(0) +var Bodies: array; @group(1) @binding(1) -var Dynamics: array; -@group(1) @binding(2) var Joints: array; -// // Contacts are points of contact between bodies. // [contact][ContactVarsN] +// // Dynamics are the dynamic rigid body elements: these actually move. // [body][DynamicVarsN] +@group(2) @binding(0) +var Dynamics: array; +// // JointControls are dynamic joint control inputs. // [joint][JointControlVarsN] alias GPUVars = i32; @@ -31,19 +34,48 @@ fn Index2D(s0: u32, s1: u32, i0: u32, i1: u32) -> u32 { //////// import: "body.go" alias BodyVars = i32; //enums:enum const Shape: BodyVars = 0; -const SizeX: BodyVars = 1; -const SizeY: BodyVars = 2; -const SizeZ: BodyVars = 3; -const Mass: BodyVars = 4; -const Bounce: BodyVars = 5; -const Friction: BodyVars = 6; -const BodyPosX: BodyVars = 7; -const BodyPosY: BodyVars = 8; -const BodyPosZ: BodyVars = 9; -const BodyRotX: BodyVars = 10; -const BodyRotY: BodyVars = 11; -const BodyRotZ: BodyVars = 12; -const BodyRotW: BodyVars = 13; +const WorldIndex: BodyVars = 1; +const SizeX: BodyVars = 2; +const SizeY: BodyVars = 3; +const SizeZ: BodyVars = 4; +const Mass: BodyVars = 5; +const InvMass: BodyVars = 6; +const Bounce: BodyVars = 7; +const Friction: BodyVars = 8; +const BodyPosX: BodyVars = 9; +const BodyPosY: BodyVars = 10; +const BodyPosZ: BodyVars = 11; +const BodyRotX: BodyVars = 12; +const BodyRotY: BodyVars = 13; +const BodyRotZ: BodyVars = 14; +const BodyRotW: BodyVars = 15; +const BodyComX: BodyVars = 16; +const BodyComY: BodyVars = 17; +const BodyComZ: BodyVars = 18; +const InertiaXX: BodyVars = 19; +const InertiaYX: BodyVars = 20; +const InertiaZX: BodyVars = 21; +const InertiaXY: BodyVars = 22; +const InertiaYY: BodyVars = 23; +const InertiaZY: BodyVars = 24; +const InertiaXZ: BodyVars = 25; +const InertiaYZ: BodyVars = 26; +const InertiaZZ: BodyVars = 27; +const InvInertiaXX: BodyVars = 28; +const InvInertiaYX: BodyVars = 29; +const InvInertiaZX: BodyVars = 30; +const InvInertiaXY: BodyVars = 31; +const InvInertiaYY: BodyVars = 32; +const InvInertiaZY: BodyVars = 33; +const InvInertiaXZ: BodyVars = 34; +const InvInertiaYZ: BodyVars = 35; +const InvInertiaZZ: BodyVars = 36; +fn BodyCom(idx: i32) -> vec3 { + var pos: vec3; + pos.x = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComX))]; + pos.y = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComY))]; + pos.z = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComZ))];return pos; +} alias DynamicVars = i32; //enums:enum const Index: DynamicVars = 0; const PosX: DynamicVars = 1; @@ -56,31 +88,45 @@ const RotW: DynamicVars = 7; const VelX: DynamicVars = 8; const VelY: DynamicVars = 9; const VelZ: DynamicVars = 10; -const AccX: DynamicVars = 11; -const AccY: DynamicVars = 12; -const AccZ: DynamicVars = 13; -const ForceX: DynamicVars = 14; -const ForceY: DynamicVars = 15; -const ForceZ: DynamicVars = 16; -const AngVelX: DynamicVars = 17; -const AngVelY: DynamicVars = 18; -const AngVelZ: DynamicVars = 19; -const AngAccX: DynamicVars = 20; -const AngAccY: DynamicVars = 21; -const AngAccZ: DynamicVars = 22; +const AngVelX: DynamicVars = 11; +const AngVelY: DynamicVars = 12; +const AngVelZ: DynamicVars = 13; +const AccX: DynamicVars = 14; +const AccY: DynamicVars = 15; +const AccZ: DynamicVars = 16; +const AngAccX: DynamicVars = 17; +const AngAccY: DynamicVars = 18; +const AngAccZ: DynamicVars = 19; +const ForceX: DynamicVars = 20; +const ForceY: DynamicVars = 21; +const ForceZ: DynamicVars = 22; +const TorqueX: DynamicVars = 23; +const TorqueY: DynamicVars = 24; +const TorqueZ: DynamicVars = 25; +const DeltaX: DynamicVars = 26; +const DeltaY: DynamicVars = 27; +const DeltaZ: DynamicVars = 28; +const AngDeltaX: DynamicVars = 29; +const AngDeltaY: DynamicVars = 30; +const AngDeltaZ: DynamicVars = 31; +fn DynamicIndex(idx: i32) -> i32 { + return i32(bitcast(Dynamics[Index2D(TensorStrides[20], TensorStrides[21], u32(idx), u32(Index))])); +} fn DynamicPos(idx: i32) -> vec3 { var pos: vec3; - pos.x = Dynamics[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(PosX))]; - pos.y = Dynamics[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(PosY))]; - pos.z = Dynamics[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(PosZ))];return pos; + pos.x = Dynamics[Index2D(TensorStrides[20], TensorStrides[21], u32(idx), u32(PosX))]; + pos.y = Dynamics[Index2D(TensorStrides[20], TensorStrides[21], u32(idx), u32(PosY))]; + pos.z = Dynamics[Index2D(TensorStrides[20], TensorStrides[21], u32(idx), u32(PosZ))];return pos; } -fn SetDynamicPos(idx: i32, pos: vec3) { - Dynamics[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(PosX))] = pos.x; - Dynamics[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(PosY))] = pos.y; - Dynamics[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(PosZ))] = pos.z; +fn DynamicRot(idx: i32) -> vec4 { + var rot: vec4; + rot.x = Dynamics[Index2D(TensorStrides[20], TensorStrides[21], u32(idx), u32(RotX))]; + rot.y = Dynamics[Index2D(TensorStrides[20], TensorStrides[21], u32(idx), u32(RotY))]; + rot.z = Dynamics[Index2D(TensorStrides[20], TensorStrides[21], u32(idx), u32(RotZ))]; + rot.w = Dynamics[Index2D(TensorStrides[20], TensorStrides[21], u32(idx), u32(RotW))];return rot; } -//////// import: "collide.go" +//////// import: "contact.go" alias ContactVars = i32; //enums:enum const ContactA: ContactVars = 0; const ContactB: ContactVars = 1; @@ -92,57 +138,109 @@ const ContactPointY: ContactVars = 6; const ContactPointZ: ContactVars = 7; const ContactDist: ContactVars = 8; +//////// import: "control.go" +alias JointControlVars = i32; //enums:enum +const JointForceX: JointControlVars = 0; +const JointForceY: JointControlVars = 1; +const JointForceZ: JointControlVars = 2; +const JointTorqueX: JointControlVars = 3; +const JointTorqueY: JointControlVars = 4; +const JointTorqueZ: JointControlVars = 5; +const JointTargetPosX: JointControlVars = 6; +const JointTargetPosY: JointControlVars = 7; +const JointTargetPosZ: JointControlVars = 8; +const JointTargetRotX: JointControlVars = 9; +const JointTargetRotY: JointControlVars = 10; +const JointTargetRotZ: JointControlVars = 11; +const JointTargetRotW: JointControlVars = 12; +const JointTargetVelX: JointControlVars = 13; +const JointTargetVelY: JointControlVars = 14; +const JointTargetVelZ: JointControlVars = 15; +const JointTargetAngVelX: JointControlVars = 16; +const JointTargetAngVelY: JointControlVars = 17; +const JointTargetAngVelZ: JointControlVars = 18; +fn JointForce(idx: i32) -> vec3 { var f: vec3;; f.x = Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointForceX))];; f.y = Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointForceY))];; f.z = Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointForceZ))];; return f; } +fn JointTorque(idx: i32) -> vec3 { var f: vec3;; f.x = Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointTorqueX))];; f.y = Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointTorqueY))];; f.z = Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointTorqueZ))];; return f; } + //////// import: "enumgen.go" -const BodyVarsN: BodyVars = 14; -const DynamicVarsN: DynamicVars = 23; +const BodyVarsN: BodyVars = 37; +const DynamicVarsN: DynamicVars = 32; const ContactVarsN: ContactVars = 9; -const GPUVarsN: GPUVars = 5; -const JointVarsN: JointVars = 8; -const JointTypesN: JointTypes = 2; +const JointControlVarsN: JointControlVars = 19; +const GPUVarsN: GPUVars = 6; +const JointVarsN: JointVars = 21; +const JointTypesN: JointTypes = 7; const ShapesN: Shapes = 4; //////// import: "joint.go" alias JointVars = i32; //enums:enum const JointType: JointVars = 0; -const JointA: JointVars = 1; -const JointB: JointVars = 2; -const JointPosX: JointVars = 3; -const JointPosY: JointVars = 4; -const JointPosZ: JointVars = 5; -const JointParamA: JointVars = 6; -const JointParamB: JointVars = 7; +const JointEnabled: JointVars = 1; +const JointParent: JointVars = 2; +const JointChild: JointVars = 3; +const JointAncestor: JointVars = 4; +const JointPPosX: JointVars = 5; +const JointPPosY: JointVars = 6; +const JointPPosZ: JointVars = 7; +const JointPRotX: JointVars = 8; +const JointPRotY: JointVars = 9; +const JointPRotZ: JointVars = 10; +const JointPRotW: JointVars = 11; +const JointCPosX: JointVars = 12; +const JointCPosY: JointVars = 13; +const JointCPosZ: JointVars = 14; +const JointCRotX: JointVars = 15; +const JointCRotY: JointVars = 16; +const JointCRotZ: JointVars = 17; +const JointCRotW: JointVars = 18; +const JointLimitLower: JointVars = 19; +const JointLimitUpper: JointVars = 20; fn GetJointType(idx: i32) -> JointTypes { - return JointTypes(bitcast(Joints[Index2D(TensorStrides[20], TensorStrides[21], u32(idx), u32(JointType))])); + return JointTypes(bitcast(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointType))])); } -fn JointAIndex(idx: i32) -> i32 { - return i32(bitcast(Joints[Index2D(TensorStrides[20], TensorStrides[21], u32(idx), u32(JointA))])); +fn JointParentIndex(idx: i32) -> i32 { + return i32(bitcast(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointParent))])); } -fn JointBIndex(idx: i32) -> i32 { - return i32(bitcast(Joints[Index2D(TensorStrides[20], TensorStrides[21], u32(idx), u32(JointB))])); +fn JointChildIndex(idx: i32) -> i32 { + return i32(bitcast(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointChild))])); } -fn JointPos(idx: i32) -> vec3 { +fn JointPPos(idx: i32) -> vec3 { var pos: vec3; - pos.x = Joints[Index2D(TensorStrides[20], TensorStrides[21], u32(idx), u32(JointPosX))]; - pos.y = Joints[Index2D(TensorStrides[20], TensorStrides[21], u32(idx), u32(JointPosY))]; - pos.z = Joints[Index2D(TensorStrides[20], TensorStrides[21], u32(idx), u32(JointPosZ))];return pos; + pos.x = Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPPosX))]; + pos.y = Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPPosY))]; + pos.z = Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPPosZ))];return pos; } -alias JointTypes = i32; //enums:enum -const Glue: JointTypes = 0; -const Ball: JointTypes = 1; -fn GlueStep(ji: i32,ba: i32,bb: i32) { - var pos = JointPos(ji); - var bap = DynamicPos(ba); - var bbp = pos+(bap); - SetDynamicPos(bb, bbp); +fn JointPRot(idx: i32) -> vec4 { + var rot: vec4; + rot.x = Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPRotX))]; + rot.y = Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPRotY))]; + rot.z = Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPRotZ))]; + rot.w = Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPRotW))];return rot; } +alias JointTypes = i32; //enums:enum +const Prismatic: JointTypes = 0; +const Revolute: JointTypes = 1; +const Ball: JointTypes = 2; +const Fixed: JointTypes = 3; +const Free: JointTypes = 4; +const Distance: JointTypes = 5; +const D6: JointTypes = 6; //////// import: "params.go" struct PhysParams { DynamicsN: i32, JointsN: i32, - Step: f32, - Gravity: f32, - GravityDir: vec4, + Iters: i32, + SoftRelax: f32, + JointLinearRelax: f32, + JointAngularRelax: f32, + JointLinearComply: f32, + JointAngularComply: f32, + ContactRelax: f32, + AngularDamping: f32, + ContactWeighting: i32, + Restitution: i32, + Gravity: vec4, } //////// import: "shapes.go" @@ -152,19 +250,79 @@ const Sphere: Shapes = 1; const Cylinder: Shapes = 2; const Capsule: Shapes = 3; +//////// import: "slmath-quaternion.go" +fn MulQuat(v: vec3, q: vec4) -> vec3 { + var qx = q.x; + var qy = q.y; + var qz = q.z; + var qw = q.w; + var ix = qw*v.x + qy*v.z - qz*v.y; + var iy = qw*v.y + qz*v.x - qx*v.z; + var iz = qw*v.z + qx*v.y - qy*v.x; + var iw = -qx*v.x - qy*v.y - qz*v.z; +return vec3(ix*qw+iw*-qx+iy*-qz-iz*-qy, + iy*qw+iw*-qy+iz*-qx-ix*-qz, + iz*qw+iw*-qz+ix*-qy-iy*-qx); +} +fn MulQuats(a: vec4,b: vec4) -> vec4 { + var q: vec4; + q.x = a.x*b.w + a.w*b.x + a.y*b.z - a.z*b.y; + q.y = a.y*b.w + a.w*b.y + a.z*b.x - a.x*b.z; + q.z = a.z*b.w + a.w*b.z + a.x*b.y - a.y*b.x; + q.w = a.w*b.w - a.x*b.x - a.y*b.y - a.z*b.z;return q; +} + //////// import: "step.go" +fn MulTransforms(aP: vec3, aQ: vec4, bP: vec3, bQ: vec4, oP: ptr>, oQ: ptr>) { + var br = MulQuat(bP, aQ); + *oP = br+(aP); + *oQ = MulQuats(aQ, bQ); +} +fn TransformPoint(xP: vec3, xQ: vec4, p: vec3) -> vec3 { + var dp = MulQuat(p, xQ);return dp+(xP); +} fn StepJoints(i: u32) { //gosl:kernel let pars = Params[0]; var ji = i32(i); if (ji >= pars.JointsN) { return; } - var ba = JointAIndex(ji); - var bb = JointBIndex(ji); + var jpi = JointParentIndex(ji); + var jpbi = DynamicIndex(jpi); + var jci = JointChildIndex(ji); + var jcbi = DynamicIndex(jci); var jt = GetJointType(ji); + var jpP = JointPPos(ji); + var jpQ = JointPRot(ji); + var xwpP = jpP; + var xwpQ = jpQ; + var posepP = jpP; + var posepQ = jpQ; + var comp: vec3; + if (jpi >= 0) { // can be fixed + posepP = DynamicPos(jpi); + posepQ = DynamicRot(jpi); + MulTransforms(posepP, posepQ, jpP, jpQ, &xwpP, &xwpQ); + comp = BodyCom(jpbi); + } + var rp = xwpP-(TransformPoint(posepP, posepQ, comp)); // parent moment arm + var posecP = DynamicPos(jci); + var posecQ = DynamicRot(jci); + var xwcP = posecP; + var xwcQ = posecQ; + var comc = BodyCom(jcbi); + var rc = xwcP-(TransformPoint(posecP, posecQ, comc)); // child moment arm + var jf = JointForce(ji); + var jtq = JointTorque(ji); + var f: vec3; + var t: vec3; switch (jt) { - case Glue: { - GlueStep(ji, ba, bb); + case Free, Distance: { + f = jf; + t = jtq; + } + case Ball: { + t = jtq; } default: { } diff --git a/physics/step.go b/physics/step.go index 85516cad..b6363b14 100644 --- a/physics/step.go +++ b/physics/step.go @@ -6,7 +6,28 @@ package physics +import ( + "cogentcore.org/core/math32" + "cogentcore.org/lab/gosl/slmath" +) + //gosl:start +//gosl:import "cogentcore.org/lab/gosl/slmath" + +// MulTransforms computes the equivalent of matrix multiplication for +// two quat-based transforms, o = a * b +func MulTransforms(aP math32.Vector3, aQ math32.Quat, bP math32.Vector3, bQ math32.Quat, oP *math32.Vector3, oQ *math32.Quat) { + // rotate b by a and add a + br := slmath.MulQuat(bP, aQ) + *oP = br.Add(aP) + *oQ = slmath.MulQuats(aQ, bQ) +} + +// TransformPoint applies quat-based transform to given point +func TransformPoint(xP math32.Vector3, xQ math32.Quat, p math32.Vector3) math32.Vector3 { + dp := slmath.MulQuat(p, xQ) + return dp.Add(xP) +} // InitDynamics copies Body initial state to dynamic state. func InitDynamics(i uint32) { //gosl:kernel @@ -37,12 +58,49 @@ func StepJoints(i uint32) { //gosl:kernel if ji >= pars.JointsN { return } - ba := JointAIndex(ji) - bb := JointBIndex(ji) + // todo: enabled + jpi := JointParentIndex(ji) + jpbi := DynamicIndex(jpi) + jci := JointChildIndex(ji) + jcbi := DynamicIndex(jci) jt := GetJointType(ji) + + jpP := JointPPos(ji) + jpQ := JointPRot(ji) + + // parent world transform + xwpP := jpP + xwpQ := jpQ + posepP := jpP + posepQ := jpQ + var comp math32.Vector3 + + if jpi >= 0 { // can be fixed + posepP = DynamicPos(jpi) + posepQ = DynamicRot(jpi) + MulTransforms(posepP, posepQ, jpP, jpQ, &xwpP, &xwpQ) + comp = BodyCom(jpbi) + } + rp := xwpP.Sub(TransformPoint(posepP, posepQ, comp)) // parent moment arm + + // child world transform + posecP := DynamicPos(jci) + posecQ := DynamicRot(jci) + xwcP := posecP + xwcQ := posecQ + comc := BodyCom(jcbi) + rc := xwcP.Sub(TransformPoint(posecP, posecQ, comc)) // child moment arm + + jf := JointForce(ji) + jtq := JointTorque(ji) + + var f, t math32.Vector3 switch jt { - case Glue: - GlueStep(ji, ba, bb) + case Free, Distance: + f = jf + t = jtq + case Ball: + t = jtq default: } } diff --git a/physics/step.goal b/physics/step.goal index bd1a723f..78320865 100644 --- a/physics/step.goal +++ b/physics/step.goal @@ -4,7 +4,28 @@ package physics +import ( + "cogentcore.org/core/math32" + "cogentcore.org/lab/gosl/slmath" +) + //gosl:start +//gosl:import "cogentcore.org/lab/gosl/slmath" + +// MulTransforms computes the equivalent of matrix multiplication for +// two quat-based transforms, o = a * b +func MulTransforms(aP math32.Vector3, aQ math32.Quat, bP math32.Vector3, bQ math32.Quat, oP *math32.Vector3, oQ *math32.Quat) { + // rotate b by a and add a + br := slmath.MulQuat(bP, aQ) + *oP = br.Add(aP) + *oQ = slmath.MulQuats(aQ, bQ) +} + +// TransformPoint applies quat-based transform to given point +func TransformPoint(xP math32.Vector3, xQ math32.Quat, p math32.Vector3) math32.Vector3 { + dp := slmath.MulQuat(p, xQ) + return dp.Add(xP) +} // InitDynamics copies Body initial state to dynamic state. func InitDynamics(i uint32) { //gosl:kernel @@ -35,12 +56,49 @@ func StepJoints(i uint32) { //gosl:kernel if ji >= pars.JointsN { return } - ba := JointAIndex(ji) - bb := JointBIndex(ji) + // todo: enabled + jpi := JointParentIndex(ji) + jpbi := DynamicIndex(jpi) + jci := JointChildIndex(ji) + jcbi := DynamicIndex(jci) jt := GetJointType(ji) + + jpP := JointPPos(ji) + jpQ := JointPRot(ji) + + // parent world transform + xwpP := jpP + xwpQ := jpQ + posepP := jpP + posepQ := jpQ + var comp math32.Vector3 + + if jpi >= 0 { // can be fixed + posepP = DynamicPos(jpi) + posepQ = DynamicRot(jpi) + MulTransforms(posepP, posepQ, jpP, jpQ, &xwpP, &xwpQ) + comp = BodyCom(jpbi) + } + rp := xwpP.Sub(TransformPoint(posepP, posepQ, comp)) // parent moment arm + + // child world transform + posecP := DynamicPos(jci) + posecQ := DynamicRot(jci) + xwcP := posecP + xwcQ := posecQ + comc := BodyCom(jcbi) + rc := xwcP.Sub(TransformPoint(posecP, posecQ, comc)) // child moment arm + + jf := JointForce(ji) + jtq := JointTorque(ji) + + var f, t math32.Vector3 switch jt { - case Glue: - GlueStep(ji, ba, bb) + case Free, Distance: + f = jf + t = jtq + case Ball: + t = jtq default: } } diff --git a/physics/typegen.go b/physics/typegen.go index c6772a9c..b73ff167 100644 --- a/physics/typegen.go +++ b/physics/typegen.go @@ -14,16 +14,18 @@ var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.DynamicVars" var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.ContactVars", IDName: "contact-vars", Doc: "Contact is one pairwise point of contact between two bodies.\nContacts are represented in spherical terms relative to the\nspherical BBox of A and B."}) +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.JointControlVars", IDName: "joint-control-vars", Doc: "JointControlVars are external joint control input variables stored in tensor.Float32.\nThese must be in one-to-one correspondence with the joints."}) + var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.GPUVars", IDName: "gpu-vars", Doc: "GPUVars is an enum for GPU variables, for specifying what to sync."}) -var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.JointVars", IDName: "joint-vars", Doc: "JointVars are joint state variables stored in tensor.Float32"}) +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.JointVars", IDName: "joint-vars", Doc: "JointVars are joint state variables stored in tensor.Float32.\nThese are all static joint properties; dynamic control variables\nin [JointControlVars] and [JointControls]."}) var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.JointTypes", IDName: "joint-types", Doc: "JointTypes are joint types that determine nature of interaction."}) -var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.PhysParams", IDName: "phys-params", Doc: "PhysParams are the physics parameters", Directives: []types.Directive{{Tool: "gosl", Directive: "start"}}, Fields: []types.Field{{Name: "DynamicsN", Doc: "DynamicsN is number of dynamics bodies."}, {Name: "JointsN", Doc: "JointsN is number of joints."}, {Name: "Step", Doc: "Step is the global stepsize for numerical integration."}, {Name: "Gravity", Doc: "Gravity is the force of gravity."}, {Name: "GravityDir", Doc: "GravityDir is the direction of Gravity."}}}) +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.PhysParams", IDName: "phys-params", Doc: "PhysParams are the physics parameters", Directives: []types.Directive{{Tool: "gosl", Directive: "start"}}, Fields: []types.Field{{Name: "DynamicsN", Doc: "DynamicsN is number of dynamics bodies."}, {Name: "JointsN", Doc: "JointsN is number of joints."}, {Name: "Iters", Doc: "Iters is the number of iterations to perform."}, {Name: "SoftRelax", Doc: "SoftRelax is soft-body relaxation constant."}, {Name: "JointLinearRelax", Doc: "JointLinearRelax is joint linear relaxation constant."}, {Name: "JointAngularRelax", Doc: "JointAngularRelax is joint angular relaxation constant."}, {Name: "JointLinearComply", Doc: "JointLinearComply is joint linear compliance constant."}, {Name: "JointAngularComply", Doc: "JointAngularComply is joint angular compliance constant."}, {Name: "ContactRelax", Doc: "ContactRelax is rigid contact relaxation constant."}, {Name: "AngularDamping", Doc: "AngularDamping is damping of angular motion."}, {Name: "ContactWeighting", Doc: "Contact weighting"}, {Name: "Restitution", Doc: "Restitution"}, {Name: "Gravity", Doc: "Gravity is the gravity acceleration function"}}}) var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.Shapes", IDName: "shapes", Doc: "Shapes are elemental shapes for rigid bodies."}) var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.State", IDName: "state", Doc: "State contains the basic physical state including position, orientation, velocity.\nThese are only the values that can be either relative or absolute -- other physical\nstate values such as Mass should go in Rigid.", Methods: []types.Method{{Name: "MoveOnAxis", Doc: "MoveOnAxis moves (translates) the specified distance on the specified local axis,\nrelative to the current rotation orientation.\nThe axis is normalized prior to aplying the distance factor.\nSets the LinVel to motion vector.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z", "dist"}}, {Name: "MoveOnAxisAbs", Doc: "MoveOnAxisAbs moves (translates) the specified distance on the specified local axis,\nin absolute X,Y,Z coordinates (does not apply the Quat rotation factor.\nThe axis is normalized prior to aplying the distance factor.\nSets the LinVel to motion vector.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z", "dist"}}, {Name: "SetEulerRotation", Doc: "SetEulerRotation sets the rotation in Euler angles (degrees).", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z"}}, {Name: "EulerRotation", Doc: "EulerRotation returns the current rotation in Euler angles (degrees).", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Returns: []string{"Vector3"}}, {Name: "SetAxisRotation", Doc: "SetAxisRotation sets rotation from local axis and angle in degrees.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z", "angle"}}, {Name: "RotateOnAxis", Doc: "RotateOnAxis rotates around the specified local axis the specified angle in degrees.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z", "angle"}}, {Name: "RotateEuler", Doc: "RotateEuler rotates by given Euler angles (in degrees) relative to existing rotation.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z"}}}, Fields: []types.Field{{Name: "Pos", Doc: "Pos is the position of center of mass of object."}, {Name: "Quat", Doc: "Quat is the rotation specified as a quaternion."}, {Name: "LinVel", Doc: "LinVel is the linear velocity."}, {Name: "AngVel", Doc: "AngVel is the angular velocity."}}}) -var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.World", IDName: "world", Doc: "World contains and manages all of the physics elements.", Directives: []types.Directive{{Tool: "go", Directive: "generate", Args: []string{"core", "generate", "-add-types", "-gosl"}}}, Fields: []types.Field{{Name: "GPU", Doc: "GPU determines whether to use GPU (else CPU)."}, {Name: "Params", Doc: "Params are global parameters."}, {Name: "Bodies", Doc: "Bodies are the rigid body elements (dynamic and static),\nspecifying the constant, non-dynamic properties,\nwhich is initial state for dynamics.\n[body][BodyVarsN]"}, {Name: "Dynamics", Doc: "Dynamics are the dynamic rigid body elements: these actually move.\nThe first set of variables are for initial values, and the second current.\n[body][DynamicVarsN]"}, {Name: "Joints", Doc: "Joints is a list of permanent joints connecting bodies.\n[joint][JointVarsN]"}, {Name: "Contacts", Doc: "Contacts are points of contact between bodies.\n[contact][ContactVarsN]"}}}) +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.World", IDName: "world", Doc: "World contains and manages all of the physics elements.", Directives: []types.Directive{{Tool: "go", Directive: "generate", Args: []string{"core", "generate", "-add-types", "-gosl"}}}, Fields: []types.Field{{Name: "GPU", Doc: "GPU determines whether to use GPU (else CPU)."}, {Name: "Params", Doc: "Params are global parameters."}, {Name: "Bodies", Doc: "Bodies are the rigid body elements (dynamic and static),\nspecifying the constant, non-dynamic properties,\nwhich is initial state for dynamics.\n[body][BodyVarsN]"}, {Name: "Joints", Doc: "Joints is a list of permanent joints connecting bodies,\nwhich do not change (no dynamic variables).\n[joint][JointVarsN]"}, {Name: "Dynamics", Doc: "Dynamics are the dynamic rigid body elements: these actually move.\nThe first set of variables are for initial values, and the second current.\n[body][DynamicVarsN]"}, {Name: "Contacts", Doc: "Contacts are points of contact between bodies.\n[contact][ContactVarsN]"}, {Name: "JointControls", Doc: "JointControls are dynamic joint control inputs.\n[joint][JointControlVarsN]"}}}) diff --git a/physics/vars.go b/physics/vars.go index a7347446..ee407021 100644 --- a/physics/vars.go +++ b/physics/vars.go @@ -27,19 +27,26 @@ var ( //gosl:dims 2 Bodies *tensor.Float32 + // Joints is a list of permanent joints connecting bodies, + // which do not change (no dynamic variables). + // [joint][JointVars] + //gosl:dims 2 + Joints *tensor.Float32 + // Dynamics are the dynamic rigid body elements: these actually move. // [body][DynamicVarsN] + //gosl:group Bodies //gosl:dims 2 Dynamics *tensor.Float32 - // Joints is a list of permanent joints connecting bodies. - // [joint][JointVars] - //gosl:dims 2 - Joints *tensor.Float32 - // Contacts are points of contact between bodies. // [contact][ContactVarsN] - //gosl:group Contacts //gosl:dims 2 Contacts *tensor.Float32 + + // JointControls are dynamic joint control inputs. + // [joint][JointControlVarsN] + //gosl:group Controls + //gosl:dims 2 + JointControls *tensor.Float32 ) diff --git a/physics/world.go b/physics/world.go index 363338c8..b3d2443a 100644 --- a/physics/world.go +++ b/physics/world.go @@ -27,18 +27,23 @@ type World struct { // [body][BodyVarsN] Bodies *tensor.Float32 + // Joints is a list of permanent joints connecting bodies, + // which do not change (no dynamic variables). + // [joint][JointVarsN] + Joints *tensor.Float32 + // Dynamics are the dynamic rigid body elements: these actually move. // The first set of variables are for initial values, and the second current. // [body][DynamicVarsN] Dynamics *tensor.Float32 - // Joints is a list of permanent joints connecting bodies. - // [joint][JointVarsN] - Joints *tensor.Float32 - // Contacts are points of contact between bodies. // [contact][ContactVarsN] Contacts *tensor.Float32 + + // JointControls are dynamic joint control inputs. + // [joint][JointControlVarsN] + JointControls *tensor.Float32 } func NewWorld() *World { @@ -51,9 +56,10 @@ func NewWorld() *World { func (wl *World) Init() { wl.Params = []PhysParams{} wl.Bodies = tensor.NewFloat32(0, int(BodyVarsN)) - wl.Dynamics = tensor.NewFloat32(0, int(DynamicVarsN)) wl.Joints = tensor.NewFloat32(0, int(JointVarsN)) + wl.Dynamics = tensor.NewFloat32(0, int(DynamicVarsN)) wl.Contacts = tensor.NewFloat32(0, int(ContactVarsN)) + wl.JointControls = tensor.NewFloat32(0, int(JointControlVarsN)) wl.SetAsCurrentVars() } @@ -81,14 +87,14 @@ func (wl *World) NewDynamic(shape Shapes, size, pos math32.Vector3, rot math32.Q return } -// NewJoint adds a new joint between two objects. -func (wl *World) NewJoint(joint JointTypes, bodyA, bodyB int32, pos math32.Vector3) int32 { +// NewJoint adds a new joint between parent and child dynamic object indexes. +func (wl *World) NewJoint(joint JointTypes, parent, child int32, pos math32.Vector3) int32 { sizes := wl.Joints.ShapeSizes() idx := int32(sizes[0]) wl.Joints.SetShapeSizes(int(idx+1), int(JointVarsN)) - SetJointA(idx, bodyA) - SetJointB(idx, bodyB) - SetJointPos(idx, pos) + SetJointParent(idx, parent) + SetJointChild(idx, child) + SetJointPPos(idx, pos) wl.Params[0].JointsN = idx + 1 return idx } @@ -110,9 +116,10 @@ func (wl *World) SetAsCurrent() { func (wl *World) SetAsCurrentVars() { Params = wl.Params Bodies = wl.Bodies - Dynamics = wl.Dynamics Joints = wl.Joints + Dynamics = wl.Dynamics Contacts = wl.Contacts + JointControls = wl.JointControls } // GPUInit initializes the GPU and transfers Infra. diff --git a/physics/world.goal b/physics/world.goal index b5019d56..6ecd7ed8 100644 --- a/physics/world.goal +++ b/physics/world.goal @@ -25,18 +25,23 @@ type World struct { // [body][BodyVarsN] Bodies *tensor.Float32 + // Joints is a list of permanent joints connecting bodies, + // which do not change (no dynamic variables). + // [joint][JointVarsN] + Joints *tensor.Float32 + // Dynamics are the dynamic rigid body elements: these actually move. // The first set of variables are for initial values, and the second current. // [body][DynamicVarsN] Dynamics *tensor.Float32 - // Joints is a list of permanent joints connecting bodies. - // [joint][JointVarsN] - Joints *tensor.Float32 - // Contacts are points of contact between bodies. // [contact][ContactVarsN] Contacts *tensor.Float32 + + // JointControls are dynamic joint control inputs. + // [joint][JointControlVarsN] + JointControls *tensor.Float32 } func NewWorld() *World { @@ -49,9 +54,10 @@ func NewWorld() *World { func (wl *World) Init() { wl.Params = []PhysParams{} wl.Bodies = tensor.NewFloat32(0, int(BodyVarsN)) - wl.Dynamics = tensor.NewFloat32(0, int(DynamicVarsN)) wl.Joints = tensor.NewFloat32(0, int(JointVarsN)) + wl.Dynamics = tensor.NewFloat32(0, int(DynamicVarsN)) wl.Contacts = tensor.NewFloat32(0, int(ContactVarsN)) + wl.JointControls = tensor.NewFloat32(0, int(JointControlVarsN)) wl.SetAsCurrentVars() } @@ -79,14 +85,14 @@ func (wl *World) NewDynamic(shape Shapes, size, pos math32.Vector3, rot math32.Q return } -// NewJoint adds a new joint between two objects. -func (wl *World) NewJoint(joint JointTypes, bodyA, bodyB int32, pos math32.Vector3) int32 { +// NewJoint adds a new joint between parent and child dynamic object indexes. +func (wl *World) NewJoint(joint JointTypes, parent, child int32, pos math32.Vector3) int32 { sizes := wl.Joints.ShapeSizes() idx := int32(sizes[0]) wl.Joints.SetShapeSizes(int(idx+1), int(JointVarsN)) - SetJointA(idx, bodyA) - SetJointB(idx, bodyB) - SetJointPos(idx, pos) + SetJointParent(idx, parent) + SetJointChild(idx, child) + SetJointPPos(idx, pos) wl.Params[0].JointsN = idx + 1 return idx } @@ -108,9 +114,10 @@ func (wl *World) SetAsCurrent() { func (wl *World) SetAsCurrentVars() { Params = wl.Params Bodies = wl.Bodies - Dynamics = wl.Dynamics Joints = wl.Joints + Dynamics = wl.Dynamics Contacts = wl.Contacts + JointControls = wl.JointControls } // GPUInit initializes the GPU and transfers Infra. From 2fd8a585982017abee5fde83727efd153debd085 Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Sun, 14 Dec 2025 21:27:56 +0100 Subject: [PATCH 13/97] physics: stepjoints compiles on cpu and gpu --- physics/body.go | 6 +- physics/body.goal | 6 +- physics/enumgen.go | 22 +-- physics/gosl.go | 81 ++++------- physics/joint.go | 107 +++++++++++++-- physics/joint.goal | 107 +++++++++++++-- physics/shaders/InitDynamics.wgsl | 74 ++++++---- physics/shaders/Step.wgsl | 215 ------------------------------ physics/shaders/StepJoints.wgsl | 133 ++++++++++++------ physics/step.go | 33 +++-- physics/step.goal | 33 +++-- physics/typegen.go | 2 +- physics/vars.go | 9 +- physics/world.go | 12 +- physics/world.goal | 12 +- 15 files changed, 428 insertions(+), 424 deletions(-) delete mode 100644 physics/shaders/Step.wgsl diff --git a/physics/body.go b/physics/body.go index 0d230163..1829592c 100644 --- a/physics/body.go +++ b/physics/body.go @@ -189,14 +189,12 @@ const ( AngAccY AngAccZ - // Linear force driving linear acceleration. - // joints write to this using atomic. must clear after each step. + // Linear force driving linear acceleration (from joints, etc). ForceX ForceY ForceZ - // Linear force driving linear acceleration. - // joints write to this using atomic. must clear after each step. + // Torque driving angular acceleration (from joints, etc). TorqueX TorqueY TorqueZ diff --git a/physics/body.goal b/physics/body.goal index f24577d4..ff04c397 100644 --- a/physics/body.goal +++ b/physics/body.goal @@ -187,14 +187,12 @@ const ( AngAccY AngAccZ - // Linear force driving linear acceleration. - // joints write to this using atomic. must clear after each step. + // Linear force driving linear acceleration (from joints, etc). ForceX ForceY ForceZ - // Linear force driving linear acceleration. - // joints write to this using atomic. must clear after each step. + // Torque driving angular acceleration (from joints, etc). TorqueX TorqueY TorqueZ diff --git a/physics/enumgen.go b/physics/enumgen.go index cafdcd98..3319d1bb 100644 --- a/physics/enumgen.go +++ b/physics/enumgen.go @@ -62,7 +62,7 @@ const DynamicVarsN DynamicVars = 32 var _DynamicVarsValueMap = map[string]DynamicVars{`Index`: 0, `PosX`: 1, `PosY`: 2, `PosZ`: 3, `RotX`: 4, `RotY`: 5, `RotZ`: 6, `RotW`: 7, `VelX`: 8, `VelY`: 9, `VelZ`: 10, `AngVelX`: 11, `AngVelY`: 12, `AngVelZ`: 13, `AccX`: 14, `AccY`: 15, `AccZ`: 16, `AngAccX`: 17, `AngAccY`: 18, `AngAccZ`: 19, `ForceX`: 20, `ForceY`: 21, `ForceZ`: 22, `TorqueX`: 23, `TorqueY`: 24, `TorqueZ`: 25, `DeltaX`: 26, `DeltaY`: 27, `DeltaZ`: 28, `AngDeltaX`: 29, `AngDeltaY`: 30, `AngDeltaZ`: 31} -var _DynamicVarsDescMap = map[DynamicVars]string{0: `Index of body in list of bodies.`, 1: `3D position of center of mass.`, 2: ``, 3: ``, 4: `Quaternion rotation.`, 5: ``, 6: ``, 7: ``, 8: `Linear velocity.`, 9: ``, 10: ``, 11: `Angular velocity.`, 12: ``, 13: ``, 14: `Linear acceleration.`, 15: ``, 16: ``, 17: `Angular acceleration due to applied torques.`, 18: ``, 19: ``, 20: `Linear force driving linear acceleration. joints write to this using atomic. must clear after each step.`, 21: ``, 22: ``, 23: `Linear force driving linear acceleration. joints write to this using atomic. must clear after each step.`, 24: ``, 25: ``, 26: `Linear deltas.`, 27: ``, 28: ``, 29: `Angular deltas.`, 30: ``, 31: ``} +var _DynamicVarsDescMap = map[DynamicVars]string{0: `Index of body in list of bodies.`, 1: `3D position of center of mass.`, 2: ``, 3: ``, 4: `Quaternion rotation.`, 5: ``, 6: ``, 7: ``, 8: `Linear velocity.`, 9: ``, 10: ``, 11: `Angular velocity.`, 12: ``, 13: ``, 14: `Linear acceleration.`, 15: ``, 16: ``, 17: `Angular acceleration due to applied torques.`, 18: ``, 19: ``, 20: `Linear force driving linear acceleration (from joints, etc).`, 21: ``, 22: ``, 23: `Torque driving angular acceleration (from joints, etc).`, 24: ``, 25: ``, 26: `Linear deltas.`, 27: ``, 28: ``, 29: `Angular deltas.`, 30: ``, 31: ``} var _DynamicVarsMap = map[DynamicVars]string{0: `Index`, 1: `PosX`, 2: `PosY`, 3: `PosZ`, 4: `RotX`, 5: `RotY`, 6: `RotZ`, 7: `RotW`, 8: `VelX`, 9: `VelY`, 10: `VelZ`, 11: `AngVelX`, 12: `AngVelY`, 13: `AngVelZ`, 14: `AccX`, 15: `AccY`, 16: `AccZ`, 17: `AngAccX`, 18: `AngAccY`, 19: `AngAccZ`, 20: `ForceX`, 21: `ForceY`, 22: `ForceZ`, 23: `TorqueX`, 24: `TorqueY`, 25: `TorqueZ`, 26: `DeltaX`, 27: `DeltaY`, 28: `DeltaZ`, 29: `AngDeltaX`, 30: `AngDeltaY`, 31: `AngDeltaZ`} @@ -192,20 +192,20 @@ func (i *JointControlVars) UnmarshalText(text []byte) error { return enums.UnmarshalText(i, text, "JointControlVars") } -var _GPUVarsValues = []GPUVars{0, 1, 2, 3, 4, 5} +var _GPUVarsValues = []GPUVars{0, 1, 2, 3, 4, 5, 6} // GPUVarsN is the highest valid value for type GPUVars, plus one. // //gosl:start -const GPUVarsN GPUVars = 6 +const GPUVarsN GPUVars = 7 //gosl:end -var _GPUVarsValueMap = map[string]GPUVars{`ParamsVar`: 0, `BodiesVar`: 1, `JointsVar`: 2, `DynamicsVar`: 3, `ContactsVar`: 4, `JointControlsVar`: 5} +var _GPUVarsValueMap = map[string]GPUVars{`ParamsVar`: 0, `BodiesVar`: 1, `JointsVar`: 2, `BodyJointsVar`: 3, `DynamicsVar`: 4, `ContactsVar`: 5, `JointControlsVar`: 6} -var _GPUVarsDescMap = map[GPUVars]string{0: ``, 1: ``, 2: ``, 3: ``, 4: ``, 5: ``} +var _GPUVarsDescMap = map[GPUVars]string{0: ``, 1: ``, 2: ``, 3: ``, 4: ``, 5: ``, 6: ``} -var _GPUVarsMap = map[GPUVars]string{0: `ParamsVar`, 1: `BodiesVar`, 2: `JointsVar`, 3: `DynamicsVar`, 4: `ContactsVar`, 5: `JointControlsVar`} +var _GPUVarsMap = map[GPUVars]string{0: `ParamsVar`, 1: `BodiesVar`, 2: `JointsVar`, 3: `BodyJointsVar`, 4: `DynamicsVar`, 5: `ContactsVar`, 6: `JointControlsVar`} // String returns the string representation of this GPUVars value. func (i GPUVars) String() string { return enums.String(i, _GPUVarsMap) } @@ -237,20 +237,20 @@ func (i GPUVars) MarshalText() ([]byte, error) { return []byte(i.String()), nil // UnmarshalText implements the [encoding.TextUnmarshaler] interface. func (i *GPUVars) UnmarshalText(text []byte) error { return enums.UnmarshalText(i, text, "GPUVars") } -var _JointVarsValues = []JointVars{0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20} +var _JointVarsValues = []JointVars{0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34} // JointVarsN is the highest valid value for type JointVars, plus one. // //gosl:start -const JointVarsN JointVars = 21 +const JointVarsN JointVars = 35 //gosl:end -var _JointVarsValueMap = map[string]JointVars{`JointType`: 0, `JointEnabled`: 1, `JointParent`: 2, `JointChild`: 3, `JointAncestor`: 4, `JointPPosX`: 5, `JointPPosY`: 6, `JointPPosZ`: 7, `JointPRotX`: 8, `JointPRotY`: 9, `JointPRotZ`: 10, `JointPRotW`: 11, `JointCPosX`: 12, `JointCPosY`: 13, `JointCPosZ`: 14, `JointCRotX`: 15, `JointCRotY`: 16, `JointCRotZ`: 17, `JointCRotW`: 18, `JointLimitLower`: 19, `JointLimitUpper`: 20} +var _JointVarsValueMap = map[string]JointVars{`JointType`: 0, `JointEnabled`: 1, `JointParent`: 2, `JointChild`: 3, `JointPPosX`: 4, `JointPPosY`: 5, `JointPPosZ`: 6, `JointPRotX`: 7, `JointPRotY`: 8, `JointPRotZ`: 9, `JointPRotW`: 10, `JointCPosX`: 11, `JointCPosY`: 12, `JointCPosZ`: 13, `JointCRotX`: 14, `JointCRotY`: 15, `JointCRotZ`: 16, `JointCRotW`: 17, `JointAxisX`: 18, `JointAxisY`: 19, `JointAxisZ`: 20, `JointLimitLower`: 21, `JointLimitUpper`: 22, `JointPForceX`: 23, `JointPForceY`: 24, `JointPForceZ`: 25, `JointPTorqueX`: 26, `JointPTorqueY`: 27, `JointPTorqueZ`: 28, `JointCForceX`: 29, `JointCForceY`: 30, `JointCForceZ`: 31, `JointCTorqueX`: 32, `JointCTorqueY`: 33, `JointCTorqueZ`: 34} -var _JointVarsDescMap = map[JointVars]string{0: `JointType (as an int32 from bits).`, 1: `JointEnabled allows joints to be dynamically enabled.`, 2: `JointParent is the dynamic body index for parent body. Can be -1 for a fixed parent for absolute anchor.`, 3: `JointChild is the dynamic body index for child body.`, 4: `JointAncestor is the joint index where current parent is a child.`, 5: `position of joint, in parent frame.`, 6: ``, 7: ``, 8: `orientation of joint, in parent frame.`, 9: ``, 10: ``, 11: ``, 12: `position of joint, in child frame.`, 13: ``, 14: ``, 15: `orientation of joint, in child frame.`, 16: ``, 17: ``, 18: ``, 19: `joint limits`, 20: ``} +var _JointVarsDescMap = map[JointVars]string{0: `JointType (as an int32 from bits).`, 1: `JointEnabled allows joints to be dynamically enabled.`, 2: `JointParent is the dynamic body index for parent body. Can be -1 for a fixed parent for absolute anchor.`, 3: `JointChild is the dynamic body index for child body.`, 4: `position of joint, in parent frame.`, 5: ``, 6: ``, 7: `orientation of joint, in parent frame.`, 8: ``, 9: ``, 10: ``, 11: `position of joint, in child frame.`, 12: ``, 13: ``, 14: `orientation of joint, in child frame.`, 15: ``, 16: ``, 17: ``, 18: ``, 19: ``, 20: ``, 21: `joint limits`, 22: ``, 23: `Computed parent joint force value.`, 24: ``, 25: ``, 26: `Computed parent joint torque value.`, 27: ``, 28: ``, 29: `Computed child joint force value.`, 30: ``, 31: ``, 32: `Computed child joint torque value.`, 33: ``, 34: ``} -var _JointVarsMap = map[JointVars]string{0: `JointType`, 1: `JointEnabled`, 2: `JointParent`, 3: `JointChild`, 4: `JointAncestor`, 5: `JointPPosX`, 6: `JointPPosY`, 7: `JointPPosZ`, 8: `JointPRotX`, 9: `JointPRotY`, 10: `JointPRotZ`, 11: `JointPRotW`, 12: `JointCPosX`, 13: `JointCPosY`, 14: `JointCPosZ`, 15: `JointCRotX`, 16: `JointCRotY`, 17: `JointCRotZ`, 18: `JointCRotW`, 19: `JointLimitLower`, 20: `JointLimitUpper`} +var _JointVarsMap = map[JointVars]string{0: `JointType`, 1: `JointEnabled`, 2: `JointParent`, 3: `JointChild`, 4: `JointPPosX`, 5: `JointPPosY`, 6: `JointPPosZ`, 7: `JointPRotX`, 8: `JointPRotY`, 9: `JointPRotZ`, 10: `JointPRotW`, 11: `JointCPosX`, 12: `JointCPosY`, 13: `JointCPosZ`, 14: `JointCRotX`, 15: `JointCRotY`, 16: `JointCRotZ`, 17: `JointCRotW`, 18: `JointAxisX`, 19: `JointAxisY`, 20: `JointAxisZ`, 21: `JointLimitLower`, 22: `JointLimitUpper`, 23: `JointPForceX`, 24: `JointPForceY`, 25: `JointPForceZ`, 26: `JointPTorqueX`, 27: `JointPTorqueY`, 28: `JointPTorqueZ`, 29: `JointCForceX`, 30: `JointCForceY`, 31: `JointCForceZ`, 32: `JointCTorqueX`, 33: `JointCTorqueY`, 34: `JointCTorqueZ`} // String returns the string representation of this JointVars value. func (i JointVars) String() string { return enums.String(i, _JointVarsMap) } diff --git a/physics/gosl.go b/physics/gosl.go index 93b52ddb..dd6b7a04 100644 --- a/physics/gosl.go +++ b/physics/gosl.go @@ -42,9 +42,10 @@ const ( ParamsVar GPUVars = 0 BodiesVar GPUVars = 1 JointsVar GPUVars = 2 - DynamicsVar GPUVars = 3 - ContactsVar GPUVars = 4 - JointControlsVar GPUVars = 5 + BodyJointsVar GPUVars = 3 + DynamicsVar GPUVars = 4 + ContactsVar GPUVars = 5 + JointControlsVar GPUVars = 6 ) // Tensor stride variables @@ -86,6 +87,7 @@ func GPUInit() { _ = vr vr = sgp.Add("Bodies", gpu.Float32, 1, gpu.ComputeShader) vr = sgp.Add("Joints", gpu.Float32, 1, gpu.ComputeShader) + vr = sgp.Add("BodyJoints", gpu.Int32, 1, gpu.ComputeShader) sgp.SetNValues(1) } { @@ -109,10 +111,6 @@ func GPUInit() { pl.AddVarUsed(1, "Bodies") pl.AddVarUsed(2, "Dynamics") pl.AddVarUsed(0, "Params") - pl = gpu.NewComputePipelineShaderFS(shaders, "shaders/Step.wgsl", sy) - pl.AddVarUsed(0, "TensorStrides") - pl.AddVarUsed(2, "Dynamics") - pl.AddVarUsed(0, "Params") pl = gpu.NewComputePipelineShaderFS(shaders, "shaders/StepJoints.wgsl", sy) pl.AddVarUsed(0, "TensorStrides") pl.AddVarUsed(1, "Bodies") @@ -180,48 +178,6 @@ func RunOneInitDynamics(n int, syncVars ...GPUVars) { RunInitDynamicsCPU(n) } } -// RunStep runs the Step kernel with given number of elements, -// on either the CPU or GPU depending on the UseGPU variable. -// Can call multiple Run* kernels in a row, which are then all launched -// in the same command submission on the GPU, which is by far the most efficient. -// MUST call RunDone (with optional vars to sync) after all Run calls. -// Alternatively, a single-shot RunOneStep call does Run and Done for a -// single run-and-sync case. -func RunStep(n int) { - if UseGPU { - RunStepGPU(n) - } else { - RunStepCPU(n) - } -} - -// RunStepGPU runs the Step kernel on the GPU. See [RunStep] for more info. -func RunStepGPU(n int) { - sy := GPUSystem - pl := sy.ComputePipelines["Step"] - ce, _ := sy.BeginComputePass() - pl.Dispatch1D(ce, n, 64) -} - -// RunStepCPU runs the Step kernel on the CPU. -func RunStepCPU(n int) { - gpu.VectorizeFunc(0, n, Step) -} - -// RunOneStep runs the Step kernel with given number of elements, -// on either the CPU or GPU depending on the UseGPU variable. -// This version then calls RunDone with the given variables to sync -// after the Run, for a single-shot Run-and-Done call. If multiple kernels -// can be run in sequence, it is much more efficient to do multiple Run* -// calls followed by a RunDone call. -func RunOneStep(n int, syncVars ...GPUVars) { - if UseGPU { - RunStepGPU(n) - RunDone(syncVars...) - } else { - RunStepCPU(n) - } -} // RunStepJoints runs the StepJoints kernel with given number of elements, // on either the CPU or GPU depending on the UseGPU variable. // Can call multiple Run* kernels in a row, which are then all launched @@ -298,6 +254,9 @@ func ToGPU(vars ...GPUVars) { case JointsVar: v, _ := syVars.ValueByIndex(1, "Joints", 0) gpu.SetValueFrom(v, Joints.Values) + case BodyJointsVar: + v, _ := syVars.ValueByIndex(1, "BodyJoints", 0) + gpu.SetValueFrom(v, BodyJoints.Values) case DynamicsVar: v, _ := syVars.ValueByIndex(2, "Dynamics", 0) gpu.SetValueFrom(v, Dynamics.Values) @@ -328,17 +287,20 @@ func ToGPUTensorStrides() { } sy := GPUSystem syVars := sy.Vars() - TensorStrides.SetShapeSizes(50) + TensorStrides.SetShapeSizes(60) TensorStrides.SetInt1D(Bodies.Shape().Strides[0], 0) TensorStrides.SetInt1D(Bodies.Shape().Strides[1], 1) TensorStrides.SetInt1D(Joints.Shape().Strides[0], 10) TensorStrides.SetInt1D(Joints.Shape().Strides[1], 11) - TensorStrides.SetInt1D(Dynamics.Shape().Strides[0], 20) - TensorStrides.SetInt1D(Dynamics.Shape().Strides[1], 21) - TensorStrides.SetInt1D(Contacts.Shape().Strides[0], 30) - TensorStrides.SetInt1D(Contacts.Shape().Strides[1], 31) - TensorStrides.SetInt1D(JointControls.Shape().Strides[0], 40) - TensorStrides.SetInt1D(JointControls.Shape().Strides[1], 41) + TensorStrides.SetInt1D(BodyJoints.Shape().Strides[0], 20) + TensorStrides.SetInt1D(BodyJoints.Shape().Strides[1], 21) + TensorStrides.SetInt1D(BodyJoints.Shape().Strides[2], 22) + TensorStrides.SetInt1D(Dynamics.Shape().Strides[0], 30) + TensorStrides.SetInt1D(Dynamics.Shape().Strides[1], 31) + TensorStrides.SetInt1D(Contacts.Shape().Strides[0], 40) + TensorStrides.SetInt1D(Contacts.Shape().Strides[1], 41) + TensorStrides.SetInt1D(JointControls.Shape().Strides[0], 50) + TensorStrides.SetInt1D(JointControls.Shape().Strides[1], 51) v, _ := syVars.ValueByIndex(0, "TensorStrides", 0) gpu.SetValueFrom(v, TensorStrides.Values) } @@ -358,6 +320,9 @@ func ReadFromGPU(vars ...GPUVars) { case JointsVar: v, _ := syVars.ValueByIndex(1, "Joints", 0) v.GPUToRead(sy.CommandEncoder) + case BodyJointsVar: + v, _ := syVars.ValueByIndex(1, "BodyJoints", 0) + v.GPUToRead(sy.CommandEncoder) case DynamicsVar: v, _ := syVars.ValueByIndex(2, "Dynamics", 0) v.GPUToRead(sy.CommandEncoder) @@ -389,6 +354,10 @@ func SyncFromGPU(vars ...GPUVars) { v, _ := syVars.ValueByIndex(1, "Joints", 0) v.ReadSync() gpu.ReadToBytes(v, Joints.Values) + case BodyJointsVar: + v, _ := syVars.ValueByIndex(1, "BodyJoints", 0) + v.ReadSync() + gpu.ReadToBytes(v, BodyJoints.Values) case DynamicsVar: v, _ := syVars.ValueByIndex(2, "Dynamics", 0) v.ReadSync() diff --git a/physics/joint.go b/physics/joint.go index 4e48788c..35b424c2 100644 --- a/physics/joint.go +++ b/physics/joint.go @@ -33,9 +33,6 @@ const ( // JointChild is the dynamic body index for child body. JointChild - // JointAncestor is the joint index where current parent is a child. - JointAncestor - // position of joint, in parent frame. JointPPosX JointPPosY @@ -58,9 +55,35 @@ const ( JointCRotZ JointCRotW + JointAxisX + JointAxisY + JointAxisZ + // joint limits JointLimitLower JointLimitUpper + + // Computed forces (temp storage until aggregated by bodies). + + // Computed parent joint force value. + JointPForceX + JointPForceY + JointPForceZ + + // Computed parent joint torque value. + JointPTorqueX + JointPTorqueY + JointPTorqueZ + + // Computed child joint force value. + JointCForceX + JointCForceY + JointCForceZ + + // Computed child joint torque value. + JointCTorqueX + JointCTorqueY + JointCTorqueZ ) func GetJointType(idx int32) JointTypes { @@ -117,6 +140,76 @@ func SetJointPRot(idx int32, rot math32.Quat) { Joints.Set(rot.W, int(idx), int(JointPRotW)) } +func JointAxis(idx int32) math32.Vector3 { + var axis math32.Vector3 + axis.X = Joints.Value(int(idx), int(JointAxisX)) + axis.Y = Joints.Value(int(idx), int(JointAxisY)) + axis.Z = Joints.Value(int(idx), int(JointAxisZ)) + return axis +} + +func SetJointAxis(idx int32, axis math32.Vector3) { + Joints.Set(axis.X, int(idx), int(JointAxisX)) + Joints.Set(axis.Y, int(idx), int(JointAxisY)) + Joints.Set(axis.Z, int(idx), int(JointAxisZ)) +} + +func JointPForce(idx int32) math32.Vector3 { + var f math32.Vector3 + f.X = Joints.Value(int(idx), int(JointPForceX)) + f.Y = Joints.Value(int(idx), int(JointPForceY)) + f.Z = Joints.Value(int(idx), int(JointPForceZ)) + return f +} + +func SetJointPForce(idx int32, f math32.Vector3) { + Joints.Set(f.X, int(idx), int(JointPForceX)) + Joints.Set(f.Y, int(idx), int(JointPForceY)) + Joints.Set(f.Z, int(idx), int(JointPForceZ)) +} + +func JointPTorque(idx int32) math32.Vector3 { + var t math32.Vector3 + t.X = Joints.Value(int(idx), int(JointPTorqueX)) + t.Y = Joints.Value(int(idx), int(JointPTorqueY)) + t.Z = Joints.Value(int(idx), int(JointPTorqueZ)) + return t +} + +func SetJointPTorque(idx int32, t math32.Vector3) { + Joints.Set(t.X, int(idx), int(JointPTorqueX)) + Joints.Set(t.Y, int(idx), int(JointPTorqueY)) + Joints.Set(t.Z, int(idx), int(JointPTorqueZ)) +} + +func JointCForce(idx int32) math32.Vector3 { + var f math32.Vector3 + f.X = Joints.Value(int(idx), int(JointCForceX)) + f.Y = Joints.Value(int(idx), int(JointCForceY)) + f.Z = Joints.Value(int(idx), int(JointCForceZ)) + return f +} + +func SetJointCForce(idx int32, f math32.Vector3) { + Joints.Set(f.X, int(idx), int(JointCForceX)) + Joints.Set(f.Y, int(idx), int(JointCForceY)) + Joints.Set(f.Z, int(idx), int(JointCForceZ)) +} + +func JointCTorque(idx int32) math32.Vector3 { + var t math32.Vector3 + t.X = Joints.Value(int(idx), int(JointCTorqueX)) + t.Y = Joints.Value(int(idx), int(JointCTorqueY)) + t.Z = Joints.Value(int(idx), int(JointCTorqueZ)) + return t +} + +func SetJointCTorque(idx int32, t math32.Vector3) { + Joints.Set(t.X, int(idx), int(JointCTorqueX)) + Joints.Set(t.Y, int(idx), int(JointCTorqueY)) + Joints.Set(t.Z, int(idx), int(JointCTorqueZ)) +} + // JointTypes are joint types that determine nature of interaction. type JointTypes int32 //enums:enum @@ -143,12 +236,4 @@ const ( D6 ) -// FixedStep does fixed joint processing -func FixedStep(ji, jp, jc int32) { - pos := JointPPos(ji) - jpp := DynamicPos(jp) - bbp := pos.Add(bap) - SetDynamicPos(bb, bbp) -} - //gosl:end diff --git a/physics/joint.goal b/physics/joint.goal index c954eae2..1d9bf5a2 100644 --- a/physics/joint.goal +++ b/physics/joint.goal @@ -31,9 +31,6 @@ const ( // JointChild is the dynamic body index for child body. JointChild - // JointAncestor is the joint index where current parent is a child. - JointAncestor - // position of joint, in parent frame. JointPPosX JointPPosY @@ -56,9 +53,35 @@ const ( JointCRotZ JointCRotW + JointAxisX + JointAxisY + JointAxisZ + // joint limits JointLimitLower JointLimitUpper + + // Computed forces (temp storage until aggregated by bodies). + + // Computed parent joint force value. + JointPForceX + JointPForceY + JointPForceZ + + // Computed parent joint torque value. + JointPTorqueX + JointPTorqueY + JointPTorqueZ + + // Computed child joint force value. + JointCForceX + JointCForceY + JointCForceZ + + // Computed child joint torque value. + JointCTorqueX + JointCTorqueY + JointCTorqueZ ) func GetJointType(idx int32) JointTypes { @@ -115,6 +138,76 @@ func SetJointPRot(idx int32, rot math32.Quat) { Joints[idx, JointPRotW] = rot.W } +func JointAxis(idx int32) math32.Vector3 { + var axis math32.Vector3 + axis.X = Joints[idx, JointAxisX] + axis.Y = Joints[idx, JointAxisY] + axis.Z = Joints[idx, JointAxisZ] + return axis +} + +func SetJointAxis(idx int32, axis math32.Vector3) { + Joints[idx, JointAxisX] = axis.X + Joints[idx, JointAxisY] = axis.Y + Joints[idx, JointAxisZ] = axis.Z +} + +func JointPForce(idx int32) math32.Vector3 { + var f math32.Vector3 + f.X = Joints[idx, JointPForceX] + f.Y = Joints[idx, JointPForceY] + f.Z = Joints[idx, JointPForceZ] + return f +} + +func SetJointPForce(idx int32, f math32.Vector3) { + Joints[idx, JointPForceX] = f.X + Joints[idx, JointPForceY] = f.Y + Joints[idx, JointPForceZ] = f.Z +} + +func JointPTorque(idx int32) math32.Vector3 { + var t math32.Vector3 + t.X = Joints[idx, JointPTorqueX] + t.Y = Joints[idx, JointPTorqueY] + t.Z = Joints[idx, JointPTorqueZ] + return t +} + +func SetJointPTorque(idx int32, t math32.Vector3) { + Joints[idx, JointPTorqueX] = t.X + Joints[idx, JointPTorqueY] = t.Y + Joints[idx, JointPTorqueZ] = t.Z +} + +func JointCForce(idx int32) math32.Vector3 { + var f math32.Vector3 + f.X = Joints[idx, JointCForceX] + f.Y = Joints[idx, JointCForceY] + f.Z = Joints[idx, JointCForceZ] + return f +} + +func SetJointCForce(idx int32, f math32.Vector3) { + Joints[idx, JointCForceX] = f.X + Joints[idx, JointCForceY] = f.Y + Joints[idx, JointCForceZ] = f.Z +} + +func JointCTorque(idx int32) math32.Vector3 { + var t math32.Vector3 + t.X = Joints[idx, JointCTorqueX] + t.Y = Joints[idx, JointCTorqueY] + t.Z = Joints[idx, JointCTorqueZ] + return t +} + +func SetJointCTorque(idx int32, t math32.Vector3) { + Joints[idx, JointCTorqueX] = t.X + Joints[idx, JointCTorqueY] = t.Y + Joints[idx, JointCTorqueZ] = t.Z +} + // JointTypes are joint types that determine nature of interaction. type JointTypes int32 //enums:enum @@ -141,12 +234,4 @@ const ( D6 ) -// FixedStep does fixed joint processing -func FixedStep(ji, jp, jc int32) { - pos := JointPPos(ji) - jpp := DynamicPos(jp) - bbp := pos.Add(bap) - SetDynamicPos(bb, bbp) -} - //gosl:end diff --git a/physics/shaders/InitDynamics.wgsl b/physics/shaders/InitDynamics.wgsl index 1c304df6..d78253ab 100644 --- a/physics/shaders/InitDynamics.wgsl +++ b/physics/shaders/InitDynamics.wgsl @@ -9,7 +9,7 @@ var Params: array; // // Bodies are the rigid body elements (dynamic and static), // specifying the constant, non-dynamic properties, // which is initial state for dynamics. // [body][BodyVarsN] @group(1) @binding(0) var Bodies: array; -// // Dynamics are the dynamic rigid body elements: these actually move. // [body][DynamicVarsN] +// // Dynamics are the dynamic rigid body elements: these actually move. // [dyn body][DynamicVarsN] @group(2) @binding(0) var Dynamics: array; // // JointControls are dynamic joint control inputs. // [joint][JointControlVarsN] @@ -102,7 +102,7 @@ const AngDeltaX: DynamicVars = 29; const AngDeltaY: DynamicVars = 30; const AngDeltaZ: DynamicVars = 31; fn DynamicIndex(idx: i32) -> i32 { - return i32(bitcast(Dynamics[Index2D(TensorStrides[20], TensorStrides[21], u32(idx), u32(Index))])); + return i32(bitcast(Dynamics[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(Index))])); } //////// import: "contact.go" @@ -144,8 +144,8 @@ const BodyVarsN: BodyVars = 37; const DynamicVarsN: DynamicVars = 32; const ContactVarsN: ContactVars = 9; const JointControlVarsN: JointControlVars = 19; -const GPUVarsN: GPUVars = 6; -const JointVarsN: JointVars = 21; +const GPUVarsN: GPUVars = 7; +const JointVarsN: JointVars = 35; const JointTypesN: JointTypes = 7; const ShapesN: Shapes = 4; @@ -155,23 +155,37 @@ const JointType: JointVars = 0; const JointEnabled: JointVars = 1; const JointParent: JointVars = 2; const JointChild: JointVars = 3; -const JointAncestor: JointVars = 4; -const JointPPosX: JointVars = 5; -const JointPPosY: JointVars = 6; -const JointPPosZ: JointVars = 7; -const JointPRotX: JointVars = 8; -const JointPRotY: JointVars = 9; -const JointPRotZ: JointVars = 10; -const JointPRotW: JointVars = 11; -const JointCPosX: JointVars = 12; -const JointCPosY: JointVars = 13; -const JointCPosZ: JointVars = 14; -const JointCRotX: JointVars = 15; -const JointCRotY: JointVars = 16; -const JointCRotZ: JointVars = 17; -const JointCRotW: JointVars = 18; -const JointLimitLower: JointVars = 19; -const JointLimitUpper: JointVars = 20; +const JointPPosX: JointVars = 4; +const JointPPosY: JointVars = 5; +const JointPPosZ: JointVars = 6; +const JointPRotX: JointVars = 7; +const JointPRotY: JointVars = 8; +const JointPRotZ: JointVars = 9; +const JointPRotW: JointVars = 10; +const JointCPosX: JointVars = 11; +const JointCPosY: JointVars = 12; +const JointCPosZ: JointVars = 13; +const JointCRotX: JointVars = 14; +const JointCRotY: JointVars = 15; +const JointCRotZ: JointVars = 16; +const JointCRotW: JointVars = 17; +const JointAxisX: JointVars = 18; +const JointAxisY: JointVars = 19; +const JointAxisZ: JointVars = 20; +const JointLimitLower: JointVars = 21; +const JointLimitUpper: JointVars = 22; +const JointPForceX: JointVars = 23; +const JointPForceY: JointVars = 24; +const JointPForceZ: JointVars = 25; +const JointPTorqueX: JointVars = 26; +const JointPTorqueY: JointVars = 27; +const JointPTorqueZ: JointVars = 28; +const JointCForceX: JointVars = 29; +const JointCForceY: JointVars = 30; +const JointCForceZ: JointVars = 31; +const JointCTorqueX: JointVars = 32; +const JointCTorqueY: JointVars = 33; +const JointCTorqueZ: JointVars = 34; alias JointTypes = i32; //enums:enum const Prismatic: JointTypes = 0; const Revolute: JointTypes = 1; @@ -207,6 +221,8 @@ const Capsule: Shapes = 3; //////// import: "slmath-quaternion.go" +//////// import: "slmath-vector3.go" + //////// import: "step.go" fn InitDynamics(i: u32) { //gosl:kernel let pars = Params[0]; @@ -215,15 +231,15 @@ fn InitDynamics(i: u32) { //gosl:kernel return; } var bi = DynamicIndex(ii); - Dynamics[Index2D(TensorStrides[20], TensorStrides[21], u32(ii), u32(PosX))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyPosX))]; - Dynamics[Index2D(TensorStrides[20], TensorStrides[21], u32(ii), u32(PosY))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyPosY))]; - Dynamics[Index2D(TensorStrides[20], TensorStrides[21], u32(ii), u32(PosZ))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyPosZ))]; - Dynamics[Index2D(TensorStrides[20], TensorStrides[21], u32(ii), u32(RotX))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyRotX))]; - Dynamics[Index2D(TensorStrides[20], TensorStrides[21], u32(ii), u32(RotY))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyRotY))]; - Dynamics[Index2D(TensorStrides[20], TensorStrides[21], u32(ii), u32(RotZ))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyRotZ))]; - Dynamics[Index2D(TensorStrides[20], TensorStrides[21], u32(ii), u32(RotW))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyRotW))]; + Dynamics[Index2D(TensorStrides[30], TensorStrides[31], u32(ii), u32(PosX))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyPosX))]; + Dynamics[Index2D(TensorStrides[30], TensorStrides[31], u32(ii), u32(PosY))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyPosY))]; + Dynamics[Index2D(TensorStrides[30], TensorStrides[31], u32(ii), u32(PosZ))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyPosZ))]; + Dynamics[Index2D(TensorStrides[30], TensorStrides[31], u32(ii), u32(RotX))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyRotX))]; + Dynamics[Index2D(TensorStrides[30], TensorStrides[31], u32(ii), u32(RotY))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyRotY))]; + Dynamics[Index2D(TensorStrides[30], TensorStrides[31], u32(ii), u32(RotZ))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyRotZ))]; + Dynamics[Index2D(TensorStrides[30], TensorStrides[31], u32(ii), u32(RotW))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyRotW))]; for (var v = VelX; v < DynamicVarsN; v++) { - Dynamics[Index2D(TensorStrides[20], TensorStrides[21], + Dynamics[Index2D(TensorStrides[30], TensorStrides[31], u32(ii), u32(v))] = 0.0; } } \ No newline at end of file diff --git a/physics/shaders/Step.wgsl b/physics/shaders/Step.wgsl deleted file mode 100644 index 264403e8..00000000 --- a/physics/shaders/Step.wgsl +++ /dev/null @@ -1,215 +0,0 @@ -// Code generated by "gosl"; DO NOT EDIT -// kernel: Step - -// // Params are global parameters. -@group(0) @binding(0) -var TensorStrides: array; -@group(0) @binding(1) -var Params: array; -// // Bodies are the rigid body elements (dynamic and static), // specifying the constant, non-dynamic properties, // which is initial state for dynamics. // [body][BodyVarsN] -// // Dynamics are the dynamic rigid body elements: these actually move. // [body][DynamicVarsN] -@group(2) @binding(0) -var Dynamics: array; -// // JointControls are dynamic joint control inputs. // [joint][JointControlVarsN] - -alias GPUVars = i32; - -@compute @workgroup_size(64, 1, 1) -fn main(@builtin(workgroup_id) wgid: vec3, @builtin(num_workgroups) nwg: vec3, @builtin(local_invocation_index) loci: u32) { - let idx = loci + (wgid.x + wgid.y * nwg.x + wgid.z * nwg.x * nwg.y) * 64; - Step(idx); -} - -fn Index2D(s0: u32, s1: u32, i0: u32, i1: u32) -> u32 { - return s0 * i0 + s1 * i1; -} - - -//////// import: "vars.go" - -//////// import: "body.go" -alias BodyVars = i32; //enums:enum -const Shape: BodyVars = 0; -const WorldIndex: BodyVars = 1; -const SizeX: BodyVars = 2; -const SizeY: BodyVars = 3; -const SizeZ: BodyVars = 4; -const Mass: BodyVars = 5; -const InvMass: BodyVars = 6; -const Bounce: BodyVars = 7; -const Friction: BodyVars = 8; -const BodyPosX: BodyVars = 9; -const BodyPosY: BodyVars = 10; -const BodyPosZ: BodyVars = 11; -const BodyRotX: BodyVars = 12; -const BodyRotY: BodyVars = 13; -const BodyRotZ: BodyVars = 14; -const BodyRotW: BodyVars = 15; -const BodyComX: BodyVars = 16; -const BodyComY: BodyVars = 17; -const BodyComZ: BodyVars = 18; -const InertiaXX: BodyVars = 19; -const InertiaYX: BodyVars = 20; -const InertiaZX: BodyVars = 21; -const InertiaXY: BodyVars = 22; -const InertiaYY: BodyVars = 23; -const InertiaZY: BodyVars = 24; -const InertiaXZ: BodyVars = 25; -const InertiaYZ: BodyVars = 26; -const InertiaZZ: BodyVars = 27; -const InvInertiaXX: BodyVars = 28; -const InvInertiaYX: BodyVars = 29; -const InvInertiaZX: BodyVars = 30; -const InvInertiaXY: BodyVars = 31; -const InvInertiaYY: BodyVars = 32; -const InvInertiaZY: BodyVars = 33; -const InvInertiaXZ: BodyVars = 34; -const InvInertiaYZ: BodyVars = 35; -const InvInertiaZZ: BodyVars = 36; -alias DynamicVars = i32; //enums:enum -const Index: DynamicVars = 0; -const PosX: DynamicVars = 1; -const PosY: DynamicVars = 2; -const PosZ: DynamicVars = 3; -const RotX: DynamicVars = 4; -const RotY: DynamicVars = 5; -const RotZ: DynamicVars = 6; -const RotW: DynamicVars = 7; -const VelX: DynamicVars = 8; -const VelY: DynamicVars = 9; -const VelZ: DynamicVars = 10; -const AngVelX: DynamicVars = 11; -const AngVelY: DynamicVars = 12; -const AngVelZ: DynamicVars = 13; -const AccX: DynamicVars = 14; -const AccY: DynamicVars = 15; -const AccZ: DynamicVars = 16; -const AngAccX: DynamicVars = 17; -const AngAccY: DynamicVars = 18; -const AngAccZ: DynamicVars = 19; -const ForceX: DynamicVars = 20; -const ForceY: DynamicVars = 21; -const ForceZ: DynamicVars = 22; -const TorqueX: DynamicVars = 23; -const TorqueY: DynamicVars = 24; -const TorqueZ: DynamicVars = 25; -const DeltaX: DynamicVars = 26; -const DeltaY: DynamicVars = 27; -const DeltaZ: DynamicVars = 28; -const AngDeltaX: DynamicVars = 29; -const AngDeltaY: DynamicVars = 30; -const AngDeltaZ: DynamicVars = 31; - -//////// import: "contact.go" -alias ContactVars = i32; //enums:enum -const ContactA: ContactVars = 0; -const ContactB: ContactVars = 1; -const ContactNormX: ContactVars = 2; -const ContactNormY: ContactVars = 3; -const ContactNormZ: ContactVars = 4; -const ContactPointX: ContactVars = 5; -const ContactPointY: ContactVars = 6; -const ContactPointZ: ContactVars = 7; -const ContactDist: ContactVars = 8; - -//////// import: "control.go" -alias JointControlVars = i32; //enums:enum -const JointForceX: JointControlVars = 0; -const JointForceY: JointControlVars = 1; -const JointForceZ: JointControlVars = 2; -const JointTorqueX: JointControlVars = 3; -const JointTorqueY: JointControlVars = 4; -const JointTorqueZ: JointControlVars = 5; -const JointTargetPosX: JointControlVars = 6; -const JointTargetPosY: JointControlVars = 7; -const JointTargetPosZ: JointControlVars = 8; -const JointTargetRotX: JointControlVars = 9; -const JointTargetRotY: JointControlVars = 10; -const JointTargetRotZ: JointControlVars = 11; -const JointTargetRotW: JointControlVars = 12; -const JointTargetVelX: JointControlVars = 13; -const JointTargetVelY: JointControlVars = 14; -const JointTargetVelZ: JointControlVars = 15; -const JointTargetAngVelX: JointControlVars = 16; -const JointTargetAngVelY: JointControlVars = 17; -const JointTargetAngVelZ: JointControlVars = 18; - -//////// import: "enumgen.go" -const BodyVarsN: BodyVars = 37; -const DynamicVarsN: DynamicVars = 32; -const ContactVarsN: ContactVars = 9; -const JointControlVarsN: JointControlVars = 19; -const GPUVarsN: GPUVars = 6; -const JointVarsN: JointVars = 21; -const JointTypesN: JointTypes = 7; -const ShapesN: Shapes = 4; - -//////// import: "joint.go" -alias JointVars = i32; //enums:enum -const JointType: JointVars = 0; -const JointEnabled: JointVars = 1; -const JointParent: JointVars = 2; -const JointChild: JointVars = 3; -const JointAncestor: JointVars = 4; -const JointPPosX: JointVars = 5; -const JointPPosY: JointVars = 6; -const JointPPosZ: JointVars = 7; -const JointPRotX: JointVars = 8; -const JointPRotY: JointVars = 9; -const JointPRotZ: JointVars = 10; -const JointPRotW: JointVars = 11; -const JointCPosX: JointVars = 12; -const JointCPosY: JointVars = 13; -const JointCPosZ: JointVars = 14; -const JointCRotX: JointVars = 15; -const JointCRotY: JointVars = 16; -const JointCRotZ: JointVars = 17; -const JointCRotW: JointVars = 18; -const JointLimitLower: JointVars = 19; -const JointLimitUpper: JointVars = 20; -alias JointTypes = i32; //enums:enum -const Prismatic: JointTypes = 0; -const Revolute: JointTypes = 1; -const Ball: JointTypes = 2; -const Fixed: JointTypes = 3; -const Free: JointTypes = 4; -const Distance: JointTypes = 5; -const D6: JointTypes = 6; - -//////// import: "params.go" -struct PhysParams { - DynamicsN: i32, - JointsN: i32, - Iters: i32, - SoftRelax: f32, - JointLinearRelax: f32, - JointAngularRelax: f32, - JointLinearComply: f32, - JointAngularComply: f32, - ContactRelax: f32, - AngularDamping: f32, - ContactWeighting: i32, - Restitution: i32, - Gravity: vec4, -} - -//////// import: "shapes.go" -alias Shapes = i32; //enums:enum -const Box: Shapes = 0; -const Sphere: Shapes = 1; -const Cylinder: Shapes = 2; -const Capsule: Shapes = 3; - -//////// import: "slmath-quaternion.go" - -//////// import: "step.go" -fn Step(i: u32) { //gosl:kernel - let pars = Params[0]; - var ii = i32(i); - if (ii >= pars.DynamicsN) { - return; - } - Dynamics[Index2D(TensorStrides[20], TensorStrides[21], u32(ii), u32(PosX))] += pars.Step * Dynamics[Index2D(TensorStrides[20], TensorStrides[21], u32(ii), u32(VelX))]; - Dynamics[Index2D(TensorStrides[20], TensorStrides[21], u32(ii), u32(PosY))] += pars.Step * Dynamics[Index2D(TensorStrides[20], TensorStrides[21], u32(ii), u32(VelY))]; - Dynamics[Index2D(TensorStrides[20], TensorStrides[21], u32(ii), u32(PosZ))] += pars.Step * Dynamics[Index2D(TensorStrides[20], TensorStrides[21], u32(ii), u32(VelZ))]; -} \ No newline at end of file diff --git a/physics/shaders/StepJoints.wgsl b/physics/shaders/StepJoints.wgsl index a3aab1a3..5ef8f590 100644 --- a/physics/shaders/StepJoints.wgsl +++ b/physics/shaders/StepJoints.wgsl @@ -11,7 +11,7 @@ var Params: array; var Bodies: array; @group(1) @binding(1) var Joints: array; -// // Dynamics are the dynamic rigid body elements: these actually move. // [body][DynamicVarsN] +// // Dynamics are the dynamic rigid body elements: these actually move. // [dyn body][DynamicVarsN] @group(2) @binding(0) var Dynamics: array; // // JointControls are dynamic joint control inputs. // [joint][JointControlVarsN] @@ -110,20 +110,20 @@ const AngDeltaX: DynamicVars = 29; const AngDeltaY: DynamicVars = 30; const AngDeltaZ: DynamicVars = 31; fn DynamicIndex(idx: i32) -> i32 { - return i32(bitcast(Dynamics[Index2D(TensorStrides[20], TensorStrides[21], u32(idx), u32(Index))])); + return i32(bitcast(Dynamics[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(Index))])); } fn DynamicPos(idx: i32) -> vec3 { var pos: vec3; - pos.x = Dynamics[Index2D(TensorStrides[20], TensorStrides[21], u32(idx), u32(PosX))]; - pos.y = Dynamics[Index2D(TensorStrides[20], TensorStrides[21], u32(idx), u32(PosY))]; - pos.z = Dynamics[Index2D(TensorStrides[20], TensorStrides[21], u32(idx), u32(PosZ))];return pos; + pos.x = Dynamics[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(PosX))]; + pos.y = Dynamics[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(PosY))]; + pos.z = Dynamics[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(PosZ))];return pos; } fn DynamicRot(idx: i32) -> vec4 { var rot: vec4; - rot.x = Dynamics[Index2D(TensorStrides[20], TensorStrides[21], u32(idx), u32(RotX))]; - rot.y = Dynamics[Index2D(TensorStrides[20], TensorStrides[21], u32(idx), u32(RotY))]; - rot.z = Dynamics[Index2D(TensorStrides[20], TensorStrides[21], u32(idx), u32(RotZ))]; - rot.w = Dynamics[Index2D(TensorStrides[20], TensorStrides[21], u32(idx), u32(RotW))];return rot; + rot.x = Dynamics[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(RotX))]; + rot.y = Dynamics[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(RotY))]; + rot.z = Dynamics[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(RotZ))]; + rot.w = Dynamics[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(RotW))];return rot; } //////// import: "contact.go" @@ -167,8 +167,8 @@ const BodyVarsN: BodyVars = 37; const DynamicVarsN: DynamicVars = 32; const ContactVarsN: ContactVars = 9; const JointControlVarsN: JointControlVars = 19; -const GPUVarsN: GPUVars = 6; -const JointVarsN: JointVars = 21; +const GPUVarsN: GPUVars = 7; +const JointVarsN: JointVars = 35; const JointTypesN: JointTypes = 7; const ShapesN: Shapes = 4; @@ -178,23 +178,37 @@ const JointType: JointVars = 0; const JointEnabled: JointVars = 1; const JointParent: JointVars = 2; const JointChild: JointVars = 3; -const JointAncestor: JointVars = 4; -const JointPPosX: JointVars = 5; -const JointPPosY: JointVars = 6; -const JointPPosZ: JointVars = 7; -const JointPRotX: JointVars = 8; -const JointPRotY: JointVars = 9; -const JointPRotZ: JointVars = 10; -const JointPRotW: JointVars = 11; -const JointCPosX: JointVars = 12; -const JointCPosY: JointVars = 13; -const JointCPosZ: JointVars = 14; -const JointCRotX: JointVars = 15; -const JointCRotY: JointVars = 16; -const JointCRotZ: JointVars = 17; -const JointCRotW: JointVars = 18; -const JointLimitLower: JointVars = 19; -const JointLimitUpper: JointVars = 20; +const JointPPosX: JointVars = 4; +const JointPPosY: JointVars = 5; +const JointPPosZ: JointVars = 6; +const JointPRotX: JointVars = 7; +const JointPRotY: JointVars = 8; +const JointPRotZ: JointVars = 9; +const JointPRotW: JointVars = 10; +const JointCPosX: JointVars = 11; +const JointCPosY: JointVars = 12; +const JointCPosZ: JointVars = 13; +const JointCRotX: JointVars = 14; +const JointCRotY: JointVars = 15; +const JointCRotZ: JointVars = 16; +const JointCRotW: JointVars = 17; +const JointAxisX: JointVars = 18; +const JointAxisY: JointVars = 19; +const JointAxisZ: JointVars = 20; +const JointLimitLower: JointVars = 21; +const JointLimitUpper: JointVars = 22; +const JointPForceX: JointVars = 23; +const JointPForceY: JointVars = 24; +const JointPForceZ: JointVars = 25; +const JointPTorqueX: JointVars = 26; +const JointPTorqueY: JointVars = 27; +const JointPTorqueZ: JointVars = 28; +const JointCForceX: JointVars = 29; +const JointCForceY: JointVars = 30; +const JointCForceZ: JointVars = 31; +const JointCTorqueX: JointVars = 32; +const JointCTorqueY: JointVars = 33; +const JointCTorqueZ: JointVars = 34; fn GetJointType(idx: i32) -> JointTypes { return JointTypes(bitcast(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointType))])); } @@ -217,6 +231,33 @@ fn JointPRot(idx: i32) -> vec4 { rot.z = Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPRotZ))]; rot.w = Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPRotW))];return rot; } +fn JointAxis(idx: i32) -> vec3 { + var axis: vec3; + axis.x = Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointAxisX))]; + axis.y = Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointAxisY))]; + axis.z = Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointAxisZ))];return axis; +} +fn SetJointPForce(idx: i32, f: vec3) { + Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPForceX))] = f.x; + Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPForceY))] = f.y; + Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPForceZ))] = f.z; +} +fn SetJointPTorque(idx: i32, t: vec3) { + Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPTorqueX))] = t.x; + Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPTorqueY))] = t.y; + Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPTorqueZ))] = t.z; +} +fn SetJointCForce(idx: i32, f: vec3) { + Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCForceX))] = f.x; + Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCForceY))] = f.y; + Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCForceZ))] = f.z; +} +fn SetJointCTorque(idx: i32, t: vec3) { + Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCTorqueX))] = t.x; + Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCTorqueY))] = t.y; + Joints[Index2D(TensorStrides[10], TensorStrides[11], + u32(idx), u32(JointCTorqueZ))] = t.z; +} alias JointTypes = i32; //enums:enum const Prismatic: JointTypes = 0; const Revolute: JointTypes = 1; @@ -252,17 +293,13 @@ const Capsule: Shapes = 3; //////// import: "slmath-quaternion.go" fn MulQuat(v: vec3, q: vec4) -> vec3 { - var qx = q.x; - var qy = q.y; - var qz = q.z; - var qw = q.w; - var ix = qw*v.x + qy*v.z - qz*v.y; - var iy = qw*v.y + qz*v.x - qx*v.z; - var iz = qw*v.z + qx*v.y - qy*v.x; - var iw = -qx*v.x - qy*v.y - qz*v.z; -return vec3(ix*qw+iw*-qx+iy*-qz-iz*-qy, - iy*qw+iw*-qy+iz*-qx-ix*-qz, - iz*qw+iw*-qz+ix*-qy-iy*-qx); + var ix = q.w*v.x + q.y*v.z - q.z*v.y; + var iy = q.w*v.y + q.z*v.x - q.x*v.z; + var iz = q.w*v.z + q.x*v.y - q.y*v.x; + var iw = -q.x*v.x - q.y*v.y - q.z*v.z; +return vec3(ix*q.w+iw*-q.x+iy*-q.z-iz*-q.y, + iy*q.w+iw*-q.y+iz*-q.x-ix*-q.z, + iz*q.w+iw*-q.z+ix*-q.y-iy*-q.x); } fn MulQuats(a: vec4,b: vec4) -> vec4 { var q: vec4; @@ -272,6 +309,14 @@ fn MulQuats(a: vec4,b: vec4) -> vec4 { q.w = a.w*b.w - a.x*b.x - a.y*b.y - a.z*b.z;return q; } +//////// import: "slmath-vector3.go" +fn MulScalar3(v: vec3, s: f32) -> vec3 { + return vec3(v.x*s, v.y*s, v.z*s); +} +fn Cross3(v: vec3,o: vec3) -> vec3 { + return vec3(v.y*o.z-v.z*o.y, v.z*o.x-v.x*o.z, v.x*o.y-v.y*o.x); +} + //////// import: "step.go" fn MulTransforms(aP: vec3, aQ: vec4, bP: vec3, bQ: vec4, oP: ptr>, oQ: ptr>) { var br = MulQuat(bP, aQ); @@ -309,7 +354,6 @@ fn StepJoints(i: u32) { //gosl:kernel var posecP = DynamicPos(jci); var posecQ = DynamicRot(jci); var xwcP = posecP; - var xwcQ = posecQ; var comc = BodyCom(jcbi); var rc = xwcP-(TransformPoint(posecP, posecQ, comc)); // child moment arm var jf = JointForce(ji); @@ -324,7 +368,16 @@ fn StepJoints(i: u32) { //gosl:kernel case Ball: { t = jtq; } + case Revolute, Prismatic: { + var axis = JointAxis(ji); + var ap = MulQuat(axis, xwpQ); + f = f+(MulScalar3(ap, jf.x)); + } default: { } } + SetJointPForce(ji, f); + SetJointCForce(ji, f); + SetJointPTorque(ji, t+(Cross3(rp, f))); + SetJointCTorque(ji, t+(Cross3(rc, f))); } \ No newline at end of file diff --git a/physics/step.go b/physics/step.go index b6363b14..4427e8cd 100644 --- a/physics/step.go +++ b/physics/step.go @@ -23,7 +23,7 @@ func MulTransforms(aP math32.Vector3, aQ math32.Quat, bP math32.Vector3, bQ math *oQ = slmath.MulQuats(aQ, bQ) } -// TransformPoint applies quat-based transform to given point +// TransformPoint applies quat-based transform to given point. func TransformPoint(xP math32.Vector3, xQ math32.Quat, p math32.Vector3) math32.Vector3 { dp := slmath.MulQuat(p, xQ) return dp.Add(xP) @@ -87,41 +87,38 @@ func StepJoints(i uint32) { //gosl:kernel posecP := DynamicPos(jci) posecQ := DynamicRot(jci) xwcP := posecP - xwcQ := posecQ + // xwcQ := posecQ comc := BodyCom(jcbi) rc := xwcP.Sub(TransformPoint(posecP, posecQ, comc)) // child moment arm + // from controls: jf := JointForce(ji) jtq := JointTorque(ji) var f, t math32.Vector3 switch jt { case Free, Distance: + // todo: distance doesn't seem to be supported here? f = jf t = jtq case Ball: t = jtq + case Revolute, Prismatic: + axis := JointAxis(ji) + ap := slmath.MulQuat(axis, xwpQ) + f = f.Add(slmath.MulScalar3(ap, jf.X)) default: + // todo: D6 requires more iteration! } -} - -// Step is dynamic update step kernel. i = body -func Step(i uint32) { //gosl:kernel - pars := GetParams(0) - ii := int32(i) - if ii >= pars.DynamicsN { - return - } - Dynamics.SetAdd(pars.Step*Dynamics.Value(int(ii), int(VelX)), int(ii), int(PosX)) - Dynamics.SetAdd(pars.Step*Dynamics.Value(int(ii), int(VelY)), int(ii), int(PosY)) - Dynamics.SetAdd(pars.Step*Dynamics.Value(int(ii), int(VelZ)), int(ii), int(PosZ)) - - // todo: force, integrated etc. + SetJointPForce(ji, f) + SetJointCForce(ji, f) + SetJointPTorque(ji, t.Add(slmath.Cross3(rp, f))) + SetJointCTorque(ji, t.Add(slmath.Cross3(rc, f))) } //gosl:end -func (wl *World) Step() { +func (wl *World) StepJoints() { pars := GetParams(0) - RunStep(int(pars.DynamicsN)) + RunStepJoints(int(pars.JointsN)) } diff --git a/physics/step.goal b/physics/step.goal index 78320865..8910eaf3 100644 --- a/physics/step.goal +++ b/physics/step.goal @@ -21,7 +21,7 @@ func MulTransforms(aP math32.Vector3, aQ math32.Quat, bP math32.Vector3, bQ math *oQ = slmath.MulQuats(aQ, bQ) } -// TransformPoint applies quat-based transform to given point +// TransformPoint applies quat-based transform to given point. func TransformPoint(xP math32.Vector3, xQ math32.Quat, p math32.Vector3) math32.Vector3 { dp := slmath.MulQuat(p, xQ) return dp.Add(xP) @@ -85,41 +85,38 @@ func StepJoints(i uint32) { //gosl:kernel posecP := DynamicPos(jci) posecQ := DynamicRot(jci) xwcP := posecP - xwcQ := posecQ + // xwcQ := posecQ comc := BodyCom(jcbi) rc := xwcP.Sub(TransformPoint(posecP, posecQ, comc)) // child moment arm + // from controls: jf := JointForce(ji) jtq := JointTorque(ji) var f, t math32.Vector3 switch jt { case Free, Distance: + // todo: distance doesn't seem to be supported here? f = jf t = jtq case Ball: t = jtq + case Revolute, Prismatic: + axis := JointAxis(ji) + ap := slmath.MulQuat(axis, xwpQ) + f = f.Add(slmath.MulScalar3(ap, jf.X)) default: + // todo: D6 requires more iteration! } -} - -// Step is dynamic update step kernel. i = body -func Step(i uint32) { //gosl:kernel - pars := GetParams(0) - ii := int32(i) - if ii >= pars.DynamicsN { - return - } - Dynamics[ii, PosX] += pars.Step * Dynamics[ii, VelX] - Dynamics[ii, PosY] += pars.Step * Dynamics[ii, VelY] - Dynamics[ii, PosZ] += pars.Step * Dynamics[ii, VelZ] - - // todo: force, integrated etc. + SetJointPForce(ji, f) + SetJointCForce(ji, f) + SetJointPTorque(ji, t.Add(slmath.Cross3(rp, f))) + SetJointCTorque(ji, t.Add(slmath.Cross3(rc, f))) } //gosl:end -func (wl *World) Step() { +func (wl *World) StepJoints() { pars := GetParams(0) - RunStep(int(pars.DynamicsN)) + RunStepJoints(int(pars.JointsN)) } diff --git a/physics/typegen.go b/physics/typegen.go index b73ff167..e0b758ac 100644 --- a/physics/typegen.go +++ b/physics/typegen.go @@ -28,4 +28,4 @@ var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.Shapes", IDN var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.State", IDName: "state", Doc: "State contains the basic physical state including position, orientation, velocity.\nThese are only the values that can be either relative or absolute -- other physical\nstate values such as Mass should go in Rigid.", Methods: []types.Method{{Name: "MoveOnAxis", Doc: "MoveOnAxis moves (translates) the specified distance on the specified local axis,\nrelative to the current rotation orientation.\nThe axis is normalized prior to aplying the distance factor.\nSets the LinVel to motion vector.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z", "dist"}}, {Name: "MoveOnAxisAbs", Doc: "MoveOnAxisAbs moves (translates) the specified distance on the specified local axis,\nin absolute X,Y,Z coordinates (does not apply the Quat rotation factor.\nThe axis is normalized prior to aplying the distance factor.\nSets the LinVel to motion vector.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z", "dist"}}, {Name: "SetEulerRotation", Doc: "SetEulerRotation sets the rotation in Euler angles (degrees).", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z"}}, {Name: "EulerRotation", Doc: "EulerRotation returns the current rotation in Euler angles (degrees).", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Returns: []string{"Vector3"}}, {Name: "SetAxisRotation", Doc: "SetAxisRotation sets rotation from local axis and angle in degrees.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z", "angle"}}, {Name: "RotateOnAxis", Doc: "RotateOnAxis rotates around the specified local axis the specified angle in degrees.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z", "angle"}}, {Name: "RotateEuler", Doc: "RotateEuler rotates by given Euler angles (in degrees) relative to existing rotation.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z"}}}, Fields: []types.Field{{Name: "Pos", Doc: "Pos is the position of center of mass of object."}, {Name: "Quat", Doc: "Quat is the rotation specified as a quaternion."}, {Name: "LinVel", Doc: "LinVel is the linear velocity."}, {Name: "AngVel", Doc: "AngVel is the angular velocity."}}}) -var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.World", IDName: "world", Doc: "World contains and manages all of the physics elements.", Directives: []types.Directive{{Tool: "go", Directive: "generate", Args: []string{"core", "generate", "-add-types", "-gosl"}}}, Fields: []types.Field{{Name: "GPU", Doc: "GPU determines whether to use GPU (else CPU)."}, {Name: "Params", Doc: "Params are global parameters."}, {Name: "Bodies", Doc: "Bodies are the rigid body elements (dynamic and static),\nspecifying the constant, non-dynamic properties,\nwhich is initial state for dynamics.\n[body][BodyVarsN]"}, {Name: "Joints", Doc: "Joints is a list of permanent joints connecting bodies,\nwhich do not change (no dynamic variables).\n[joint][JointVarsN]"}, {Name: "Dynamics", Doc: "Dynamics are the dynamic rigid body elements: these actually move.\nThe first set of variables are for initial values, and the second current.\n[body][DynamicVarsN]"}, {Name: "Contacts", Doc: "Contacts are points of contact between bodies.\n[contact][ContactVarsN]"}, {Name: "JointControls", Doc: "JointControls are dynamic joint control inputs.\n[joint][JointControlVarsN]"}}}) +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.World", IDName: "world", Doc: "World contains and manages all of the physics elements.", Directives: []types.Directive{{Tool: "go", Directive: "generate", Args: []string{"core", "generate", "-add-types", "-gosl"}}}, Fields: []types.Field{{Name: "GPU", Doc: "GPU determines whether to use GPU (else CPU)."}, {Name: "Params", Doc: "Params are global parameters."}, {Name: "Bodies", Doc: "Bodies are the rigid body elements (dynamic and static),\nspecifying the constant, non-dynamic properties,\nwhich is initial state for dynamics.\n[body][BodyVarsN]"}, {Name: "Joints", Doc: "Joints is a list of permanent joints connecting bodies,\nwhich do not change (no dynamic variables).\n[joint][JointVarsN]"}, {Name: "BodyJoints", Doc: "BodyJoints is a list of joint indexes for each dynamic body, for aggregating.\n[dyn body][parent, child][maxjointsperbody]"}, {Name: "Dynamics", Doc: "Dynamics are the dynamic rigid body elements: these actually move.\nThe first set of variables are for initial values, and the second current.\n[body][DynamicVarsN]"}, {Name: "Contacts", Doc: "Contacts are points of contact between bodies.\n[contact][ContactVarsN]"}, {Name: "JointControls", Doc: "JointControls are dynamic joint control inputs.\n[joint][JointControlVarsN]"}}}) diff --git a/physics/vars.go b/physics/vars.go index ee407021..b5edbf6e 100644 --- a/physics/vars.go +++ b/physics/vars.go @@ -28,13 +28,18 @@ var ( Bodies *tensor.Float32 // Joints is a list of permanent joints connecting bodies, - // which do not change (no dynamic variables). + // which do not change (no dynamic variables, except temps). // [joint][JointVars] //gosl:dims 2 Joints *tensor.Float32 + // BodyJoints is a list of joint indexes for each dynamic body, for aggregating. + // [dyn body][parent, child][maxjointsperbody] + //gosl:dims 3 + BodyJoints *tensor.Int32 + // Dynamics are the dynamic rigid body elements: these actually move. - // [body][DynamicVarsN] + // [dyn body][DynamicVarsN] //gosl:group Bodies //gosl:dims 2 Dynamics *tensor.Float32 diff --git a/physics/world.go b/physics/world.go index b3d2443a..d8b52b25 100644 --- a/physics/world.go +++ b/physics/world.go @@ -32,6 +32,10 @@ type World struct { // [joint][JointVarsN] Joints *tensor.Float32 + // BodyJoints is a list of joint indexes for each dynamic body, for aggregating. + // [dyn body][parent, child][maxjointsperbody] + BodyJoints *tensor.Int32 + // Dynamics are the dynamic rigid body elements: these actually move. // The first set of variables are for initial values, and the second current. // [body][DynamicVarsN] @@ -57,6 +61,7 @@ func (wl *World) Init() { wl.Params = []PhysParams{} wl.Bodies = tensor.NewFloat32(0, int(BodyVarsN)) wl.Joints = tensor.NewFloat32(0, int(JointVarsN)) + wl.BodyJoints = tensor.NewInt32(0, 2, 2) wl.Dynamics = tensor.NewFloat32(0, int(DynamicVarsN)) wl.Contacts = tensor.NewFloat32(0, int(ContactVarsN)) wl.JointControls = tensor.NewFloat32(0, int(JointControlVarsN)) @@ -99,6 +104,8 @@ func (wl *World) NewJoint(joint JointTypes, parent, child int32, pos math32.Vect return idx } +// todo: init bodyjoints + // SetAsCurrent sets these as the current global values that are // processed in the code (on the GPU). If this was not the setter of // the current variables, then the parameter variables are copied up @@ -117,6 +124,7 @@ func (wl *World) SetAsCurrentVars() { Params = wl.Params Bodies = wl.Bodies Joints = wl.Joints + BodyJoints = wl.BodyJoints Dynamics = wl.Dynamics Contacts = wl.Contacts JointControls = wl.JointControls @@ -131,8 +139,8 @@ func (wl *World) GPUInit() { } // ToGPUInfra copies all the infrastructure for these filters up to -// the GPU. This is done in GPUInit, and +// the GPU. This is done in GPUInit, and if current switched. func (wl *World) ToGPUInfra() { ToGPUTensorStrides() - ToGPU(ParamsVar) + ToGPU(ParamsVar, BodiesVar, JointsVar, BodyJointsVar) } diff --git a/physics/world.goal b/physics/world.goal index 6ecd7ed8..e1e11454 100644 --- a/physics/world.goal +++ b/physics/world.goal @@ -30,6 +30,10 @@ type World struct { // [joint][JointVarsN] Joints *tensor.Float32 + // BodyJoints is a list of joint indexes for each dynamic body, for aggregating. + // [dyn body][parent, child][maxjointsperbody] + BodyJoints *tensor.Int32 + // Dynamics are the dynamic rigid body elements: these actually move. // The first set of variables are for initial values, and the second current. // [body][DynamicVarsN] @@ -55,6 +59,7 @@ func (wl *World) Init() { wl.Params = []PhysParams{} wl.Bodies = tensor.NewFloat32(0, int(BodyVarsN)) wl.Joints = tensor.NewFloat32(0, int(JointVarsN)) + wl.BodyJoints = tensor.NewInt32(0, 2, 2) wl.Dynamics = tensor.NewFloat32(0, int(DynamicVarsN)) wl.Contacts = tensor.NewFloat32(0, int(ContactVarsN)) wl.JointControls = tensor.NewFloat32(0, int(JointControlVarsN)) @@ -97,6 +102,8 @@ func (wl *World) NewJoint(joint JointTypes, parent, child int32, pos math32.Vect return idx } +// todo: init bodyjoints + // SetAsCurrent sets these as the current global values that are // processed in the code (on the GPU). If this was not the setter of // the current variables, then the parameter variables are copied up @@ -115,6 +122,7 @@ func (wl *World) SetAsCurrentVars() { Params = wl.Params Bodies = wl.Bodies Joints = wl.Joints + BodyJoints = wl.BodyJoints Dynamics = wl.Dynamics Contacts = wl.Contacts JointControls = wl.JointControls @@ -129,8 +137,8 @@ func (wl *World) GPUInit() { } // ToGPUInfra copies all the infrastructure for these filters up to -// the GPU. This is done in GPUInit, and +// the GPU. This is done in GPUInit, and if current switched. func (wl *World) ToGPUInfra() { ToGPUTensorStrides() - ToGPU(ParamsVar) + ToGPU(ParamsVar, BodiesVar, JointsVar, BodyJointsVar) } From 3c3d4448232a3f05f677543aa0832173b81f15c1 Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Mon, 15 Dec 2025 14:34:06 +0100 Subject: [PATCH 14/97] physics: integrate bodies compiling with inline math exprs, updated gosl --- gosl/gotosl/nodes.go | 15 +- gosl/gotosl/sledits.go | 7 +- gosl/gotosl/testdata/Compute.golden | 31 +-- gosl/gotosl/testdata/CycleUpdt.golden | 6 +- gosl/gotosl/testdata/basic.go | 6 +- gosl/gotosl/testdata/basic.goal | 6 +- gosl/slmath/matrix3.go | 9 + gosl/slmath/quaternion.go | 68 ++++-- gosl/slmath/vector3.go | 42 ++++ physics/body.go | 304 +++++++++++++------------- physics/body.goal | 304 +++++++++++++------------- physics/enumgen.go | 10 +- physics/gosl.go | 47 ++++ physics/params.go | 6 + physics/shaders/InitDynamics.wgsl | 142 ++++++------ physics/shaders/StepJoints.wgsl | 177 +++++++-------- physics/state.go | 8 +- physics/step.go | 126 ++++++++--- physics/step.goal | 126 ++++++++--- physics/typegen.go | 2 +- physics/vars.go | 2 +- 21 files changed, 873 insertions(+), 571 deletions(-) create mode 100644 gosl/slmath/matrix3.go create mode 100644 gosl/slmath/vector3.go diff --git a/gosl/gotosl/nodes.go b/gosl/gotosl/nodes.go index 40106049..a5c05966 100644 --- a/gosl/gotosl/nodes.go +++ b/gosl/gotosl/nodes.go @@ -1962,9 +1962,10 @@ func (p *printer) methodExpr(x *ast.CallExpr, depth int) { rwargs = append(rwargs, rwArg{idx: idx, tmpVar: recvPath}) } } else { - err := fmt.Errorf("gosl methodExpr ERROR: path expression for method call must be simple list of fields, not %#v:", path.X) - p.userError(err) - return + // fmt.Println("arg issue with:", methName) + // err := fmt.Errorf("gosl methodExpr ERROR: path expression for method call must be simple list of fields, not %#v:", path.X) + // p.userError(err) + // return } args := x.Args if pathType != nil { @@ -2058,15 +2059,17 @@ func (p *printer) mathMeth(x *ast.CallExpr, depth int, methName, recvPath, recvT opr = token.ADD case "Sub": opr = token.SUB - case "Mul": + case "Mul", "MulVector", "MulVector3", "MulScalar": opr = token.MUL - case "Div": + case "Div", "DivScalar": opr = token.QUO } if opr == token.ILLEGAL { return false } - p.print(recvPath, opr) + path := x.Fun.(*ast.SelectorExpr) // we know fun is selector + p.expr(path.X) + p.print(opr) p.setPos(x.Lparen) p.print(token.LPAREN) p.exprList(x.Lparen, x.Args, depth, commaTerm, x.Rparen, false) diff --git a/gosl/gotosl/sledits.go b/gosl/gotosl/sledits.go index 6f96b7ae..08f5f39d 100644 --- a/gosl/gotosl/sledits.go +++ b/gosl/gotosl/sledits.go @@ -46,11 +46,16 @@ var Replaces = []Replace{ {[]byte("math32.Vector2"), []byte("vec2")}, {[]byte("math32.Vector3"), []byte("vec3")}, {[]byte("math32.Vector4"), []byte("vec4")}, + {[]byte("math32.Matrix2"), []byte("mat2x3f")}, + {[]byte("math32.Matrix3"), []byte("mat3x3f")}, + {[]byte("math32.Matrix4"), []byte("mat4x4f")}, + {[]byte("math32.Quat"), []byte("vec4")}, {[]byte("math32.Vec2i"), []byte("vec2")}, {[]byte("math32.Vec2"), []byte("vec2")}, {[]byte("math32.Vec3"), []byte("vec3")}, {[]byte("math32.Vec4"), []byte("vec4")}, - {[]byte("math32.Quat"), []byte("vec4")}, + {[]byte("math32.NewQuat"), []byte("vec4")}, + {[]byte("math32.Mat3"), []byte("mat3x3f")}, {[]byte("float32"), []byte("f32")}, {[]byte("float64"), []byte("f64")}, // TODO: not yet supported {[]byte("uint32"), []byte("u32")}, diff --git a/gosl/gotosl/testdata/Compute.golden b/gosl/gotosl/testdata/Compute.golden index 05ffa41e..1f2682b5 100644 --- a/gosl/gotosl/testdata/Compute.golden +++ b/gosl/gotosl/testdata/Compute.golden @@ -127,10 +127,10 @@ const Integ: i32 = 1; const Exp: i32 = 2; const NVars: i32 = 3; fn TransformPoint(xP: vec3, xQ: vec4, p: vec3) -> vec3 { - var dp = MulQuat(p, xQ);return dp+(xP); + var dp = MulQuatVector(xQ, p);return dp+(xP); } fn MulTransforms(aP: vec3, aQ: vec4, bP: vec3, bQ: vec4, oP: ptr>, oQ: ptr>) { - var br = MulQuat(bP, aQ); + var br = MulQuatVector(aQ, bP); *oP = br+(aP); *oQ = MulQuats(aQ, bQ); } @@ -183,7 +183,7 @@ fn ParamStruct_IntegFromRaw(ps: ParamStruct, idx: i32) -> f32 { var op: vec3; var oq: vec4; MulTransforms(vec3(ps.Pos.x,ps.Pos.y,ps.Pos.z), ps.Rot, vec3(ps.Pos.x,ps.Pos.y,ps.Pos.z), ps.Rot, &op, &oq); - var d = MulQuat(op, oq); + var d = MulQuatVector(oq, op); d = TransformPoint(op, oq, d); let ctx = Ctx[0]; ParamStruct_AnotherMeth(ps, ctx, idx, &a); @@ -236,19 +236,12 @@ fn Compute(i: u32) { //gosl:kernel ParamStruct_IntegFromRaw(params, i32(i)); } +//////// import: "slmath-matrix3.go" + //////// import: "slmath-quaternion.go" -fn MulQuat(v: vec3, q: vec4) -> vec3 { - var qx = q.x; - var qy = q.y; - var qz = q.z; - var qw = q.w; - var ix = qw*v.x + qy*v.z - qz*v.y; - var iy = qw*v.y + qz*v.x - qx*v.z; - var iz = qw*v.z + qx*v.y - qy*v.x; - var iw = -qx*v.x - qy*v.y - qz*v.z; -return vec3(ix*qw+iw*-qx+iy*-qz-iz*-qy, - iy*qw+iw*-qy+iz*-qx-ix*-qz, - iz*qw+iw*-qz+ix*-qy-iy*-qx); +fn MulQuatVector(q: vec4, v: vec3) -> vec3 { + var xyz = vec3(q.x, q.y, q.z); + var t = MulScalar3(Cross3(xyz, v), f32(f32(2)));return v+(MulScalar3(t, q.w))+(Cross3(xyz, t)); } fn MulQuats(a: vec4,b: vec4) -> vec4 { var q: vec4; @@ -256,4 +249,12 @@ fn MulQuats(a: vec4,b: vec4) -> vec4 { q.y = a.y*b.w + a.w*b.y + a.z*b.x - a.x*b.z; q.z = a.z*b.w + a.w*b.z + a.x*b.y - a.y*b.x; q.w = a.w*b.w - a.x*b.x - a.y*b.y - a.z*b.z;return q; +} + +//////// import: "slmath-vector3.go" +fn MulScalar3(v: vec3, s: f32) -> vec3 { + return vec3(v.x*s, v.y*s, v.z*s); +} +fn Cross3(v: vec3,o: vec3) -> vec3 { + return vec3(v.y*o.z-v.z*o.y, v.z*o.x-v.x*o.z, v.x*o.y-v.y*o.x); } \ No newline at end of file diff --git a/gosl/gotosl/testdata/CycleUpdt.golden b/gosl/gotosl/testdata/CycleUpdt.golden index cccfb6f3..1de6f72b 100644 --- a/gosl/gotosl/testdata/CycleUpdt.golden +++ b/gosl/gotosl/testdata/CycleUpdt.golden @@ -66,4 +66,8 @@ fn CycleUpdt(i: u32) { //gosl:kernel read-write:Ctx Ctx[0] = ctx; } -//////// import: "slmath-quaternion.go" \ No newline at end of file +//////// import: "slmath-matrix3.go" + +//////// import: "slmath-quaternion.go" + +//////// import: "slmath-vector3.go" \ No newline at end of file diff --git a/gosl/gotosl/testdata/basic.go b/gosl/gotosl/testdata/basic.go index 8c94a422..fd079bd8 100644 --- a/gosl/gotosl/testdata/basic.go +++ b/gosl/gotosl/testdata/basic.go @@ -67,7 +67,7 @@ func FastExp(x float32) float32 { // TransformPoint applies quat-based transform to given point func TransformPoint(xP math32.Vector3, xQ math32.Quat, p math32.Vector3) math32.Vector3 { - dp := slmath.MulQuat(p, xQ) + dp := slmath.MulQuatVector(xQ, p) return dp.Add(xP) } @@ -75,7 +75,7 @@ func TransformPoint(xP math32.Vector3, xQ math32.Quat, p math32.Vector3) math32. // two quat-based transforms, o = a * b func MulTransforms(aP math32.Vector3, aQ math32.Quat, bP math32.Vector3, bQ math32.Quat, oP *math32.Vector3, oQ *math32.Quat) { // rotate b by a and add a - br := slmath.MulQuat(bP, aQ) + br := slmath.MulQuatVector(aQ, bP) *oP = br.Add(aP) *oQ = slmath.MulQuats(aQ, bQ) } @@ -171,7 +171,7 @@ func (ps *ParamStruct) IntegFromRaw(idx int) float32 { var op math32.Vector3 var oq math32.Quat MulTransforms(ps.Pos.V(), ps.Rot, ps.Pos.V(), ps.Rot, &op, &oq) - d := slmath.MulQuat(op, oq) + d := slmath.MulQuatVector(oq, op) d = TransformPoint(op, oq, d) ctx := GetCtx(0) diff --git a/gosl/gotosl/testdata/basic.goal b/gosl/gotosl/testdata/basic.goal index 23052256..87e4da7f 100644 --- a/gosl/gotosl/testdata/basic.goal +++ b/gosl/gotosl/testdata/basic.goal @@ -60,7 +60,7 @@ func FastExp(x float32) float32 { // TransformPoint applies quat-based transform to given point func TransformPoint(xP math32.Vector3, xQ math32.Quat, p math32.Vector3) math32.Vector3 { - dp := slmath.MulQuat(p, xQ) + dp := slmath.MulQuatVector(xQ, p) return dp.Add(xP) } @@ -68,7 +68,7 @@ func TransformPoint(xP math32.Vector3, xQ math32.Quat, p math32.Vector3) math32. // two quat-based transforms, o = a * b func MulTransforms(aP math32.Vector3, aQ math32.Quat, bP math32.Vector3, bQ math32.Quat, oP *math32.Vector3, oQ *math32.Quat) { // rotate b by a and add a - br := slmath.MulQuat(bP, aQ) + br := slmath.MulQuatVector(aQ, bP) *oP = br.Add(aP) *oQ = slmath.MulQuats(aQ, bQ) } @@ -164,7 +164,7 @@ func (ps *ParamStruct) IntegFromRaw(idx int) float32 { var op math32.Vector3 var oq math32.Quat MulTransforms(ps.Pos.V(), ps.Rot, ps.Pos.V(), ps.Rot, &op, &oq) - d := slmath.MulQuat(op, oq) + d := slmath.MulQuatVector(oq, op) d = TransformPoint(op, oq, d) ctx := GetCtx(0) diff --git a/gosl/slmath/matrix3.go b/gosl/slmath/matrix3.go new file mode 100644 index 00000000..a7c0114f --- /dev/null +++ b/gosl/slmath/matrix3.go @@ -0,0 +1,9 @@ +// Copyright (c) 2025, Cogent Core. All rights reserved. +// Use of this source code is governed by a BSD-style +// license that can be found in the LICENSE file. + +package slmath + +//gosl:start + +//gosl:end diff --git a/gosl/slmath/quaternion.go b/gosl/slmath/quaternion.go index b0e3f90c..5f5304cc 100644 --- a/gosl/slmath/quaternion.go +++ b/gosl/slmath/quaternion.go @@ -8,22 +8,47 @@ import "cogentcore.org/core/math32" //gosl:start -// MulQuat returns vector multiplied by specified quaternion and -// then by the quaternion inverse. -// It basically applies the rotation encoded in the quaternion to this vector. -func MulQuat(v math32.Vector3, q math32.Quat) math32.Vector3 { - // calculate quat * vector - ix := q.W*v.X + q.Y*v.Z - q.Z*v.Y - iy := q.W*v.Y + q.Z*v.X - q.X*v.Z - iz := q.W*v.Z + q.X*v.Y - q.Y*v.X - iw := -q.X*v.X - q.Y*v.Y - q.Z*v.Z - // calculate result * inverse quat - return math32.Vec3(ix*q.W+iw*-q.X+iy*-q.Z-iz*-q.Y, - iy*q.W+iw*-q.Y+iz*-q.X-ix*-q.Z, - iz*q.W+iw*-q.Z+ix*-q.Y-iy*-q.X) +// QuatLength returns the length of this quaternion. +func QuatLength(q math32.Quat) float32 { + return math32.Sqrt(q.X*q.X + q.Y*q.Y + q.Z*q.Z + q.W*q.W) } -// MulQuats set this quaternion to the multiplication of a by b. +// QuatNormalize normalizes the quaternion. +func QuatNormalize(q math32.Quat) math32.Quat { + var nq math32.Quat + l := QuatLength(q) + if l == 0 { + nq.X = 0 + nq.Y = 0 + nq.Z = 0 + nq.W = 1 + } else { + l = 1 / l + nq.X *= l + nq.Y *= l + nq.Z *= l + nq.W *= l + } + return nq +} + +// MulQuatVector applies the rotation encoded in the [math32.Quat] +// to the [math32.Vector3]. +func MulQuatVector(q math32.Quat, v math32.Vector3) math32.Vector3 { + xyz := math32.Vec3(q.X, q.Y, q.Z) + t := MulScalar3(Cross3(xyz, v), 2) + return v.Add(MulScalar3(t, q.W)).Add(Cross3(xyz, t)) +} + +// MulQuatVectorInverse applies the inverse of the rotation encoded +// in the [math32.Quat] to the [math32.Vector3]. +func MulQuatVectorInverse(q math32.Quat, v math32.Vector3) math32.Vector3 { + xyz := math32.Vec3(q.X, q.Y, q.Z) + t := MulScalar3(Cross3(xyz, v), 2) + return v.Sub(MulScalar3(t, q.W)).Add(Cross3(xyz, t)) +} + +// MulQuats returns multiplication of a by b quaternions. func MulQuats(a, b math32.Quat) math32.Quat { // from http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/code/index.htm var q math32.Quat @@ -34,4 +59,19 @@ func MulQuats(a, b math32.Quat) math32.Quat { return q } +// MulQPTransforms computes the equivalent of matrix multiplication for +// two quat-point spatial transforms: o = a * b +func MulQPTransforms(aP math32.Vector3, aQ math32.Quat, bP math32.Vector3, bQ math32.Quat, oP *math32.Vector3, oQ *math32.Quat) { + // rotate b by a and add a + br := MulQuatVector(aQ, bP) + *oP = br.Add(aP) + *oQ = MulQuats(aQ, bQ) +} + +// MulQPPoint applies quat-point spatial transform to given 3D point. +func MulQPPoint(xP math32.Vector3, xQ math32.Quat, p math32.Vector3) math32.Vector3 { + dp := MulQuatVector(xQ, p) + return dp.Add(xP) +} + //gosl:end diff --git a/gosl/slmath/vector3.go b/gosl/slmath/vector3.go new file mode 100644 index 00000000..0d8f1c33 --- /dev/null +++ b/gosl/slmath/vector3.go @@ -0,0 +1,42 @@ +// Copyright (c) 2025, Cogent Core. All rights reserved. +// Use of this source code is governed by a BSD-style +// license that can be found in the LICENSE file. + +package slmath + +import "cogentcore.org/core/math32" + +//gosl:start + +func AddScalar3(v math32.Vector3, s float32) math32.Vector3 { + return math32.Vec3(v.X+s, v.Y+s, v.Z+s) +} + +func SubScalar3(v math32.Vector3, s float32) math32.Vector3 { + return math32.Vec3(v.X-s, v.Y-s, v.Z-s) +} + +func MulScalar3(v math32.Vector3, s float32) math32.Vector3 { + return math32.Vec3(v.X*s, v.Y*s, v.Z*s) +} + +func DivScalar3(v math32.Vector3, s float32) math32.Vector3 { + return math32.Vec3(v.X/s, v.Y/s, v.Z/s) +} + +// Length3 returns the length (magnitude) of this vector. +func Length3(v math32.Vector3) float32 { + return math32.Sqrt(v.X*v.X + v.Y*v.Y + v.Z*v.Z) +} + +// Normal3 returns this vector divided by its length (its unit vector). +func Normal3(v math32.Vector3) math32.Vector3 { + return DivScalar3(v, Length3(v)) +} + +// Cross3 returns the cross product of this vector with other. +func Cross3(v, o math32.Vector3) math32.Vector3 { + return math32.Vec3(v.Y*o.Z-v.Z*o.Y, v.Z*o.X-v.X*o.Z, v.X*o.Y-v.Y*o.X) +} + +//gosl:end diff --git a/physics/body.go b/physics/body.go index 1829592c..1b1ddf2f 100644 --- a/physics/body.go +++ b/physics/body.go @@ -18,33 +18,33 @@ import ( type BodyVars int32 //enums:enum const ( - // Shape is the shape type of the object, as a Shapes type. - Shape BodyVars = iota + // BodyShape is the shape type of the object, as a Shapes type. + BodyShape BodyVars = iota - // WorldIndex partitions body into different worlds; Global are -1 - WorldIndex + // BodyWorldIndex partitions body into different worlds; Global are -1 + BodyWorldIndex - // Size is the size of the object (values depend on shape type). - SizeX - SizeY - SizeZ + // BodySize is the size of the object (values depend on shape type). + BodySizeX + BodySizeY + BodySizeZ // physical properties - // Mass is the mass of the object. - Mass + // BodyMass is the mass of the object. + BodyMass - // InvMass is 1/mass of the object or 0 if no mass. - InvMass + // BodyInvMass is 1/mass of the object or 0 if no mass. + BodyInvMass - // Bounce specifies the COR or coefficient of restitution (0..1), + // BodyBounce specifies the COR or coefficient of restitution (0..1), // which determines how elastic the collision is, // i.e., final velocity / initial velocity. - Bounce + BodyBounce - // Friction coefficient: how much friction is generated by transverse motion. + // BodyFriction coefficient: how much friction is generated by transverse motion. // Additive across the two surfaces. - Friction + BodyFriction // 3D position of body (structural center). BodyPosX @@ -63,56 +63,48 @@ const ( BodyComZ // Inertia 3x3 matrix (column matrix organization, r,c labels). - InertiaXX - InertiaYX - InertiaZX - InertiaXY - InertiaYY - InertiaZY - InertiaXZ - InertiaYZ - InertiaZZ + BodyInertiaXX + BodyInertiaYX + BodyInertiaZX + BodyInertiaXY + BodyInertiaYY + BodyInertiaZY + BodyInertiaXZ + BodyInertiaYZ + BodyInertiaZZ // InvInertia inverse inertia 3x3 matrix (column matrix organization, r,c labels). - InvInertiaXX - InvInertiaYX - InvInertiaZX - InvInertiaXY - InvInertiaYY - InvInertiaZY - InvInertiaXZ - InvInertiaYZ - InvInertiaZZ + BodyInvInertiaXX + BodyInvInertiaYX + BodyInvInertiaZX + BodyInvInertiaXY + BodyInvInertiaYY + BodyInvInertiaZY + BodyInvInertiaXZ + BodyInvInertiaYZ + BodyInvInertiaZZ ) -func BodyShape(idx int32) Shapes { - return Shapes(math.Float32bits(Bodies.Value(int(idx), int(Shape)))) +func GetBodyShape(idx int32) Shapes { + return Shapes(math.Float32bits(Bodies.Value(int(idx), int(BodyShape)))) } func SetBodyShape(idx int32, shape Shapes) { - Bodies.Set(math.Float32frombits(uint32(shape)), int(idx), int(Shape)) + Bodies.Set(math.Float32frombits(uint32(shape)), int(idx), int(BodyShape)) } func BodySize(idx int32) math32.Vector3 { - var size math32.Vector3 - size.X = Bodies.Value(int(idx), int(SizeX)) - size.Y = Bodies.Value(int(idx), int(SizeY)) - size.Z = Bodies.Value(int(idx), int(SizeZ)) - return size + return math32.Vec3(Bodies.Value(int(idx), int(BodySizeX)), Bodies.Value(int(idx), int(BodySizeY)), Bodies.Value(int(idx), int(BodySizeZ))) } func SetBodySize(idx int32, size math32.Vector3) { - Bodies.Set(size.X, int(idx), int(SizeX)) - Bodies.Set(size.Y, int(idx), int(SizeY)) - Bodies.Set(size.Z, int(idx), int(SizeZ)) + Bodies.Set(size.X, int(idx), int(BodySizeX)) + Bodies.Set(size.Y, int(idx), int(BodySizeY)) + Bodies.Set(size.Z, int(idx), int(BodySizeZ)) } func BodyPos(idx int32) math32.Vector3 { - var pos math32.Vector3 - pos.X = Bodies.Value(int(idx), int(BodyPosX)) - pos.Y = Bodies.Value(int(idx), int(BodyPosY)) - pos.Z = Bodies.Value(int(idx), int(BodyPosZ)) - return pos + return math32.Vec3(Bodies.Value(int(idx), int(BodyPosX)), Bodies.Value(int(idx), int(BodyPosY)), Bodies.Value(int(idx), int(BodyPosZ))) } func SetBodyPos(idx int32, pos math32.Vector3) { @@ -122,12 +114,7 @@ func SetBodyPos(idx int32, pos math32.Vector3) { } func BodyRot(idx int32) math32.Quat { - var rot math32.Quat - rot.X = Bodies.Value(int(idx), int(BodyRotX)) - rot.Y = Bodies.Value(int(idx), int(BodyRotY)) - rot.Z = Bodies.Value(int(idx), int(BodyRotZ)) - rot.W = Bodies.Value(int(idx), int(BodyRotW)) - return rot + return math32.NewQuat(Bodies.Value(int(idx), int(BodyRotX)), Bodies.Value(int(idx), int(BodyRotY)), Bodies.Value(int(idx), int(BodyRotZ)), Bodies.Value(int(idx), int(BodyRotW))) } func SetBodyRot(idx int32, rot math32.Quat) { @@ -138,11 +125,7 @@ func SetBodyRot(idx int32, rot math32.Quat) { } func BodyCom(idx int32) math32.Vector3 { - var pos math32.Vector3 - pos.X = Bodies.Value(int(idx), int(BodyComX)) - pos.Y = Bodies.Value(int(idx), int(BodyComY)) - pos.Z = Bodies.Value(int(idx), int(BodyComZ)) - return pos + return math32.Vec3(Bodies.Value(int(idx), int(BodyComX)), Bodies.Value(int(idx), int(BodyComY)), Bodies.Value(int(idx), int(BodyComZ))) } func SetBodyCom(idx int32, pos math32.Vector3) { @@ -151,171 +134,186 @@ func SetBodyCom(idx int32, pos math32.Vector3) { Bodies.Set(pos.Z, int(idx), int(BodyComZ)) } +func BodyInertia(idx int32) math32.Matrix3 { + return math32.Mat3(Bodies.Value(int(idx), int(BodyInertiaXX)), Bodies.Value(int(idx), int(BodyInertiaYX)), Bodies.Value(int(idx), int(BodyInertiaZX)), + Bodies.Value(int(idx), int(BodyInertiaXY)), Bodies.Value(int(idx), int(BodyInertiaYY)), Bodies.Value(int(idx), int(BodyInertiaZY)), + Bodies.Value(int(idx), int(BodyInertiaXZ)), Bodies.Value(int(idx), int(BodyInertiaYZ)), Bodies.Value(int(idx), int(BodyInertiaZZ))) +} + +func BodyInvInertia(idx int32) math32.Matrix3 { + return math32.Mat3(Bodies.Value(int(idx), int(BodyInvInertiaXX)), Bodies.Value(int(idx), int(BodyInvInertiaYX)), Bodies.Value(int(idx), int(BodyInvInertiaZX)), + Bodies.Value(int(idx), int(BodyInvInertiaXY)), Bodies.Value(int(idx), int(BodyInvInertiaYY)), Bodies.Value(int(idx), int(BodyInvInertiaZY)), + Bodies.Value(int(idx), int(BodyInvInertiaXZ)), Bodies.Value(int(idx), int(BodyInvInertiaYZ)), Bodies.Value(int(idx), int(BodyInvInertiaZZ))) +} + +//////// Dynamic + // DynamicVars are dynamic body variables stored in tensor.Float32. type DynamicVars int32 //enums:enum const ( // Index of body in list of bodies. - Index DynamicVars = iota + DynIndex DynamicVars = iota // 3D position of center of mass. - PosX - PosY - PosZ + DynPosX + DynPosY + DynPosZ // Quaternion rotation. - RotX - RotY - RotZ - RotW + DynRotX + DynRotY + DynRotZ + DynRotW // Linear velocity. - VelX - VelY - VelZ + DynVelX + DynVelY + DynVelZ // Angular velocity. - AngVelX - AngVelY - AngVelZ + DynAngVelX + DynAngVelY + DynAngVelZ // Linear acceleration. - AccX - AccY - AccZ + DynAccX + DynAccY + DynAccZ // Angular acceleration due to applied torques. - AngAccX - AngAccY - AngAccZ + DynAngAccX + DynAngAccY + DynAngAccZ // Linear force driving linear acceleration (from joints, etc). - ForceX - ForceY - ForceZ + DynForceX + DynForceY + DynForceZ // Torque driving angular acceleration (from joints, etc). - TorqueX - TorqueY - TorqueZ + DynTorqueX + DynTorqueY + DynTorqueZ // Linear deltas. - DeltaX - DeltaY - DeltaZ + DynDeltaX + DynDeltaY + DynDeltaZ // Angular deltas. - AngDeltaX - AngDeltaY - AngDeltaZ + DynAngDeltaX + DynAngDeltaY + DynAngDeltaZ ) func SetDynamicIndex(idx, bodyIdx int32) { - Dynamics.Set(math.Float32frombits(uint32(bodyIdx)), int(idx), int(Index)) + Dynamics.Set(math.Float32frombits(uint32(bodyIdx)), int(idx), int(DynIndex)) } func DynamicIndex(idx int32) int32 { - return int32(math.Float32bits(Dynamics.Value(int(idx), int(Index)))) + return int32(math.Float32bits(Dynamics.Value(int(idx), int(DynIndex)))) } func DynamicPos(idx int32) math32.Vector3 { - var pos math32.Vector3 - pos.X = Dynamics.Value(int(idx), int(PosX)) - pos.Y = Dynamics.Value(int(idx), int(PosY)) - pos.Z = Dynamics.Value(int(idx), int(PosZ)) - return pos + return math32.Vec3(Dynamics.Value(int(idx), int(DynPosX)), Dynamics.Value(int(idx), int(DynPosY)), Dynamics.Value(int(idx), int(DynPosZ))) } func SetDynamicPos(idx int32, pos math32.Vector3) { - Dynamics.Set(pos.X, int(idx), int(PosX)) - Dynamics.Set(pos.Y, int(idx), int(PosY)) - Dynamics.Set(pos.Z, int(idx), int(PosZ)) + Dynamics.Set(pos.X, int(idx), int(DynPosX)) + Dynamics.Set(pos.Y, int(idx), int(DynPosY)) + Dynamics.Set(pos.Z, int(idx), int(DynPosZ)) } func DynamicRot(idx int32) math32.Quat { - var rot math32.Quat - rot.X = Dynamics.Value(int(idx), int(RotX)) - rot.Y = Dynamics.Value(int(idx), int(RotY)) - rot.Z = Dynamics.Value(int(idx), int(RotZ)) - rot.W = Dynamics.Value(int(idx), int(RotW)) - return rot + return math32.NewQuat(Dynamics.Value(int(idx), int(DynRotX)), Dynamics.Value(int(idx), int(DynRotY)), Dynamics.Value(int(idx), int(DynRotZ)), Dynamics.Value(int(idx), int(DynRotW))) } func SetDynamicRot(idx int32, rot math32.Quat) { - Dynamics.Set(rot.X, int(idx), int(RotX)) - Dynamics.Set(rot.Y, int(idx), int(RotY)) - Dynamics.Set(rot.Z, int(idx), int(RotZ)) - Dynamics.Set(rot.W, int(idx), int(RotW)) + Dynamics.Set(rot.X, int(idx), int(DynRotX)) + Dynamics.Set(rot.Y, int(idx), int(DynRotY)) + Dynamics.Set(rot.Z, int(idx), int(DynRotZ)) + Dynamics.Set(rot.W, int(idx), int(DynRotW)) } func DynamicVel(idx int32) math32.Vector3 { - var vel math32.Vector3 - vel.X = Dynamics.Value(int(idx), int(VelX)) - vel.Y = Dynamics.Value(int(idx), int(VelY)) - vel.Z = Dynamics.Value(int(idx), int(VelZ)) - return vel + return math32.Vec3(Dynamics.Value(int(idx), int(DynVelX)), Dynamics.Value(int(idx), int(DynVelY)), Dynamics.Value(int(idx), int(DynVelZ))) } func SetDynamicVel(idx int32, vel math32.Vector3) { - Dynamics.Set(vel.X, int(idx), int(VelX)) - Dynamics.Set(vel.Y, int(idx), int(VelY)) - Dynamics.Set(vel.Z, int(idx), int(VelZ)) + Dynamics.Set(vel.X, int(idx), int(DynVelX)) + Dynamics.Set(vel.Y, int(idx), int(DynVelY)) + Dynamics.Set(vel.Z, int(idx), int(DynVelZ)) } func DynamicAcc(idx int32) math32.Vector3 { - var acc math32.Vector3 - acc.X = Dynamics.Value(int(idx), int(AccX)) - acc.Y = Dynamics.Value(int(idx), int(AccY)) - acc.Z = Dynamics.Value(int(idx), int(AccZ)) - return acc + return math32.Vec3(Dynamics.Value(int(idx), int(DynAccX)), Dynamics.Value(int(idx), int(DynAccY)), Dynamics.Value(int(idx), int(DynAccZ))) } func SetDynamicAcc(idx int32, acc math32.Vector3) { - Dynamics.Set(acc.X, int(idx), int(AccX)) - Dynamics.Set(acc.Y, int(idx), int(AccY)) - Dynamics.Set(acc.Z, int(idx), int(AccZ)) + Dynamics.Set(acc.X, int(idx), int(DynAccX)) + Dynamics.Set(acc.Y, int(idx), int(DynAccY)) + Dynamics.Set(acc.Z, int(idx), int(DynAccZ)) } func DynamicForce(idx int32) math32.Vector3 { - var force math32.Vector3 - force.X = Dynamics.Value(int(idx), int(ForceX)) - force.Y = Dynamics.Value(int(idx), int(ForceY)) - force.Z = Dynamics.Value(int(idx), int(ForceZ)) - return force + return math32.Vec3(Dynamics.Value(int(idx), int(DynForceX)), Dynamics.Value(int(idx), int(DynForceY)), Dynamics.Value(int(idx), int(DynForceZ))) } func SetDynamicForce(idx int32, force math32.Vector3) { - Dynamics.Set(force.X, int(idx), int(ForceX)) - Dynamics.Set(force.Y, int(idx), int(ForceY)) - Dynamics.Set(force.Z, int(idx), int(ForceZ)) + Dynamics.Set(force.X, int(idx), int(DynForceX)) + Dynamics.Set(force.Y, int(idx), int(DynForceY)) + Dynamics.Set(force.Z, int(idx), int(DynForceZ)) +} + +func DynamicTorque(idx int32) math32.Vector3 { + return math32.Vec3(Dynamics.Value(int(idx), int(DynTorqueX)), Dynamics.Value(int(idx), int(DynTorqueY)), Dynamics.Value(int(idx), int(DynTorqueZ))) +} + +func SetDynamicTorque(idx int32, torque math32.Vector3) { + Dynamics.Set(torque.X, int(idx), int(DynTorqueX)) + Dynamics.Set(torque.Y, int(idx), int(DynTorqueY)) + Dynamics.Set(torque.Z, int(idx), int(DynTorqueZ)) } func DynamicAngVel(idx int32) math32.Vector3 { - var angVel math32.Vector3 - angVel.X = Dynamics.Value(int(idx), int(AngVelX)) - angVel.Y = Dynamics.Value(int(idx), int(AngVelY)) - angVel.Z = Dynamics.Value(int(idx), int(AngVelZ)) - return angVel + return math32.Vec3(Dynamics.Value(int(idx), int(DynAngVelX)), Dynamics.Value(int(idx), int(DynAngVelY)), Dynamics.Value(int(idx), int(DynAngVelZ))) } func SetDynamicAngVel(idx int32, angVel math32.Vector3) { - Dynamics.Set(angVel.X, int(idx), int(AngVelX)) - Dynamics.Set(angVel.Y, int(idx), int(AngVelY)) - Dynamics.Set(angVel.Z, int(idx), int(AngVelZ)) + Dynamics.Set(angVel.X, int(idx), int(DynAngVelX)) + Dynamics.Set(angVel.Y, int(idx), int(DynAngVelY)) + Dynamics.Set(angVel.Z, int(idx), int(DynAngVelZ)) } func DynamicAngAcc(idx int32) math32.Vector3 { - var angAcc math32.Vector3 - angAcc.X = Dynamics.Value(int(idx), int(AngAccX)) - angAcc.Y = Dynamics.Value(int(idx), int(AngAccY)) - angAcc.Z = Dynamics.Value(int(idx), int(AngAccZ)) - return angAcc + return math32.Vec3(Dynamics.Value(int(idx), int(DynAngAccX)), Dynamics.Value(int(idx), int(DynAngAccY)), Dynamics.Value(int(idx), int(DynAngAccZ))) } func SetDynamicAngAcc(idx int32, angAcc math32.Vector3) { - Dynamics.Set(angAcc.X, int(idx), int(AngAccX)) - Dynamics.Set(angAcc.Y, int(idx), int(AngAccY)) - Dynamics.Set(angAcc.Z, int(idx), int(AngAccZ)) + Dynamics.Set(angAcc.X, int(idx), int(DynAngAccX)) + Dynamics.Set(angAcc.Y, int(idx), int(DynAngAccY)) + Dynamics.Set(angAcc.Z, int(idx), int(DynAngAccZ)) +} + +func DynamicDelta(idx int32) math32.Vector3 { + return math32.Vec3(Dynamics.Value(int(idx), int(DynDeltaX)), Dynamics.Value(int(idx), int(DynDeltaY)), Dynamics.Value(int(idx), int(DynDeltaZ))) +} + +func SetDynamicDelta(idx int32, delta math32.Vector3) { + Dynamics.Set(delta.X, int(idx), int(DynDeltaX)) + Dynamics.Set(delta.Y, int(idx), int(DynDeltaY)) + Dynamics.Set(delta.Z, int(idx), int(DynDeltaZ)) +} + +func DynamicAngDelta(idx int32) math32.Vector3 { + return math32.Vec3(Dynamics.Value(int(idx), int(DynAngDeltaX)), Dynamics.Value(int(idx), int(DynAngDeltaY)), Dynamics.Value(int(idx), int(DynAngDeltaZ))) +} + +func SetDynamicAngDelta(idx int32, angDelta math32.Vector3) { + Dynamics.Set(angDelta.X, int(idx), int(DynAngDeltaX)) + Dynamics.Set(angDelta.Y, int(idx), int(DynAngDeltaY)) + Dynamics.Set(angDelta.Z, int(idx), int(DynAngDeltaZ)) } //gosl:end diff --git a/physics/body.goal b/physics/body.goal index ff04c397..d292a2b5 100644 --- a/physics/body.goal +++ b/physics/body.goal @@ -16,33 +16,33 @@ import ( type BodyVars int32 //enums:enum const ( - // Shape is the shape type of the object, as a Shapes type. - Shape BodyVars = iota + // BodyShape is the shape type of the object, as a Shapes type. + BodyShape BodyVars = iota - // WorldIndex partitions body into different worlds; Global are -1 - WorldIndex + // BodyWorldIndex partitions body into different worlds; Global are -1 + BodyWorldIndex - // Size is the size of the object (values depend on shape type). - SizeX - SizeY - SizeZ + // BodySize is the size of the object (values depend on shape type). + BodySizeX + BodySizeY + BodySizeZ // physical properties - // Mass is the mass of the object. - Mass + // BodyMass is the mass of the object. + BodyMass - // InvMass is 1/mass of the object or 0 if no mass. - InvMass + // BodyInvMass is 1/mass of the object or 0 if no mass. + BodyInvMass - // Bounce specifies the COR or coefficient of restitution (0..1), + // BodyBounce specifies the COR or coefficient of restitution (0..1), // which determines how elastic the collision is, // i.e., final velocity / initial velocity. - Bounce + BodyBounce - // Friction coefficient: how much friction is generated by transverse motion. + // BodyFriction coefficient: how much friction is generated by transverse motion. // Additive across the two surfaces. - Friction + BodyFriction // 3D position of body (structural center). BodyPosX @@ -61,56 +61,48 @@ const ( BodyComZ // Inertia 3x3 matrix (column matrix organization, r,c labels). - InertiaXX - InertiaYX - InertiaZX - InertiaXY - InertiaYY - InertiaZY - InertiaXZ - InertiaYZ - InertiaZZ + BodyInertiaXX + BodyInertiaYX + BodyInertiaZX + BodyInertiaXY + BodyInertiaYY + BodyInertiaZY + BodyInertiaXZ + BodyInertiaYZ + BodyInertiaZZ // InvInertia inverse inertia 3x3 matrix (column matrix organization, r,c labels). - InvInertiaXX - InvInertiaYX - InvInertiaZX - InvInertiaXY - InvInertiaYY - InvInertiaZY - InvInertiaXZ - InvInertiaYZ - InvInertiaZZ + BodyInvInertiaXX + BodyInvInertiaYX + BodyInvInertiaZX + BodyInvInertiaXY + BodyInvInertiaYY + BodyInvInertiaZY + BodyInvInertiaXZ + BodyInvInertiaYZ + BodyInvInertiaZZ ) -func BodyShape(idx int32) Shapes { - return Shapes(math.Float32bits(Bodies[idx, Shape])) +func GetBodyShape(idx int32) Shapes { + return Shapes(math.Float32bits(Bodies[idx, BodyShape])) } func SetBodyShape(idx int32, shape Shapes) { - Bodies[idx, Shape] = math.Float32frombits(uint32(shape)) + Bodies[idx, BodyShape] = math.Float32frombits(uint32(shape)) } func BodySize(idx int32) math32.Vector3 { - var size math32.Vector3 - size.X = Bodies[idx, SizeX] - size.Y = Bodies[idx, SizeY] - size.Z = Bodies[idx, SizeZ] - return size + return math32.Vec3(Bodies[idx, BodySizeX], Bodies[idx, BodySizeY], Bodies[idx, BodySizeZ]) } func SetBodySize(idx int32, size math32.Vector3) { - Bodies[idx, SizeX] = size.X - Bodies[idx, SizeY] = size.Y - Bodies[idx, SizeZ] = size.Z + Bodies[idx, BodySizeX] = size.X + Bodies[idx, BodySizeY] = size.Y + Bodies[idx, BodySizeZ] = size.Z } func BodyPos(idx int32) math32.Vector3 { - var pos math32.Vector3 - pos.X = Bodies[idx, BodyPosX] - pos.Y = Bodies[idx, BodyPosY] - pos.Z = Bodies[idx, BodyPosZ] - return pos + return math32.Vec3(Bodies[idx, BodyPosX], Bodies[idx, BodyPosY], Bodies[idx, BodyPosZ]) } func SetBodyPos(idx int32, pos math32.Vector3) { @@ -120,12 +112,7 @@ func SetBodyPos(idx int32, pos math32.Vector3) { } func BodyRot(idx int32) math32.Quat { - var rot math32.Quat - rot.X = Bodies[idx, BodyRotX] - rot.Y = Bodies[idx, BodyRotY] - rot.Z = Bodies[idx, BodyRotZ] - rot.W = Bodies[idx, BodyRotW] - return rot + return math32.NewQuat(Bodies[idx, BodyRotX], Bodies[idx, BodyRotY], Bodies[idx, BodyRotZ], Bodies[idx, BodyRotW]) } func SetBodyRot(idx int32, rot math32.Quat) { @@ -136,11 +123,7 @@ func SetBodyRot(idx int32, rot math32.Quat) { } func BodyCom(idx int32) math32.Vector3 { - var pos math32.Vector3 - pos.X = Bodies[idx, BodyComX] - pos.Y = Bodies[idx, BodyComY] - pos.Z = Bodies[idx, BodyComZ] - return pos + return math32.Vec3(Bodies[idx, BodyComX], Bodies[idx, BodyComY], Bodies[idx, BodyComZ]) } func SetBodyCom(idx int32, pos math32.Vector3) { @@ -149,171 +132,186 @@ func SetBodyCom(idx int32, pos math32.Vector3) { Bodies[idx, BodyComZ] = pos.Z } +func BodyInertia(idx int32) math32.Matrix3 { + return math32.Mat3(Bodies[idx, BodyInertiaXX], Bodies[idx, BodyInertiaYX], Bodies[idx, BodyInertiaZX], + Bodies[idx, BodyInertiaXY], Bodies[idx, BodyInertiaYY], Bodies[idx, BodyInertiaZY], + Bodies[idx, BodyInertiaXZ], Bodies[idx, BodyInertiaYZ], Bodies[idx, BodyInertiaZZ]) +} + +func BodyInvInertia(idx int32) math32.Matrix3 { + return math32.Mat3(Bodies[idx, BodyInvInertiaXX], Bodies[idx, BodyInvInertiaYX], Bodies[idx, BodyInvInertiaZX], + Bodies[idx, BodyInvInertiaXY], Bodies[idx, BodyInvInertiaYY], Bodies[idx, BodyInvInertiaZY], + Bodies[idx, BodyInvInertiaXZ], Bodies[idx, BodyInvInertiaYZ], Bodies[idx, BodyInvInertiaZZ]) +} + +//////// Dynamic + // DynamicVars are dynamic body variables stored in tensor.Float32. type DynamicVars int32 //enums:enum const ( // Index of body in list of bodies. - Index DynamicVars = iota + DynIndex DynamicVars = iota // 3D position of center of mass. - PosX - PosY - PosZ + DynPosX + DynPosY + DynPosZ // Quaternion rotation. - RotX - RotY - RotZ - RotW + DynRotX + DynRotY + DynRotZ + DynRotW // Linear velocity. - VelX - VelY - VelZ + DynVelX + DynVelY + DynVelZ // Angular velocity. - AngVelX - AngVelY - AngVelZ + DynAngVelX + DynAngVelY + DynAngVelZ // Linear acceleration. - AccX - AccY - AccZ + DynAccX + DynAccY + DynAccZ // Angular acceleration due to applied torques. - AngAccX - AngAccY - AngAccZ + DynAngAccX + DynAngAccY + DynAngAccZ // Linear force driving linear acceleration (from joints, etc). - ForceX - ForceY - ForceZ + DynForceX + DynForceY + DynForceZ // Torque driving angular acceleration (from joints, etc). - TorqueX - TorqueY - TorqueZ + DynTorqueX + DynTorqueY + DynTorqueZ // Linear deltas. - DeltaX - DeltaY - DeltaZ + DynDeltaX + DynDeltaY + DynDeltaZ // Angular deltas. - AngDeltaX - AngDeltaY - AngDeltaZ + DynAngDeltaX + DynAngDeltaY + DynAngDeltaZ ) func SetDynamicIndex(idx, bodyIdx int32) { - Dynamics[idx, Index] = math.Float32frombits(uint32(bodyIdx)) + Dynamics[idx, DynIndex] = math.Float32frombits(uint32(bodyIdx)) } func DynamicIndex(idx int32) int32 { - return int32(math.Float32bits(Dynamics[idx, Index])) + return int32(math.Float32bits(Dynamics[idx, DynIndex])) } func DynamicPos(idx int32) math32.Vector3 { - var pos math32.Vector3 - pos.X = Dynamics[idx, PosX] - pos.Y = Dynamics[idx, PosY] - pos.Z = Dynamics[idx, PosZ] - return pos + return math32.Vec3(Dynamics[idx, DynPosX], Dynamics[idx, DynPosY], Dynamics[idx, DynPosZ]) } func SetDynamicPos(idx int32, pos math32.Vector3) { - Dynamics[idx, PosX] = pos.X - Dynamics[idx, PosY] = pos.Y - Dynamics[idx, PosZ] = pos.Z + Dynamics[idx, DynPosX] = pos.X + Dynamics[idx, DynPosY] = pos.Y + Dynamics[idx, DynPosZ] = pos.Z } func DynamicRot(idx int32) math32.Quat { - var rot math32.Quat - rot.X = Dynamics[idx, RotX] - rot.Y = Dynamics[idx, RotY] - rot.Z = Dynamics[idx, RotZ] - rot.W = Dynamics[idx, RotW] - return rot + return math32.NewQuat(Dynamics[idx, DynRotX], Dynamics[idx, DynRotY], Dynamics[idx, DynRotZ], Dynamics[idx, DynRotW]) } func SetDynamicRot(idx int32, rot math32.Quat) { - Dynamics[idx, RotX] = rot.X - Dynamics[idx, RotY] = rot.Y - Dynamics[idx, RotZ] = rot.Z - Dynamics[idx, RotW] = rot.W + Dynamics[idx, DynRotX] = rot.X + Dynamics[idx, DynRotY] = rot.Y + Dynamics[idx, DynRotZ] = rot.Z + Dynamics[idx, DynRotW] = rot.W } func DynamicVel(idx int32) math32.Vector3 { - var vel math32.Vector3 - vel.X = Dynamics[idx, VelX] - vel.Y = Dynamics[idx, VelY] - vel.Z = Dynamics[idx, VelZ] - return vel + return math32.Vec3(Dynamics[idx, DynVelX], Dynamics[idx, DynVelY], Dynamics[idx, DynVelZ]) } func SetDynamicVel(idx int32, vel math32.Vector3) { - Dynamics[idx, VelX] = vel.X - Dynamics[idx, VelY] = vel.Y - Dynamics[idx, VelZ] = vel.Z + Dynamics[idx, DynVelX] = vel.X + Dynamics[idx, DynVelY] = vel.Y + Dynamics[idx, DynVelZ] = vel.Z } func DynamicAcc(idx int32) math32.Vector3 { - var acc math32.Vector3 - acc.X = Dynamics[idx, AccX] - acc.Y = Dynamics[idx, AccY] - acc.Z = Dynamics[idx, AccZ] - return acc + return math32.Vec3(Dynamics[idx, DynAccX], Dynamics[idx, DynAccY], Dynamics[idx, DynAccZ]) } func SetDynamicAcc(idx int32, acc math32.Vector3) { - Dynamics[idx, AccX] = acc.X - Dynamics[idx, AccY] = acc.Y - Dynamics[idx, AccZ] = acc.Z + Dynamics[idx, DynAccX] = acc.X + Dynamics[idx, DynAccY] = acc.Y + Dynamics[idx, DynAccZ] = acc.Z } func DynamicForce(idx int32) math32.Vector3 { - var force math32.Vector3 - force.X = Dynamics[idx, ForceX] - force.Y = Dynamics[idx, ForceY] - force.Z = Dynamics[idx, ForceZ] - return force + return math32.Vec3(Dynamics[idx, DynForceX], Dynamics[idx, DynForceY], Dynamics[idx, DynForceZ]) } func SetDynamicForce(idx int32, force math32.Vector3) { - Dynamics[idx, ForceX] = force.X - Dynamics[idx, ForceY] = force.Y - Dynamics[idx, ForceZ] = force.Z + Dynamics[idx, DynForceX] = force.X + Dynamics[idx, DynForceY] = force.Y + Dynamics[idx, DynForceZ] = force.Z +} + +func DynamicTorque(idx int32) math32.Vector3 { + return math32.Vec3(Dynamics[idx, DynTorqueX], Dynamics[idx, DynTorqueY], Dynamics[idx, DynTorqueZ]) +} + +func SetDynamicTorque(idx int32, torque math32.Vector3) { + Dynamics[idx, DynTorqueX] = torque.X + Dynamics[idx, DynTorqueY] = torque.Y + Dynamics[idx, DynTorqueZ] = torque.Z } func DynamicAngVel(idx int32) math32.Vector3 { - var angVel math32.Vector3 - angVel.X = Dynamics[idx, AngVelX] - angVel.Y = Dynamics[idx, AngVelY] - angVel.Z = Dynamics[idx, AngVelZ] - return angVel + return math32.Vec3(Dynamics[idx, DynAngVelX], Dynamics[idx, DynAngVelY], Dynamics[idx, DynAngVelZ]) } func SetDynamicAngVel(idx int32, angVel math32.Vector3) { - Dynamics[idx, AngVelX] = angVel.X - Dynamics[idx, AngVelY] = angVel.Y - Dynamics[idx, AngVelZ] = angVel.Z + Dynamics[idx, DynAngVelX] = angVel.X + Dynamics[idx, DynAngVelY] = angVel.Y + Dynamics[idx, DynAngVelZ] = angVel.Z } func DynamicAngAcc(idx int32) math32.Vector3 { - var angAcc math32.Vector3 - angAcc.X = Dynamics[idx, AngAccX] - angAcc.Y = Dynamics[idx, AngAccY] - angAcc.Z = Dynamics[idx, AngAccZ] - return angAcc + return math32.Vec3(Dynamics[idx, DynAngAccX], Dynamics[idx, DynAngAccY], Dynamics[idx, DynAngAccZ]) } func SetDynamicAngAcc(idx int32, angAcc math32.Vector3) { - Dynamics[idx, AngAccX] = angAcc.X - Dynamics[idx, AngAccY] = angAcc.Y - Dynamics[idx, AngAccZ] = angAcc.Z + Dynamics[idx, DynAngAccX] = angAcc.X + Dynamics[idx, DynAngAccY] = angAcc.Y + Dynamics[idx, DynAngAccZ] = angAcc.Z +} + +func DynamicDelta(idx int32) math32.Vector3 { + return math32.Vec3(Dynamics[idx, DynDeltaX], Dynamics[idx, DynDeltaY], Dynamics[idx, DynDeltaZ]) +} + +func SetDynamicDelta(idx int32, delta math32.Vector3) { + Dynamics[idx, DynDeltaX] = delta.X + Dynamics[idx, DynDeltaY] = delta.Y + Dynamics[idx, DynDeltaZ] = delta.Z +} + +func DynamicAngDelta(idx int32) math32.Vector3 { + return math32.Vec3(Dynamics[idx, DynAngDeltaX], Dynamics[idx, DynAngDeltaY], Dynamics[idx, DynAngDeltaZ]) +} + +func SetDynamicAngDelta(idx int32, angDelta math32.Vector3) { + Dynamics[idx, DynAngDeltaX] = angDelta.X + Dynamics[idx, DynAngDeltaY] = angDelta.Y + Dynamics[idx, DynAngDeltaZ] = angDelta.Z } //gosl:end diff --git a/physics/enumgen.go b/physics/enumgen.go index 3319d1bb..b309ba97 100644 --- a/physics/enumgen.go +++ b/physics/enumgen.go @@ -15,11 +15,11 @@ const BodyVarsN BodyVars = 37 //gosl:end -var _BodyVarsValueMap = map[string]BodyVars{`Shape`: 0, `WorldIndex`: 1, `SizeX`: 2, `SizeY`: 3, `SizeZ`: 4, `Mass`: 5, `InvMass`: 6, `Bounce`: 7, `Friction`: 8, `BodyPosX`: 9, `BodyPosY`: 10, `BodyPosZ`: 11, `BodyRotX`: 12, `BodyRotY`: 13, `BodyRotZ`: 14, `BodyRotW`: 15, `BodyComX`: 16, `BodyComY`: 17, `BodyComZ`: 18, `InertiaXX`: 19, `InertiaYX`: 20, `InertiaZX`: 21, `InertiaXY`: 22, `InertiaYY`: 23, `InertiaZY`: 24, `InertiaXZ`: 25, `InertiaYZ`: 26, `InertiaZZ`: 27, `InvInertiaXX`: 28, `InvInertiaYX`: 29, `InvInertiaZX`: 30, `InvInertiaXY`: 31, `InvInertiaYY`: 32, `InvInertiaZY`: 33, `InvInertiaXZ`: 34, `InvInertiaYZ`: 35, `InvInertiaZZ`: 36} +var _BodyVarsValueMap = map[string]BodyVars{`BodyShape`: 0, `BodyWorldIndex`: 1, `BodySizeX`: 2, `BodySizeY`: 3, `BodySizeZ`: 4, `BodyMass`: 5, `BodyInvMass`: 6, `BodyBounce`: 7, `BodyFriction`: 8, `BodyPosX`: 9, `BodyPosY`: 10, `BodyPosZ`: 11, `BodyRotX`: 12, `BodyRotY`: 13, `BodyRotZ`: 14, `BodyRotW`: 15, `BodyComX`: 16, `BodyComY`: 17, `BodyComZ`: 18, `BodyInertiaXX`: 19, `BodyInertiaYX`: 20, `BodyInertiaZX`: 21, `BodyInertiaXY`: 22, `BodyInertiaYY`: 23, `BodyInertiaZY`: 24, `BodyInertiaXZ`: 25, `BodyInertiaYZ`: 26, `BodyInertiaZZ`: 27, `BodyInvInertiaXX`: 28, `BodyInvInertiaYX`: 29, `BodyInvInertiaZX`: 30, `BodyInvInertiaXY`: 31, `BodyInvInertiaYY`: 32, `BodyInvInertiaZY`: 33, `BodyInvInertiaXZ`: 34, `BodyInvInertiaYZ`: 35, `BodyInvInertiaZZ`: 36} -var _BodyVarsDescMap = map[BodyVars]string{0: `Shape is the shape type of the object, as a Shapes type.`, 1: `WorldIndex partitions body into different worlds; Global are -1`, 2: `Size is the size of the object (values depend on shape type).`, 3: ``, 4: ``, 5: `Mass is the mass of the object.`, 6: `InvMass is 1/mass of the object or 0 if no mass.`, 7: `Bounce specifies the COR or coefficient of restitution (0..1), which determines how elastic the collision is, i.e., final velocity / initial velocity.`, 8: `Friction coefficient: how much friction is generated by transverse motion. Additive across the two surfaces.`, 9: `3D position of body (structural center).`, 10: ``, 11: ``, 12: `Quaternion rotation of body.`, 13: ``, 14: ``, 15: ``, 16: `Relative center-of-mass offset from 3D position of body.`, 17: ``, 18: ``, 19: `Inertia 3x3 matrix (column matrix organization, r,c labels).`, 20: ``, 21: ``, 22: ``, 23: ``, 24: ``, 25: ``, 26: ``, 27: ``, 28: `InvInertia inverse inertia 3x3 matrix (column matrix organization, r,c labels).`, 29: ``, 30: ``, 31: ``, 32: ``, 33: ``, 34: ``, 35: ``, 36: ``} +var _BodyVarsDescMap = map[BodyVars]string{0: `BodyShape is the shape type of the object, as a Shapes type.`, 1: `BodyWorldIndex partitions body into different worlds; Global are -1`, 2: `BodySize is the size of the object (values depend on shape type).`, 3: ``, 4: ``, 5: `BodyMass is the mass of the object.`, 6: `BodyInvMass is 1/mass of the object or 0 if no mass.`, 7: `BodyBounce specifies the COR or coefficient of restitution (0..1), which determines how elastic the collision is, i.e., final velocity / initial velocity.`, 8: `BodyFriction coefficient: how much friction is generated by transverse motion. Additive across the two surfaces.`, 9: `3D position of body (structural center).`, 10: ``, 11: ``, 12: `Quaternion rotation of body.`, 13: ``, 14: ``, 15: ``, 16: `Relative center-of-mass offset from 3D position of body.`, 17: ``, 18: ``, 19: `Inertia 3x3 matrix (column matrix organization, r,c labels).`, 20: ``, 21: ``, 22: ``, 23: ``, 24: ``, 25: ``, 26: ``, 27: ``, 28: `InvInertia inverse inertia 3x3 matrix (column matrix organization, r,c labels).`, 29: ``, 30: ``, 31: ``, 32: ``, 33: ``, 34: ``, 35: ``, 36: ``} -var _BodyVarsMap = map[BodyVars]string{0: `Shape`, 1: `WorldIndex`, 2: `SizeX`, 3: `SizeY`, 4: `SizeZ`, 5: `Mass`, 6: `InvMass`, 7: `Bounce`, 8: `Friction`, 9: `BodyPosX`, 10: `BodyPosY`, 11: `BodyPosZ`, 12: `BodyRotX`, 13: `BodyRotY`, 14: `BodyRotZ`, 15: `BodyRotW`, 16: `BodyComX`, 17: `BodyComY`, 18: `BodyComZ`, 19: `InertiaXX`, 20: `InertiaYX`, 21: `InertiaZX`, 22: `InertiaXY`, 23: `InertiaYY`, 24: `InertiaZY`, 25: `InertiaXZ`, 26: `InertiaYZ`, 27: `InertiaZZ`, 28: `InvInertiaXX`, 29: `InvInertiaYX`, 30: `InvInertiaZX`, 31: `InvInertiaXY`, 32: `InvInertiaYY`, 33: `InvInertiaZY`, 34: `InvInertiaXZ`, 35: `InvInertiaYZ`, 36: `InvInertiaZZ`} +var _BodyVarsMap = map[BodyVars]string{0: `BodyShape`, 1: `BodyWorldIndex`, 2: `BodySizeX`, 3: `BodySizeY`, 4: `BodySizeZ`, 5: `BodyMass`, 6: `BodyInvMass`, 7: `BodyBounce`, 8: `BodyFriction`, 9: `BodyPosX`, 10: `BodyPosY`, 11: `BodyPosZ`, 12: `BodyRotX`, 13: `BodyRotY`, 14: `BodyRotZ`, 15: `BodyRotW`, 16: `BodyComX`, 17: `BodyComY`, 18: `BodyComZ`, 19: `BodyInertiaXX`, 20: `BodyInertiaYX`, 21: `BodyInertiaZX`, 22: `BodyInertiaXY`, 23: `BodyInertiaYY`, 24: `BodyInertiaZY`, 25: `BodyInertiaXZ`, 26: `BodyInertiaYZ`, 27: `BodyInertiaZZ`, 28: `BodyInvInertiaXX`, 29: `BodyInvInertiaYX`, 30: `BodyInvInertiaZX`, 31: `BodyInvInertiaXY`, 32: `BodyInvInertiaYY`, 33: `BodyInvInertiaZY`, 34: `BodyInvInertiaXZ`, 35: `BodyInvInertiaYZ`, 36: `BodyInvInertiaZZ`} // String returns the string representation of this BodyVars value. func (i BodyVars) String() string { return enums.String(i, _BodyVarsMap) } @@ -60,11 +60,11 @@ const DynamicVarsN DynamicVars = 32 //gosl:end -var _DynamicVarsValueMap = map[string]DynamicVars{`Index`: 0, `PosX`: 1, `PosY`: 2, `PosZ`: 3, `RotX`: 4, `RotY`: 5, `RotZ`: 6, `RotW`: 7, `VelX`: 8, `VelY`: 9, `VelZ`: 10, `AngVelX`: 11, `AngVelY`: 12, `AngVelZ`: 13, `AccX`: 14, `AccY`: 15, `AccZ`: 16, `AngAccX`: 17, `AngAccY`: 18, `AngAccZ`: 19, `ForceX`: 20, `ForceY`: 21, `ForceZ`: 22, `TorqueX`: 23, `TorqueY`: 24, `TorqueZ`: 25, `DeltaX`: 26, `DeltaY`: 27, `DeltaZ`: 28, `AngDeltaX`: 29, `AngDeltaY`: 30, `AngDeltaZ`: 31} +var _DynamicVarsValueMap = map[string]DynamicVars{`DynIndex`: 0, `DynPosX`: 1, `DynPosY`: 2, `DynPosZ`: 3, `DynRotX`: 4, `DynRotY`: 5, `DynRotZ`: 6, `DynRotW`: 7, `DynVelX`: 8, `DynVelY`: 9, `DynVelZ`: 10, `DynAngVelX`: 11, `DynAngVelY`: 12, `DynAngVelZ`: 13, `DynAccX`: 14, `DynAccY`: 15, `DynAccZ`: 16, `DynAngAccX`: 17, `DynAngAccY`: 18, `DynAngAccZ`: 19, `DynForceX`: 20, `DynForceY`: 21, `DynForceZ`: 22, `DynTorqueX`: 23, `DynTorqueY`: 24, `DynTorqueZ`: 25, `DynDeltaX`: 26, `DynDeltaY`: 27, `DynDeltaZ`: 28, `DynAngDeltaX`: 29, `DynAngDeltaY`: 30, `DynAngDeltaZ`: 31} var _DynamicVarsDescMap = map[DynamicVars]string{0: `Index of body in list of bodies.`, 1: `3D position of center of mass.`, 2: ``, 3: ``, 4: `Quaternion rotation.`, 5: ``, 6: ``, 7: ``, 8: `Linear velocity.`, 9: ``, 10: ``, 11: `Angular velocity.`, 12: ``, 13: ``, 14: `Linear acceleration.`, 15: ``, 16: ``, 17: `Angular acceleration due to applied torques.`, 18: ``, 19: ``, 20: `Linear force driving linear acceleration (from joints, etc).`, 21: ``, 22: ``, 23: `Torque driving angular acceleration (from joints, etc).`, 24: ``, 25: ``, 26: `Linear deltas.`, 27: ``, 28: ``, 29: `Angular deltas.`, 30: ``, 31: ``} -var _DynamicVarsMap = map[DynamicVars]string{0: `Index`, 1: `PosX`, 2: `PosY`, 3: `PosZ`, 4: `RotX`, 5: `RotY`, 6: `RotZ`, 7: `RotW`, 8: `VelX`, 9: `VelY`, 10: `VelZ`, 11: `AngVelX`, 12: `AngVelY`, 13: `AngVelZ`, 14: `AccX`, 15: `AccY`, 16: `AccZ`, 17: `AngAccX`, 18: `AngAccY`, 19: `AngAccZ`, 20: `ForceX`, 21: `ForceY`, 22: `ForceZ`, 23: `TorqueX`, 24: `TorqueY`, 25: `TorqueZ`, 26: `DeltaX`, 27: `DeltaY`, 28: `DeltaZ`, 29: `AngDeltaX`, 30: `AngDeltaY`, 31: `AngDeltaZ`} +var _DynamicVarsMap = map[DynamicVars]string{0: `DynIndex`, 1: `DynPosX`, 2: `DynPosY`, 3: `DynPosZ`, 4: `DynRotX`, 5: `DynRotY`, 6: `DynRotZ`, 7: `DynRotW`, 8: `DynVelX`, 9: `DynVelY`, 10: `DynVelZ`, 11: `DynAngVelX`, 12: `DynAngVelY`, 13: `DynAngVelZ`, 14: `DynAccX`, 15: `DynAccY`, 16: `DynAccZ`, 17: `DynAngAccX`, 18: `DynAngAccY`, 19: `DynAngAccZ`, 20: `DynForceX`, 21: `DynForceY`, 22: `DynForceZ`, 23: `DynTorqueX`, 24: `DynTorqueY`, 25: `DynTorqueZ`, 26: `DynDeltaX`, 27: `DynDeltaY`, 28: `DynDeltaZ`, 29: `DynAngDeltaX`, 30: `DynAngDeltaY`, 31: `DynAngDeltaZ`} // String returns the string representation of this DynamicVars value. func (i DynamicVars) String() string { return enums.String(i, _DynamicVarsMap) } diff --git a/physics/gosl.go b/physics/gosl.go index dd6b7a04..e396127b 100644 --- a/physics/gosl.go +++ b/physics/gosl.go @@ -111,6 +111,11 @@ func GPUInit() { pl.AddVarUsed(1, "Bodies") pl.AddVarUsed(2, "Dynamics") pl.AddVarUsed(0, "Params") + pl = gpu.NewComputePipelineShaderFS(shaders, "shaders/StepIntegrateBodies.wgsl", sy) + pl.AddVarUsed(0, "TensorStrides") + pl.AddVarUsed(1, "Bodies") + pl.AddVarUsed(2, "Dynamics") + pl.AddVarUsed(0, "Params") pl = gpu.NewComputePipelineShaderFS(shaders, "shaders/StepJoints.wgsl", sy) pl.AddVarUsed(0, "TensorStrides") pl.AddVarUsed(1, "Bodies") @@ -178,6 +183,48 @@ func RunOneInitDynamics(n int, syncVars ...GPUVars) { RunInitDynamicsCPU(n) } } +// RunStepIntegrateBodies runs the StepIntegrateBodies kernel with given number of elements, +// on either the CPU or GPU depending on the UseGPU variable. +// Can call multiple Run* kernels in a row, which are then all launched +// in the same command submission on the GPU, which is by far the most efficient. +// MUST call RunDone (with optional vars to sync) after all Run calls. +// Alternatively, a single-shot RunOneStepIntegrateBodies call does Run and Done for a +// single run-and-sync case. +func RunStepIntegrateBodies(n int) { + if UseGPU { + RunStepIntegrateBodiesGPU(n) + } else { + RunStepIntegrateBodiesCPU(n) + } +} + +// RunStepIntegrateBodiesGPU runs the StepIntegrateBodies kernel on the GPU. See [RunStepIntegrateBodies] for more info. +func RunStepIntegrateBodiesGPU(n int) { + sy := GPUSystem + pl := sy.ComputePipelines["StepIntegrateBodies"] + ce, _ := sy.BeginComputePass() + pl.Dispatch1D(ce, n, 64) +} + +// RunStepIntegrateBodiesCPU runs the StepIntegrateBodies kernel on the CPU. +func RunStepIntegrateBodiesCPU(n int) { + gpu.VectorizeFunc(0, n, StepIntegrateBodies) +} + +// RunOneStepIntegrateBodies runs the StepIntegrateBodies kernel with given number of elements, +// on either the CPU or GPU depending on the UseGPU variable. +// This version then calls RunDone with the given variables to sync +// after the Run, for a single-shot Run-and-Done call. If multiple kernels +// can be run in sequence, it is much more efficient to do multiple Run* +// calls followed by a RunDone call. +func RunOneStepIntegrateBodies(n int, syncVars ...GPUVars) { + if UseGPU { + RunStepIntegrateBodiesGPU(n) + RunDone(syncVars...) + } else { + RunStepIntegrateBodiesCPU(n) + } +} // RunStepJoints runs the StepJoints kernel with given number of elements, // on either the CPU or GPU depending on the UseGPU variable. // Can call multiple Run* kernels in a row, which are then all launched diff --git a/physics/params.go b/physics/params.go index 26f44fe8..d5811510 100644 --- a/physics/params.go +++ b/physics/params.go @@ -22,6 +22,9 @@ type PhysParams struct { // Iters is the number of iterations to perform. Iters int32 `default:"2"` + // Dt is the integration stepsize. + Dt float32 `default:"0.01"` + // SoftRelax is soft-body relaxation constant. SoftRelax float32 `default:"0.9"` @@ -49,12 +52,15 @@ type PhysParams struct { // Restitution Restitution slbool.Bool `default:"false"` + pad, pad1, pad2 float32 + // Gravity is the gravity acceleration function Gravity slvec.Vector3 } func (pr *PhysParams) Defaults() { pr.Iters = 2 + pr.Dt = 0.01 pr.Gravity.Set(0, -9.81, 0) pr.SoftRelax = 0.9 pr.JointLinearRelax = 0.7 diff --git a/physics/shaders/InitDynamics.wgsl b/physics/shaders/InitDynamics.wgsl index d78253ab..06553dcd 100644 --- a/physics/shaders/InitDynamics.wgsl +++ b/physics/shaders/InitDynamics.wgsl @@ -31,15 +31,15 @@ fn Index2D(s0: u32, s1: u32, i0: u32, i1: u32) -> u32 { //////// import: "body.go" alias BodyVars = i32; //enums:enum -const Shape: BodyVars = 0; -const WorldIndex: BodyVars = 1; -const SizeX: BodyVars = 2; -const SizeY: BodyVars = 3; -const SizeZ: BodyVars = 4; -const Mass: BodyVars = 5; -const InvMass: BodyVars = 6; -const Bounce: BodyVars = 7; -const Friction: BodyVars = 8; +const BodyShape: BodyVars = 0; +const BodyWorldIndex: BodyVars = 1; +const BodySizeX: BodyVars = 2; +const BodySizeY: BodyVars = 3; +const BodySizeZ: BodyVars = 4; +const BodyMass: BodyVars = 5; +const BodyInvMass: BodyVars = 6; +const BodyBounce: BodyVars = 7; +const BodyFriction: BodyVars = 8; const BodyPosX: BodyVars = 9; const BodyPosY: BodyVars = 10; const BodyPosZ: BodyVars = 11; @@ -50,59 +50,59 @@ const BodyRotW: BodyVars = 15; const BodyComX: BodyVars = 16; const BodyComY: BodyVars = 17; const BodyComZ: BodyVars = 18; -const InertiaXX: BodyVars = 19; -const InertiaYX: BodyVars = 20; -const InertiaZX: BodyVars = 21; -const InertiaXY: BodyVars = 22; -const InertiaYY: BodyVars = 23; -const InertiaZY: BodyVars = 24; -const InertiaXZ: BodyVars = 25; -const InertiaYZ: BodyVars = 26; -const InertiaZZ: BodyVars = 27; -const InvInertiaXX: BodyVars = 28; -const InvInertiaYX: BodyVars = 29; -const InvInertiaZX: BodyVars = 30; -const InvInertiaXY: BodyVars = 31; -const InvInertiaYY: BodyVars = 32; -const InvInertiaZY: BodyVars = 33; -const InvInertiaXZ: BodyVars = 34; -const InvInertiaYZ: BodyVars = 35; -const InvInertiaZZ: BodyVars = 36; +const BodyInertiaXX: BodyVars = 19; +const BodyInertiaYX: BodyVars = 20; +const BodyInertiaZX: BodyVars = 21; +const BodyInertiaXY: BodyVars = 22; +const BodyInertiaYY: BodyVars = 23; +const BodyInertiaZY: BodyVars = 24; +const BodyInertiaXZ: BodyVars = 25; +const BodyInertiaYZ: BodyVars = 26; +const BodyInertiaZZ: BodyVars = 27; +const BodyInvInertiaXX: BodyVars = 28; +const BodyInvInertiaYX: BodyVars = 29; +const BodyInvInertiaZX: BodyVars = 30; +const BodyInvInertiaXY: BodyVars = 31; +const BodyInvInertiaYY: BodyVars = 32; +const BodyInvInertiaZY: BodyVars = 33; +const BodyInvInertiaXZ: BodyVars = 34; +const BodyInvInertiaYZ: BodyVars = 35; +const BodyInvInertiaZZ: BodyVars = 36; alias DynamicVars = i32; //enums:enum -const Index: DynamicVars = 0; -const PosX: DynamicVars = 1; -const PosY: DynamicVars = 2; -const PosZ: DynamicVars = 3; -const RotX: DynamicVars = 4; -const RotY: DynamicVars = 5; -const RotZ: DynamicVars = 6; -const RotW: DynamicVars = 7; -const VelX: DynamicVars = 8; -const VelY: DynamicVars = 9; -const VelZ: DynamicVars = 10; -const AngVelX: DynamicVars = 11; -const AngVelY: DynamicVars = 12; -const AngVelZ: DynamicVars = 13; -const AccX: DynamicVars = 14; -const AccY: DynamicVars = 15; -const AccZ: DynamicVars = 16; -const AngAccX: DynamicVars = 17; -const AngAccY: DynamicVars = 18; -const AngAccZ: DynamicVars = 19; -const ForceX: DynamicVars = 20; -const ForceY: DynamicVars = 21; -const ForceZ: DynamicVars = 22; -const TorqueX: DynamicVars = 23; -const TorqueY: DynamicVars = 24; -const TorqueZ: DynamicVars = 25; -const DeltaX: DynamicVars = 26; -const DeltaY: DynamicVars = 27; -const DeltaZ: DynamicVars = 28; -const AngDeltaX: DynamicVars = 29; -const AngDeltaY: DynamicVars = 30; -const AngDeltaZ: DynamicVars = 31; +const DynIndex: DynamicVars = 0; +const DynPosX: DynamicVars = 1; +const DynPosY: DynamicVars = 2; +const DynPosZ: DynamicVars = 3; +const DynRotX: DynamicVars = 4; +const DynRotY: DynamicVars = 5; +const DynRotZ: DynamicVars = 6; +const DynRotW: DynamicVars = 7; +const DynVelX: DynamicVars = 8; +const DynVelY: DynamicVars = 9; +const DynVelZ: DynamicVars = 10; +const DynAngVelX: DynamicVars = 11; +const DynAngVelY: DynamicVars = 12; +const DynAngVelZ: DynamicVars = 13; +const DynAccX: DynamicVars = 14; +const DynAccY: DynamicVars = 15; +const DynAccZ: DynamicVars = 16; +const DynAngAccX: DynamicVars = 17; +const DynAngAccY: DynamicVars = 18; +const DynAngAccZ: DynamicVars = 19; +const DynForceX: DynamicVars = 20; +const DynForceY: DynamicVars = 21; +const DynForceZ: DynamicVars = 22; +const DynTorqueX: DynamicVars = 23; +const DynTorqueY: DynamicVars = 24; +const DynTorqueZ: DynamicVars = 25; +const DynDeltaX: DynamicVars = 26; +const DynDeltaY: DynamicVars = 27; +const DynDeltaZ: DynamicVars = 28; +const DynAngDeltaX: DynamicVars = 29; +const DynAngDeltaY: DynamicVars = 30; +const DynAngDeltaZ: DynamicVars = 31; fn DynamicIndex(idx: i32) -> i32 { - return i32(bitcast(Dynamics[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(Index))])); + return i32(bitcast(Dynamics[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(DynIndex))])); } //////// import: "contact.go" @@ -200,6 +200,7 @@ struct PhysParams { DynamicsN: i32, JointsN: i32, Iters: i32, + Dt: f32, SoftRelax: f32, JointLinearRelax: f32, JointAngularRelax: f32, @@ -209,6 +210,9 @@ struct PhysParams { AngularDamping: f32, ContactWeighting: i32, Restitution: i32, + pad: f32, + pad1: f32, + pad2: f32, Gravity: vec4, } @@ -219,6 +223,8 @@ const Sphere: Shapes = 1; const Cylinder: Shapes = 2; const Capsule: Shapes = 3; +//////// import: "slmath-matrix3.go" + //////// import: "slmath-quaternion.go" //////// import: "slmath-vector3.go" @@ -231,14 +237,14 @@ fn InitDynamics(i: u32) { //gosl:kernel return; } var bi = DynamicIndex(ii); - Dynamics[Index2D(TensorStrides[30], TensorStrides[31], u32(ii), u32(PosX))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyPosX))]; - Dynamics[Index2D(TensorStrides[30], TensorStrides[31], u32(ii), u32(PosY))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyPosY))]; - Dynamics[Index2D(TensorStrides[30], TensorStrides[31], u32(ii), u32(PosZ))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyPosZ))]; - Dynamics[Index2D(TensorStrides[30], TensorStrides[31], u32(ii), u32(RotX))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyRotX))]; - Dynamics[Index2D(TensorStrides[30], TensorStrides[31], u32(ii), u32(RotY))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyRotY))]; - Dynamics[Index2D(TensorStrides[30], TensorStrides[31], u32(ii), u32(RotZ))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyRotZ))]; - Dynamics[Index2D(TensorStrides[30], TensorStrides[31], u32(ii), u32(RotW))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyRotW))]; - for (var v = VelX; v < DynamicVarsN; v++) { + Dynamics[Index2D(TensorStrides[30], TensorStrides[31], u32(ii), u32(DynPosX))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyPosX))]; + Dynamics[Index2D(TensorStrides[30], TensorStrides[31], u32(ii), u32(DynPosY))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyPosY))]; + Dynamics[Index2D(TensorStrides[30], TensorStrides[31], u32(ii), u32(DynPosZ))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyPosZ))]; + Dynamics[Index2D(TensorStrides[30], TensorStrides[31], u32(ii), u32(DynRotX))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyRotX))]; + Dynamics[Index2D(TensorStrides[30], TensorStrides[31], u32(ii), u32(DynRotY))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyRotY))]; + Dynamics[Index2D(TensorStrides[30], TensorStrides[31], u32(ii), u32(DynRotZ))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyRotZ))]; + Dynamics[Index2D(TensorStrides[30], TensorStrides[31], u32(ii), u32(DynRotW))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyRotW))]; + for (var v = DynVelX; v < DynamicVarsN; v++) { Dynamics[Index2D(TensorStrides[30], TensorStrides[31], u32(ii), u32(v))] = 0.0; } diff --git a/physics/shaders/StepJoints.wgsl b/physics/shaders/StepJoints.wgsl index 5ef8f590..5d90f625 100644 --- a/physics/shaders/StepJoints.wgsl +++ b/physics/shaders/StepJoints.wgsl @@ -33,15 +33,15 @@ fn Index2D(s0: u32, s1: u32, i0: u32, i1: u32) -> u32 { //////// import: "body.go" alias BodyVars = i32; //enums:enum -const Shape: BodyVars = 0; -const WorldIndex: BodyVars = 1; -const SizeX: BodyVars = 2; -const SizeY: BodyVars = 3; -const SizeZ: BodyVars = 4; -const Mass: BodyVars = 5; -const InvMass: BodyVars = 6; -const Bounce: BodyVars = 7; -const Friction: BodyVars = 8; +const BodyShape: BodyVars = 0; +const BodyWorldIndex: BodyVars = 1; +const BodySizeX: BodyVars = 2; +const BodySizeY: BodyVars = 3; +const BodySizeZ: BodyVars = 4; +const BodyMass: BodyVars = 5; +const BodyInvMass: BodyVars = 6; +const BodyBounce: BodyVars = 7; +const BodyFriction: BodyVars = 8; const BodyPosX: BodyVars = 9; const BodyPosY: BodyVars = 10; const BodyPosZ: BodyVars = 11; @@ -52,78 +52,68 @@ const BodyRotW: BodyVars = 15; const BodyComX: BodyVars = 16; const BodyComY: BodyVars = 17; const BodyComZ: BodyVars = 18; -const InertiaXX: BodyVars = 19; -const InertiaYX: BodyVars = 20; -const InertiaZX: BodyVars = 21; -const InertiaXY: BodyVars = 22; -const InertiaYY: BodyVars = 23; -const InertiaZY: BodyVars = 24; -const InertiaXZ: BodyVars = 25; -const InertiaYZ: BodyVars = 26; -const InertiaZZ: BodyVars = 27; -const InvInertiaXX: BodyVars = 28; -const InvInertiaYX: BodyVars = 29; -const InvInertiaZX: BodyVars = 30; -const InvInertiaXY: BodyVars = 31; -const InvInertiaYY: BodyVars = 32; -const InvInertiaZY: BodyVars = 33; -const InvInertiaXZ: BodyVars = 34; -const InvInertiaYZ: BodyVars = 35; -const InvInertiaZZ: BodyVars = 36; +const BodyInertiaXX: BodyVars = 19; +const BodyInertiaYX: BodyVars = 20; +const BodyInertiaZX: BodyVars = 21; +const BodyInertiaXY: BodyVars = 22; +const BodyInertiaYY: BodyVars = 23; +const BodyInertiaZY: BodyVars = 24; +const BodyInertiaXZ: BodyVars = 25; +const BodyInertiaYZ: BodyVars = 26; +const BodyInertiaZZ: BodyVars = 27; +const BodyInvInertiaXX: BodyVars = 28; +const BodyInvInertiaYX: BodyVars = 29; +const BodyInvInertiaZX: BodyVars = 30; +const BodyInvInertiaXY: BodyVars = 31; +const BodyInvInertiaYY: BodyVars = 32; +const BodyInvInertiaZY: BodyVars = 33; +const BodyInvInertiaXZ: BodyVars = 34; +const BodyInvInertiaYZ: BodyVars = 35; +const BodyInvInertiaZZ: BodyVars = 36; fn BodyCom(idx: i32) -> vec3 { - var pos: vec3; - pos.x = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComX))]; - pos.y = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComY))]; - pos.z = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComZ))];return pos; + return vec3(Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComX))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComY))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComZ))]); } alias DynamicVars = i32; //enums:enum -const Index: DynamicVars = 0; -const PosX: DynamicVars = 1; -const PosY: DynamicVars = 2; -const PosZ: DynamicVars = 3; -const RotX: DynamicVars = 4; -const RotY: DynamicVars = 5; -const RotZ: DynamicVars = 6; -const RotW: DynamicVars = 7; -const VelX: DynamicVars = 8; -const VelY: DynamicVars = 9; -const VelZ: DynamicVars = 10; -const AngVelX: DynamicVars = 11; -const AngVelY: DynamicVars = 12; -const AngVelZ: DynamicVars = 13; -const AccX: DynamicVars = 14; -const AccY: DynamicVars = 15; -const AccZ: DynamicVars = 16; -const AngAccX: DynamicVars = 17; -const AngAccY: DynamicVars = 18; -const AngAccZ: DynamicVars = 19; -const ForceX: DynamicVars = 20; -const ForceY: DynamicVars = 21; -const ForceZ: DynamicVars = 22; -const TorqueX: DynamicVars = 23; -const TorqueY: DynamicVars = 24; -const TorqueZ: DynamicVars = 25; -const DeltaX: DynamicVars = 26; -const DeltaY: DynamicVars = 27; -const DeltaZ: DynamicVars = 28; -const AngDeltaX: DynamicVars = 29; -const AngDeltaY: DynamicVars = 30; -const AngDeltaZ: DynamicVars = 31; +const DynIndex: DynamicVars = 0; +const DynPosX: DynamicVars = 1; +const DynPosY: DynamicVars = 2; +const DynPosZ: DynamicVars = 3; +const DynRotX: DynamicVars = 4; +const DynRotY: DynamicVars = 5; +const DynRotZ: DynamicVars = 6; +const DynRotW: DynamicVars = 7; +const DynVelX: DynamicVars = 8; +const DynVelY: DynamicVars = 9; +const DynVelZ: DynamicVars = 10; +const DynAngVelX: DynamicVars = 11; +const DynAngVelY: DynamicVars = 12; +const DynAngVelZ: DynamicVars = 13; +const DynAccX: DynamicVars = 14; +const DynAccY: DynamicVars = 15; +const DynAccZ: DynamicVars = 16; +const DynAngAccX: DynamicVars = 17; +const DynAngAccY: DynamicVars = 18; +const DynAngAccZ: DynamicVars = 19; +const DynForceX: DynamicVars = 20; +const DynForceY: DynamicVars = 21; +const DynForceZ: DynamicVars = 22; +const DynTorqueX: DynamicVars = 23; +const DynTorqueY: DynamicVars = 24; +const DynTorqueZ: DynamicVars = 25; +const DynDeltaX: DynamicVars = 26; +const DynDeltaY: DynamicVars = 27; +const DynDeltaZ: DynamicVars = 28; +const DynAngDeltaX: DynamicVars = 29; +const DynAngDeltaY: DynamicVars = 30; +const DynAngDeltaZ: DynamicVars = 31; fn DynamicIndex(idx: i32) -> i32 { - return i32(bitcast(Dynamics[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(Index))])); + return i32(bitcast(Dynamics[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(DynIndex))])); } fn DynamicPos(idx: i32) -> vec3 { - var pos: vec3; - pos.x = Dynamics[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(PosX))]; - pos.y = Dynamics[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(PosY))]; - pos.z = Dynamics[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(PosZ))];return pos; + return vec3(Dynamics[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(DynPosX))], Dynamics[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(DynPosY))], Dynamics[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(DynPosZ))]); } fn DynamicRot(idx: i32) -> vec4 { - var rot: vec4; - rot.x = Dynamics[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(RotX))]; - rot.y = Dynamics[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(RotY))]; - rot.z = Dynamics[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(RotZ))]; - rot.w = Dynamics[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(RotW))];return rot; + return vec4(Dynamics[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(DynRotX))], Dynamics[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(DynRotY))], Dynamics[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(DynRotZ))], Dynamics[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(DynRotW))]); } //////// import: "contact.go" @@ -272,6 +262,7 @@ struct PhysParams { DynamicsN: i32, JointsN: i32, Iters: i32, + Dt: f32, SoftRelax: f32, JointLinearRelax: f32, JointAngularRelax: f32, @@ -281,6 +272,9 @@ struct PhysParams { AngularDamping: f32, ContactWeighting: i32, Restitution: i32, + pad: f32, + pad1: f32, + pad2: f32, Gravity: vec4, } @@ -291,15 +285,12 @@ const Sphere: Shapes = 1; const Cylinder: Shapes = 2; const Capsule: Shapes = 3; +//////// import: "slmath-matrix3.go" + //////// import: "slmath-quaternion.go" -fn MulQuat(v: vec3, q: vec4) -> vec3 { - var ix = q.w*v.x + q.y*v.z - q.z*v.y; - var iy = q.w*v.y + q.z*v.x - q.x*v.z; - var iz = q.w*v.z + q.x*v.y - q.y*v.x; - var iw = -q.x*v.x - q.y*v.y - q.z*v.z; -return vec3(ix*q.w+iw*-q.x+iy*-q.z-iz*-q.y, - iy*q.w+iw*-q.y+iz*-q.x-ix*-q.z, - iz*q.w+iw*-q.z+ix*-q.y-iy*-q.x); +fn MulQuatVector(q: vec4, v: vec3) -> vec3 { + var xyz = vec3(q.x, q.y, q.z); + var t = MulScalar3(Cross3(xyz, v), f32(f32(2)));return v+(MulScalar3(t, q.w))+(Cross3(xyz, t)); } fn MulQuats(a: vec4,b: vec4) -> vec4 { var q: vec4; @@ -308,6 +299,14 @@ fn MulQuats(a: vec4,b: vec4) -> vec4 { q.z = a.z*b.w + a.w*b.z + a.x*b.y - a.y*b.x; q.w = a.w*b.w - a.x*b.x - a.y*b.y - a.z*b.z;return q; } +fn MulQPTransforms(aP: vec3, aQ: vec4, bP: vec3, bQ: vec4, oP: ptr>, oQ: ptr>) { + var br = MulQuatVector(aQ, bP); + *oP = br+(aP); + *oQ = MulQuats(aQ, bQ); +} +fn MulQPPoint(xP: vec3, xQ: vec4, p: vec3) -> vec3 { + var dp = MulQuatVector(xQ, p);return dp+(xP); +} //////// import: "slmath-vector3.go" fn MulScalar3(v: vec3, s: f32) -> vec3 { @@ -318,14 +317,6 @@ fn Cross3(v: vec3,o: vec3) -> vec3 { } //////// import: "step.go" -fn MulTransforms(aP: vec3, aQ: vec4, bP: vec3, bQ: vec4, oP: ptr>, oQ: ptr>) { - var br = MulQuat(bP, aQ); - *oP = br+(aP); - *oQ = MulQuats(aQ, bQ); -} -fn TransformPoint(xP: vec3, xQ: vec4, p: vec3) -> vec3 { - var dp = MulQuat(p, xQ);return dp+(xP); -} fn StepJoints(i: u32) { //gosl:kernel let pars = Params[0]; var ji = i32(i); @@ -347,15 +338,15 @@ fn StepJoints(i: u32) { //gosl:kernel if (jpi >= 0) { // can be fixed posepP = DynamicPos(jpi); posepQ = DynamicRot(jpi); - MulTransforms(posepP, posepQ, jpP, jpQ, &xwpP, &xwpQ); + MulQPTransforms(posepP, posepQ, jpP, jpQ, &xwpP, &xwpQ); comp = BodyCom(jpbi); } - var rp = xwpP-(TransformPoint(posepP, posepQ, comp)); // parent moment arm + var rp = xwpP-(MulQPPoint(posepP, posepQ, comp)); // parent moment arm var posecP = DynamicPos(jci); var posecQ = DynamicRot(jci); var xwcP = posecP; var comc = BodyCom(jcbi); - var rc = xwcP-(TransformPoint(posecP, posecQ, comc)); // child moment arm + var rc = xwcP-(MulQPPoint(posecP, posecQ, comc)); // child moment arm var jf = JointForce(ji); var jtq = JointTorque(ji); var f: vec3; @@ -370,7 +361,7 @@ fn StepJoints(i: u32) { //gosl:kernel } case Revolute, Prismatic: { var axis = JointAxis(ji); - var ap = MulQuat(axis, xwpQ); + var ap = MulQuatVector(xwpQ, axis); f = f+(MulScalar3(ap, jf.x)); } default: { diff --git a/physics/state.go b/physics/state.go index ff04d537..3b2f4ec2 100644 --- a/physics/state.go +++ b/physics/state.go @@ -40,9 +40,9 @@ func (ps *State) Defaults() { // FromRel sets state from relative values compared to a parent state func (ps *State) FromRel(rel, par *State) { ps.Quat = rel.Quat.Mul(par.Quat) - ps.Pos = rel.Pos.MulQuat(par.Quat).Add(par.Pos) - ps.LinVel = rel.LinVel.MulQuat(rel.Quat).Add(par.LinVel) - ps.AngVel = rel.AngVel.MulQuat(rel.Quat).Add(par.AngVel) + ps.Pos = par.Quat.MulVector(rel.Pos).Add(par.Pos) + ps.LinVel = rel.Quat.MulVector(rel.LinVel).Add(par.LinVel) + ps.AngVel = rel.Quat.MulVector(rel.AngVel).Add(par.AngVel) } // AngMotionMax is maximum angular motion that can be taken per update @@ -89,7 +89,7 @@ func (ps *State) Move(delta math32.Vector3) { // The axis is normalized prior to aplying the distance factor. // Sets the LinVel to motion vector. func (ps *State) MoveOnAxis(x, y, z, dist float32) { //types:add - ps.LinVel = math32.Vec3(x, y, z).Normal().MulQuat(ps.Quat).MulScalar(dist) + ps.LinVel = ps.Quat.MulVector(math32.Vec3(x, y, z).Normal()).MulScalar(dist) ps.Pos.SetAdd(ps.LinVel) } diff --git a/physics/step.go b/physics/step.go index 4427e8cd..3c31f7d5 100644 --- a/physics/step.go +++ b/physics/step.go @@ -14,19 +14,11 @@ import ( //gosl:start //gosl:import "cogentcore.org/lab/gosl/slmath" -// MulTransforms computes the equivalent of matrix multiplication for -// two quat-based transforms, o = a * b -func MulTransforms(aP math32.Vector3, aQ math32.Quat, bP math32.Vector3, bQ math32.Quat, oP *math32.Vector3, oQ *math32.Quat) { - // rotate b by a and add a - br := slmath.MulQuat(bP, aQ) - *oP = br.Add(aP) - *oQ = slmath.MulQuats(aQ, bQ) -} - -// TransformPoint applies quat-based transform to given point. -func TransformPoint(xP math32.Vector3, xQ math32.Quat, p math32.Vector3) math32.Vector3 { - dp := slmath.MulQuat(p, xQ) - return dp.Add(xP) +func OneIfNonzero(f float32) float32 { + if f != 0.0 { + return 1.0 + } + return 0.0 } // InitDynamics copies Body initial state to dynamic state. @@ -37,20 +29,46 @@ func InitDynamics(i uint32) { //gosl:kernel return } bi := DynamicIndex(ii) - Dynamics.Set(Bodies.Value(int(bi), int(BodyPosX)), int(ii), int(PosX)) - Dynamics.Set(Bodies.Value(int(bi), int(BodyPosY)), int(ii), int(PosY)) - Dynamics.Set(Bodies.Value(int(bi), int(BodyPosZ)), int(ii), int(PosZ)) + Dynamics.Set(Bodies.Value(int(bi), int(BodyPosX)), int(ii), int(DynPosX)) + Dynamics.Set(Bodies.Value(int(bi), int(BodyPosY)), int(ii), int(DynPosY)) + Dynamics.Set(Bodies.Value(int(bi), int(BodyPosZ)), int(ii), int(DynPosZ)) - Dynamics.Set(Bodies.Value(int(bi), int(BodyRotX)), int(ii), int(RotX)) - Dynamics.Set(Bodies.Value(int(bi), int(BodyRotY)), int(ii), int(RotY)) - Dynamics.Set(Bodies.Value(int(bi), int(BodyRotZ)), int(ii), int(RotZ)) - Dynamics.Set(Bodies.Value(int(bi), int(BodyRotW)), int(ii), int(RotW)) + Dynamics.Set(Bodies.Value(int(bi), int(BodyRotX)), int(ii), int(DynRotX)) + Dynamics.Set(Bodies.Value(int(bi), int(BodyRotY)), int(ii), int(DynRotY)) + Dynamics.Set(Bodies.Value(int(bi), int(BodyRotZ)), int(ii), int(DynRotZ)) + Dynamics.Set(Bodies.Value(int(bi), int(BodyRotW)), int(ii), int(DynRotW)) - for v := VelX; v < DynamicVarsN; v++ { + for v := DynVelX; v < DynamicVarsN; v++ { Dynamics.Set(0.0, int(ii), int(v)) } } +// step does the following: +// if self.compute_body_velocity_from_position_delta or self.enable_restitution: +// body_q_init = wp.clone(state_in.body_q) +// body_qd_init = wp.clone(state_in.body_qd) +// body_deltas = wp.empty_like(state_out.body_qd) +// kernel=apply_joint_forces, +// self.integrate_bodies(model, state_in, state_out, dt, self.angular_damping) +// for i in range(self.iterations): +// kernel=solve_body_joints, +// body_q, body_qd = self.apply_body_deltas(model, state_in, state_out, body_deltas, dt) +// kernel=solve_body_contact_positions, +// if self.enable_restitution and i == 0: +// # remember contact constraint weighting from the first iteration +// if self.rigid_contact_con_weighting: +// rigid_contact_inv_weight_init = wp.clone(rigid_contact_inv_weight) +// else: +// rigid_contact_inv_weight_init = None +// body_q, body_qd = self.apply_body_deltas( +// model, state_in, state_out, body_deltas, dt, rigid_contact_inv_weight +// ) +// # update body velocities from position changes +// if self.compute_body_velocity_from_position_delta and model.body_count and not requires_grad: +// kernel=update_body_velocities, +// kernel=apply_rigid_restitution, +// kernel=apply_body_delta_velocities, + // StepJoints does joint-based update. func StepJoints(i uint32) { //gosl:kernel pars := GetParams(0) @@ -78,10 +96,10 @@ func StepJoints(i uint32) { //gosl:kernel if jpi >= 0 { // can be fixed posepP = DynamicPos(jpi) posepQ = DynamicRot(jpi) - MulTransforms(posepP, posepQ, jpP, jpQ, &xwpP, &xwpQ) + slmath.MulQPTransforms(posepP, posepQ, jpP, jpQ, &xwpP, &xwpQ) comp = BodyCom(jpbi) } - rp := xwpP.Sub(TransformPoint(posepP, posepQ, comp)) // parent moment arm + rp := xwpP.Sub(slmath.MulQPPoint(posepP, posepQ, comp)) // parent moment arm // child world transform posecP := DynamicPos(jci) @@ -89,7 +107,7 @@ func StepJoints(i uint32) { //gosl:kernel xwcP := posecP // xwcQ := posecQ comc := BodyCom(jcbi) - rc := xwcP.Sub(TransformPoint(posecP, posecQ, comc)) // child moment arm + rc := xwcP.Sub(slmath.MulQPPoint(posecP, posecQ, comc)) // child moment arm // from controls: jf := JointForce(ji) @@ -105,7 +123,7 @@ func StepJoints(i uint32) { //gosl:kernel t = jtq case Revolute, Prismatic: axis := JointAxis(ji) - ap := slmath.MulQuat(axis, xwpQ) + ap := slmath.MulQuatVector(xwpQ, axis) f = f.Add(slmath.MulScalar3(ap, jf.X)) default: // todo: D6 requires more iteration! @@ -116,6 +134,64 @@ func StepJoints(i uint32) { //gosl:kernel SetJointCTorque(ji, t.Add(slmath.Cross3(rc, f))) } +// todo: aggregate forces + +// StepIntegrateBodies +func StepIntegrateBodies(i uint32) { //gosl:kernel + pars := GetParams(0) + di := int32(i) + if di >= pars.DynamicsN { + return + } + bi := DynamicIndex(di) + + invMass := Bodies.Value(int(bi), int(BodyInvMass)) + inertia := BodyInertia(bi) + invInertia := BodyInvInertia(bi) + + com := BodyCom(bi) + + // unpack transform + x0 := DynamicPos(di) + r0 := DynamicRot(di) + + // unpack spatial twist + v0 := DynamicDelta(di) + w0 := DynamicAngDelta(di) + + // unpack spatial wrench + f0 := DynamicForce(di) + t0 := DynamicTorque(di) + + xcom := slmath.MulQuatVector(r0, com).Add(x0) + + // linear part + v1 := v0.Add(f0.MulScalar(invMass).Add(pars.Gravity.V().MulScalar(OneIfNonzero(invMass))).MulScalar(pars.Dt)) + x1 := xcom.Add(v1.MulScalar(pars.Dt)) + + // angular part (compute in body frame) + wb := slmath.MulQuatVectorInverse(r0, w0) + tb := slmath.MulQuatVectorInverse(r0, t0).Sub(slmath.Cross3(wb, inertia.MulVector3(wb))) // coriolis forces + + w1 := slmath.MulQuatVector(r0, wb.Add(invInertia.MulVector3(tb).MulScalar(pars.Dt))) + r1 := slmath.MulQuats(math32.NewQuat(w1.X, w1.Y, w1.Z, 0), r0).MulScalar(0.5 * pars.Dt) + r1 = slmath.QuatNormalize(r1) + + // angular damping + w1 = w1.MulScalar(1.0 - pars.AngularDamping*pars.Dt) + + newP := x1.Sub(slmath.MulQuatVector(r1, com)) // pos + newQ := r1 // rot + + newV := v1 // delta + newW := w1 // angDelta + + // todo: write new values + + // q_new = wp.transform(x1 - wp.quat_rotate(r1, com), r1) + // qd_new = wp.spatial_vector(v1, w1) +} + //gosl:end func (wl *World) StepJoints() { diff --git a/physics/step.goal b/physics/step.goal index 8910eaf3..c8f77be7 100644 --- a/physics/step.goal +++ b/physics/step.goal @@ -12,19 +12,11 @@ import ( //gosl:start //gosl:import "cogentcore.org/lab/gosl/slmath" -// MulTransforms computes the equivalent of matrix multiplication for -// two quat-based transforms, o = a * b -func MulTransforms(aP math32.Vector3, aQ math32.Quat, bP math32.Vector3, bQ math32.Quat, oP *math32.Vector3, oQ *math32.Quat) { - // rotate b by a and add a - br := slmath.MulQuat(bP, aQ) - *oP = br.Add(aP) - *oQ = slmath.MulQuats(aQ, bQ) -} - -// TransformPoint applies quat-based transform to given point. -func TransformPoint(xP math32.Vector3, xQ math32.Quat, p math32.Vector3) math32.Vector3 { - dp := slmath.MulQuat(p, xQ) - return dp.Add(xP) +func OneIfNonzero(f float32) float32 { + if f != 0.0 { + return 1.0 + } + return 0.0 } // InitDynamics copies Body initial state to dynamic state. @@ -35,20 +27,46 @@ func InitDynamics(i uint32) { //gosl:kernel return } bi := DynamicIndex(ii) - Dynamics[ii, PosX] = Bodies[bi, BodyPosX] - Dynamics[ii, PosY] = Bodies[bi, BodyPosY] - Dynamics[ii, PosZ] = Bodies[bi, BodyPosZ] + Dynamics[ii, DynPosX] = Bodies[bi, BodyPosX] + Dynamics[ii, DynPosY] = Bodies[bi, BodyPosY] + Dynamics[ii, DynPosZ] = Bodies[bi, BodyPosZ] - Dynamics[ii, RotX] = Bodies[bi, BodyRotX] - Dynamics[ii, RotY] = Bodies[bi, BodyRotY] - Dynamics[ii, RotZ] = Bodies[bi, BodyRotZ] - Dynamics[ii, RotW] = Bodies[bi, BodyRotW] + Dynamics[ii, DynRotX] = Bodies[bi, BodyRotX] + Dynamics[ii, DynRotY] = Bodies[bi, BodyRotY] + Dynamics[ii, DynRotZ] = Bodies[bi, BodyRotZ] + Dynamics[ii, DynRotW] = Bodies[bi, BodyRotW] - for v := VelX; v < DynamicVarsN; v++ { + for v := DynVelX; v < DynamicVarsN; v++ { Dynamics[ii, v] = 0.0 } } +// step does the following: +// if self.compute_body_velocity_from_position_delta or self.enable_restitution: +// body_q_init = wp.clone(state_in.body_q) +// body_qd_init = wp.clone(state_in.body_qd) +// body_deltas = wp.empty_like(state_out.body_qd) +// kernel=apply_joint_forces, +// self.integrate_bodies(model, state_in, state_out, dt, self.angular_damping) +// for i in range(self.iterations): +// kernel=solve_body_joints, +// body_q, body_qd = self.apply_body_deltas(model, state_in, state_out, body_deltas, dt) +// kernel=solve_body_contact_positions, +// if self.enable_restitution and i == 0: +// # remember contact constraint weighting from the first iteration +// if self.rigid_contact_con_weighting: +// rigid_contact_inv_weight_init = wp.clone(rigid_contact_inv_weight) +// else: +// rigid_contact_inv_weight_init = None +// body_q, body_qd = self.apply_body_deltas( +// model, state_in, state_out, body_deltas, dt, rigid_contact_inv_weight +// ) +// # update body velocities from position changes +// if self.compute_body_velocity_from_position_delta and model.body_count and not requires_grad: +// kernel=update_body_velocities, +// kernel=apply_rigid_restitution, +// kernel=apply_body_delta_velocities, + // StepJoints does joint-based update. func StepJoints(i uint32) { //gosl:kernel pars := GetParams(0) @@ -76,10 +94,10 @@ func StepJoints(i uint32) { //gosl:kernel if jpi >= 0 { // can be fixed posepP = DynamicPos(jpi) posepQ = DynamicRot(jpi) - MulTransforms(posepP, posepQ, jpP, jpQ, &xwpP, &xwpQ) + slmath.MulQPTransforms(posepP, posepQ, jpP, jpQ, &xwpP, &xwpQ) comp = BodyCom(jpbi) } - rp := xwpP.Sub(TransformPoint(posepP, posepQ, comp)) // parent moment arm + rp := xwpP.Sub(slmath.MulQPPoint(posepP, posepQ, comp)) // parent moment arm // child world transform posecP := DynamicPos(jci) @@ -87,7 +105,7 @@ func StepJoints(i uint32) { //gosl:kernel xwcP := posecP // xwcQ := posecQ comc := BodyCom(jcbi) - rc := xwcP.Sub(TransformPoint(posecP, posecQ, comc)) // child moment arm + rc := xwcP.Sub(slmath.MulQPPoint(posecP, posecQ, comc)) // child moment arm // from controls: jf := JointForce(ji) @@ -103,7 +121,7 @@ func StepJoints(i uint32) { //gosl:kernel t = jtq case Revolute, Prismatic: axis := JointAxis(ji) - ap := slmath.MulQuat(axis, xwpQ) + ap := slmath.MulQuatVector(xwpQ, axis) f = f.Add(slmath.MulScalar3(ap, jf.X)) default: // todo: D6 requires more iteration! @@ -114,6 +132,64 @@ func StepJoints(i uint32) { //gosl:kernel SetJointCTorque(ji, t.Add(slmath.Cross3(rc, f))) } +// todo: aggregate forces + +// StepIntegrateBodies +func StepIntegrateBodies(i uint32) { //gosl:kernel + pars := GetParams(0) + di := int32(i) + if di >= pars.DynamicsN { + return + } + bi := DynamicIndex(di) + + invMass := Bodies[bi, BodyInvMass] + inertia := BodyInertia(bi) + invInertia := BodyInvInertia(bi) + + com := BodyCom(bi) + + // unpack transform + x0 := DynamicPos(di) + r0 := DynamicRot(di) + + // unpack spatial twist + v0 := DynamicDelta(di) + w0 := DynamicAngDelta(di) + + // unpack spatial wrench + f0 := DynamicForce(di) + t0 := DynamicTorque(di) + + xcom := slmath.MulQuatVector(r0, com).Add(x0) + + // linear part + v1 := v0.Add(f0.MulScalar(invMass).Add(pars.Gravity.V().MulScalar(OneIfNonzero(invMass))).MulScalar(pars.Dt)) + x1 := xcom.Add(v1.MulScalar(pars.Dt)) + + // angular part (compute in body frame) + wb := slmath.MulQuatVectorInverse(r0, w0) + tb := slmath.MulQuatVectorInverse(r0, t0).Sub(slmath.Cross3(wb, inertia.MulVector3(wb))) // coriolis forces + + w1 := slmath.MulQuatVector(r0, wb.Add(invInertia.MulVector3(tb).MulScalar(pars.Dt))) + r1 := slmath.MulQuats(math32.NewQuat(w1.X, w1.Y, w1.Z, 0), r0).MulScalar(0.5 * pars.Dt) + r1 = slmath.QuatNormalize(r1) + + // angular damping + w1 = w1.MulScalar(1.0 - pars.AngularDamping*pars.Dt) + + newP := x1.Sub(slmath.MulQuatVector(r1, com)) // pos + newQ := r1 // rot + + newV := v1 // delta + newW := w1 // angDelta + + // todo: write new values + + // q_new = wp.transform(x1 - wp.quat_rotate(r1, com), r1) + // qd_new = wp.spatial_vector(v1, w1) +} + //gosl:end func (wl *World) StepJoints() { diff --git a/physics/typegen.go b/physics/typegen.go index e0b758ac..1c7bb9cc 100644 --- a/physics/typegen.go +++ b/physics/typegen.go @@ -22,7 +22,7 @@ var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.JointVars", var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.JointTypes", IDName: "joint-types", Doc: "JointTypes are joint types that determine nature of interaction."}) -var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.PhysParams", IDName: "phys-params", Doc: "PhysParams are the physics parameters", Directives: []types.Directive{{Tool: "gosl", Directive: "start"}}, Fields: []types.Field{{Name: "DynamicsN", Doc: "DynamicsN is number of dynamics bodies."}, {Name: "JointsN", Doc: "JointsN is number of joints."}, {Name: "Iters", Doc: "Iters is the number of iterations to perform."}, {Name: "SoftRelax", Doc: "SoftRelax is soft-body relaxation constant."}, {Name: "JointLinearRelax", Doc: "JointLinearRelax is joint linear relaxation constant."}, {Name: "JointAngularRelax", Doc: "JointAngularRelax is joint angular relaxation constant."}, {Name: "JointLinearComply", Doc: "JointLinearComply is joint linear compliance constant."}, {Name: "JointAngularComply", Doc: "JointAngularComply is joint angular compliance constant."}, {Name: "ContactRelax", Doc: "ContactRelax is rigid contact relaxation constant."}, {Name: "AngularDamping", Doc: "AngularDamping is damping of angular motion."}, {Name: "ContactWeighting", Doc: "Contact weighting"}, {Name: "Restitution", Doc: "Restitution"}, {Name: "Gravity", Doc: "Gravity is the gravity acceleration function"}}}) +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.PhysParams", IDName: "phys-params", Doc: "PhysParams are the physics parameters", Directives: []types.Directive{{Tool: "gosl", Directive: "start"}}, Fields: []types.Field{{Name: "DynamicsN", Doc: "DynamicsN is number of dynamics bodies."}, {Name: "JointsN", Doc: "JointsN is number of joints."}, {Name: "Iters", Doc: "Iters is the number of iterations to perform."}, {Name: "Dt", Doc: "Dt is the integration stepsize."}, {Name: "SoftRelax", Doc: "SoftRelax is soft-body relaxation constant."}, {Name: "JointLinearRelax", Doc: "JointLinearRelax is joint linear relaxation constant."}, {Name: "JointAngularRelax", Doc: "JointAngularRelax is joint angular relaxation constant."}, {Name: "JointLinearComply", Doc: "JointLinearComply is joint linear compliance constant."}, {Name: "JointAngularComply", Doc: "JointAngularComply is joint angular compliance constant."}, {Name: "ContactRelax", Doc: "ContactRelax is rigid contact relaxation constant."}, {Name: "AngularDamping", Doc: "AngularDamping is damping of angular motion."}, {Name: "ContactWeighting", Doc: "Contact weighting"}, {Name: "Restitution", Doc: "Restitution"}, {Name: "pad"}, {Name: "pad1"}, {Name: "pad2"}, {Name: "Gravity", Doc: "Gravity is the gravity acceleration function"}}}) var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.Shapes", IDName: "shapes", Doc: "Shapes are elemental shapes for rigid bodies."}) diff --git a/physics/vars.go b/physics/vars.go index b5edbf6e..f2f5a6a1 100644 --- a/physics/vars.go +++ b/physics/vars.go @@ -6,7 +6,7 @@ package physics import "cogentcore.org/lab/tensor" -//go:generate gosl -exclude=Update,Defaults,ShouldDisplay -max-buffer-size=2147483616 +//go:generate gosl -keep -exclude=Update,Defaults,ShouldDisplay -max-buffer-size=2147483616 //gosl:start From b53fd7aeccd15747296d98492bf3238c0dd7a8bb Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Mon, 15 Dec 2025 14:39:50 +0100 Subject: [PATCH 15/97] physics: note about dynamics --- physics/step.goal | 3 +++ 1 file changed, 3 insertions(+) diff --git a/physics/step.goal b/physics/step.goal index c8f77be7..23bffde6 100644 --- a/physics/step.goal +++ b/physics/step.goal @@ -12,6 +12,9 @@ import ( //gosl:start //gosl:import "cogentcore.org/lab/gosl/slmath" +// todo: Dynamics has an extra 2 index for Cur and Next, +// switch between those. + func OneIfNonzero(f float32) float32 { if f != 0.0 { return 1.0 From eaa8f88f9ecc652f763b43d7917276d141b08333 Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Mon, 15 Dec 2025 23:27:36 +0100 Subject: [PATCH 16/97] physics: start on joint solve -- that's a lot of code.. ugh. --- gosl/slmath/quaternion.go | 51 ++- gosl/slmath/vector3.go | 48 +++ physics/body.go | 170 --------- physics/body.goal | 170 --------- physics/control.go | 107 ++++++ physics/control.goal | 84 +++-- physics/dynamics.go | 185 ++++++++++ physics/dynamics.goal | 183 ++++++++++ physics/enumgen.go | 108 +++--- physics/gosl.go | 129 ++++++- physics/joint.go | 94 +++-- physics/joint.goal | 94 +++-- physics/params.go | 8 +- physics/shaders/InitDynamics.wgsl | 148 ++++---- physics/shaders/StepJoints.wgsl | 374 ------------------- physics/step.go | 157 +------- physics/step.goal | 160 +------- physics/step_body.go | 111 ++++++ physics/step_body.goal | 109 ++++++ physics/step_joint.go | 588 ++++++++++++++++++++++++++++++ physics/step_joint.goal | 575 +++++++++++++++++++++++++++++ physics/typegen.go | 8 +- physics/vars.go | 5 +- physics/world.go | 9 +- physics/world.goal | 9 +- 25 files changed, 2413 insertions(+), 1271 deletions(-) create mode 100644 physics/control.go create mode 100644 physics/dynamics.go create mode 100644 physics/dynamics.goal delete mode 100644 physics/shaders/StepJoints.wgsl create mode 100644 physics/step_body.go create mode 100644 physics/step_body.goal create mode 100644 physics/step_joint.go create mode 100644 physics/step_joint.goal diff --git a/gosl/slmath/quaternion.go b/gosl/slmath/quaternion.go index 5f5304cc..fa12e1f6 100644 --- a/gosl/slmath/quaternion.go +++ b/gosl/slmath/quaternion.go @@ -63,8 +63,7 @@ func MulQuats(a, b math32.Quat) math32.Quat { // two quat-point spatial transforms: o = a * b func MulQPTransforms(aP math32.Vector3, aQ math32.Quat, bP math32.Vector3, bQ math32.Quat, oP *math32.Vector3, oQ *math32.Quat) { // rotate b by a and add a - br := MulQuatVector(aQ, bP) - *oP = br.Add(aP) + *oP = MulQuatVector(aQ, bP).Add(aP) *oQ = MulQuats(aQ, bQ) } @@ -74,4 +73,52 @@ func MulQPPoint(xP math32.Vector3, xQ math32.Quat, p math32.Vector3) math32.Vect return dp.Add(xP) } +func QPTransformInverse(p math32.Vector3, q math32.Quat, oP *math32.Vector3, oQ *math32.Quat) { + qi := QuatInverse(q) + *oP = Negate3(MulQuatVector(qi, p)) + *oQ = qi +} + +func QuatInverse(q math32.Quat) math32.Quat { + nq := q + nq.X *= -1 + nq.Y *= -1 + nq.Z *= -1 + return QuatNormalize(nq) +} + +func QuatToMatrix3(q math32.Quat) math32.Matrix3 { + var m math32.Matrix3 + x := q.X + y := q.Y + z := q.Z + w := q.W + x2 := x + x + y2 := y + y + z2 := z + z + xx := x * x2 + xy := x * y2 + xz := x * z2 + yy := y * y2 + yz := y * z2 + zz := z * z2 + wx := w * x2 + wy := w * y2 + wz := w * z2 + + m[0] = 1 - (yy + zz) + m[3] = xy - wz + m[6] = xz + wy + + m[1] = xy + wz + m[4] = 1 - (xx + zz) + m[7] = yz - wx + + m[2] = xz - wy + m[5] = yz + wx + m[8] = 1 - (xx + yy) + + return m +} + //gosl:end diff --git a/gosl/slmath/vector3.go b/gosl/slmath/vector3.go index 0d8f1c33..6993575b 100644 --- a/gosl/slmath/vector3.go +++ b/gosl/slmath/vector3.go @@ -24,11 +24,59 @@ func DivScalar3(v math32.Vector3, s float32) math32.Vector3 { return math32.Vec3(v.X/s, v.Y/s, v.Z/s) } +func Negate3(v math32.Vector3) math32.Vector3 { + return math32.Vec3(-v.X, -v.Y, -v.Z) +} + // Length3 returns the length (magnitude) of this vector. func Length3(v math32.Vector3) float32 { return math32.Sqrt(v.X*v.X + v.Y*v.Y + v.Z*v.Z) } +// LengthSquared3 returns the length squared of this vector. +func LengthSquared3(v math32.Vector3) float32 { + return v.X*v.X + v.Y*v.Y + v.Z*v.Z +} + +func Dot3(v, o math32.Vector3) float32 { + return v.X*o.X + v.Y*o.Y + v.Z*o.Z +} + +// Max3 returns max of this vector components vs. other vector. +func Max3(v, o math32.Vector3) math32.Vector3 { + return math32.Vec3(max(v.X, o.X), max(v.Y, o.Y), max(v.Z, o.Z)) +} + +// Min3 returns min of this vector components vs. other vector. +func Min3(v, o math32.Vector3) math32.Vector3 { + return math32.Vec3(min(v.X, o.X), min(v.Y, o.Y), min(v.Z, o.Z)) +} + +// Abs3 returns abs of this vector components. +func Abs3(v math32.Vector3) math32.Vector3 { + return math32.Vec3(math32.Abs(v.X), math32.Abs(v.Y), math32.Abs(v.Z)) +} + +func Clamp3(v, min, max math32.Vector3) math32.Vector3 { + r := v + if r.X < min.X { + r.X = min.X + } else if r.X > max.X { + r.X = max.X + } + if r.Y < min.Y { + r.Y = min.Y + } else if r.Y > max.Y { + r.Y = max.Y + } + if r.Z < min.Z { + r.Z = min.Z + } else if r.Z > max.Z { + r.Z = max.Z + } + return r +} + // Normal3 returns this vector divided by its length (its unit vector). func Normal3(v math32.Vector3) math32.Vector3 { return DivScalar3(v, Length3(v)) diff --git a/physics/body.go b/physics/body.go index 1b1ddf2f..b71b399d 100644 --- a/physics/body.go +++ b/physics/body.go @@ -146,174 +146,4 @@ func BodyInvInertia(idx int32) math32.Matrix3 { Bodies.Value(int(idx), int(BodyInvInertiaXZ)), Bodies.Value(int(idx), int(BodyInvInertiaYZ)), Bodies.Value(int(idx), int(BodyInvInertiaZZ))) } -//////// Dynamic - -// DynamicVars are dynamic body variables stored in tensor.Float32. -type DynamicVars int32 //enums:enum - -const ( - // Index of body in list of bodies. - DynIndex DynamicVars = iota - - // 3D position of center of mass. - DynPosX - DynPosY - DynPosZ - - // Quaternion rotation. - DynRotX - DynRotY - DynRotZ - DynRotW - - // Linear velocity. - DynVelX - DynVelY - DynVelZ - - // Angular velocity. - DynAngVelX - DynAngVelY - DynAngVelZ - - // Linear acceleration. - DynAccX - DynAccY - DynAccZ - - // Angular acceleration due to applied torques. - DynAngAccX - DynAngAccY - DynAngAccZ - - // Linear force driving linear acceleration (from joints, etc). - DynForceX - DynForceY - DynForceZ - - // Torque driving angular acceleration (from joints, etc). - DynTorqueX - DynTorqueY - DynTorqueZ - - // Linear deltas. - DynDeltaX - DynDeltaY - DynDeltaZ - - // Angular deltas. - DynAngDeltaX - DynAngDeltaY - DynAngDeltaZ -) - -func SetDynamicIndex(idx, bodyIdx int32) { - Dynamics.Set(math.Float32frombits(uint32(bodyIdx)), int(idx), int(DynIndex)) -} - -func DynamicIndex(idx int32) int32 { - return int32(math.Float32bits(Dynamics.Value(int(idx), int(DynIndex)))) -} - -func DynamicPos(idx int32) math32.Vector3 { - return math32.Vec3(Dynamics.Value(int(idx), int(DynPosX)), Dynamics.Value(int(idx), int(DynPosY)), Dynamics.Value(int(idx), int(DynPosZ))) -} - -func SetDynamicPos(idx int32, pos math32.Vector3) { - Dynamics.Set(pos.X, int(idx), int(DynPosX)) - Dynamics.Set(pos.Y, int(idx), int(DynPosY)) - Dynamics.Set(pos.Z, int(idx), int(DynPosZ)) -} - -func DynamicRot(idx int32) math32.Quat { - return math32.NewQuat(Dynamics.Value(int(idx), int(DynRotX)), Dynamics.Value(int(idx), int(DynRotY)), Dynamics.Value(int(idx), int(DynRotZ)), Dynamics.Value(int(idx), int(DynRotW))) -} - -func SetDynamicRot(idx int32, rot math32.Quat) { - Dynamics.Set(rot.X, int(idx), int(DynRotX)) - Dynamics.Set(rot.Y, int(idx), int(DynRotY)) - Dynamics.Set(rot.Z, int(idx), int(DynRotZ)) - Dynamics.Set(rot.W, int(idx), int(DynRotW)) -} - -func DynamicVel(idx int32) math32.Vector3 { - return math32.Vec3(Dynamics.Value(int(idx), int(DynVelX)), Dynamics.Value(int(idx), int(DynVelY)), Dynamics.Value(int(idx), int(DynVelZ))) -} - -func SetDynamicVel(idx int32, vel math32.Vector3) { - Dynamics.Set(vel.X, int(idx), int(DynVelX)) - Dynamics.Set(vel.Y, int(idx), int(DynVelY)) - Dynamics.Set(vel.Z, int(idx), int(DynVelZ)) -} - -func DynamicAcc(idx int32) math32.Vector3 { - return math32.Vec3(Dynamics.Value(int(idx), int(DynAccX)), Dynamics.Value(int(idx), int(DynAccY)), Dynamics.Value(int(idx), int(DynAccZ))) -} - -func SetDynamicAcc(idx int32, acc math32.Vector3) { - Dynamics.Set(acc.X, int(idx), int(DynAccX)) - Dynamics.Set(acc.Y, int(idx), int(DynAccY)) - Dynamics.Set(acc.Z, int(idx), int(DynAccZ)) -} - -func DynamicForce(idx int32) math32.Vector3 { - return math32.Vec3(Dynamics.Value(int(idx), int(DynForceX)), Dynamics.Value(int(idx), int(DynForceY)), Dynamics.Value(int(idx), int(DynForceZ))) -} - -func SetDynamicForce(idx int32, force math32.Vector3) { - Dynamics.Set(force.X, int(idx), int(DynForceX)) - Dynamics.Set(force.Y, int(idx), int(DynForceY)) - Dynamics.Set(force.Z, int(idx), int(DynForceZ)) -} - -func DynamicTorque(idx int32) math32.Vector3 { - return math32.Vec3(Dynamics.Value(int(idx), int(DynTorqueX)), Dynamics.Value(int(idx), int(DynTorqueY)), Dynamics.Value(int(idx), int(DynTorqueZ))) -} - -func SetDynamicTorque(idx int32, torque math32.Vector3) { - Dynamics.Set(torque.X, int(idx), int(DynTorqueX)) - Dynamics.Set(torque.Y, int(idx), int(DynTorqueY)) - Dynamics.Set(torque.Z, int(idx), int(DynTorqueZ)) -} - -func DynamicAngVel(idx int32) math32.Vector3 { - return math32.Vec3(Dynamics.Value(int(idx), int(DynAngVelX)), Dynamics.Value(int(idx), int(DynAngVelY)), Dynamics.Value(int(idx), int(DynAngVelZ))) -} - -func SetDynamicAngVel(idx int32, angVel math32.Vector3) { - Dynamics.Set(angVel.X, int(idx), int(DynAngVelX)) - Dynamics.Set(angVel.Y, int(idx), int(DynAngVelY)) - Dynamics.Set(angVel.Z, int(idx), int(DynAngVelZ)) -} - -func DynamicAngAcc(idx int32) math32.Vector3 { - return math32.Vec3(Dynamics.Value(int(idx), int(DynAngAccX)), Dynamics.Value(int(idx), int(DynAngAccY)), Dynamics.Value(int(idx), int(DynAngAccZ))) -} - -func SetDynamicAngAcc(idx int32, angAcc math32.Vector3) { - Dynamics.Set(angAcc.X, int(idx), int(DynAngAccX)) - Dynamics.Set(angAcc.Y, int(idx), int(DynAngAccY)) - Dynamics.Set(angAcc.Z, int(idx), int(DynAngAccZ)) -} - -func DynamicDelta(idx int32) math32.Vector3 { - return math32.Vec3(Dynamics.Value(int(idx), int(DynDeltaX)), Dynamics.Value(int(idx), int(DynDeltaY)), Dynamics.Value(int(idx), int(DynDeltaZ))) -} - -func SetDynamicDelta(idx int32, delta math32.Vector3) { - Dynamics.Set(delta.X, int(idx), int(DynDeltaX)) - Dynamics.Set(delta.Y, int(idx), int(DynDeltaY)) - Dynamics.Set(delta.Z, int(idx), int(DynDeltaZ)) -} - -func DynamicAngDelta(idx int32) math32.Vector3 { - return math32.Vec3(Dynamics.Value(int(idx), int(DynAngDeltaX)), Dynamics.Value(int(idx), int(DynAngDeltaY)), Dynamics.Value(int(idx), int(DynAngDeltaZ))) -} - -func SetDynamicAngDelta(idx int32, angDelta math32.Vector3) { - Dynamics.Set(angDelta.X, int(idx), int(DynAngDeltaX)) - Dynamics.Set(angDelta.Y, int(idx), int(DynAngDeltaY)) - Dynamics.Set(angDelta.Z, int(idx), int(DynAngDeltaZ)) -} - //gosl:end diff --git a/physics/body.goal b/physics/body.goal index d292a2b5..41318489 100644 --- a/physics/body.goal +++ b/physics/body.goal @@ -144,174 +144,4 @@ func BodyInvInertia(idx int32) math32.Matrix3 { Bodies[idx, BodyInvInertiaXZ], Bodies[idx, BodyInvInertiaYZ], Bodies[idx, BodyInvInertiaZZ]) } -//////// Dynamic - -// DynamicVars are dynamic body variables stored in tensor.Float32. -type DynamicVars int32 //enums:enum - -const ( - // Index of body in list of bodies. - DynIndex DynamicVars = iota - - // 3D position of center of mass. - DynPosX - DynPosY - DynPosZ - - // Quaternion rotation. - DynRotX - DynRotY - DynRotZ - DynRotW - - // Linear velocity. - DynVelX - DynVelY - DynVelZ - - // Angular velocity. - DynAngVelX - DynAngVelY - DynAngVelZ - - // Linear acceleration. - DynAccX - DynAccY - DynAccZ - - // Angular acceleration due to applied torques. - DynAngAccX - DynAngAccY - DynAngAccZ - - // Linear force driving linear acceleration (from joints, etc). - DynForceX - DynForceY - DynForceZ - - // Torque driving angular acceleration (from joints, etc). - DynTorqueX - DynTorqueY - DynTorqueZ - - // Linear deltas. - DynDeltaX - DynDeltaY - DynDeltaZ - - // Angular deltas. - DynAngDeltaX - DynAngDeltaY - DynAngDeltaZ -) - -func SetDynamicIndex(idx, bodyIdx int32) { - Dynamics[idx, DynIndex] = math.Float32frombits(uint32(bodyIdx)) -} - -func DynamicIndex(idx int32) int32 { - return int32(math.Float32bits(Dynamics[idx, DynIndex])) -} - -func DynamicPos(idx int32) math32.Vector3 { - return math32.Vec3(Dynamics[idx, DynPosX], Dynamics[idx, DynPosY], Dynamics[idx, DynPosZ]) -} - -func SetDynamicPos(idx int32, pos math32.Vector3) { - Dynamics[idx, DynPosX] = pos.X - Dynamics[idx, DynPosY] = pos.Y - Dynamics[idx, DynPosZ] = pos.Z -} - -func DynamicRot(idx int32) math32.Quat { - return math32.NewQuat(Dynamics[idx, DynRotX], Dynamics[idx, DynRotY], Dynamics[idx, DynRotZ], Dynamics[idx, DynRotW]) -} - -func SetDynamicRot(idx int32, rot math32.Quat) { - Dynamics[idx, DynRotX] = rot.X - Dynamics[idx, DynRotY] = rot.Y - Dynamics[idx, DynRotZ] = rot.Z - Dynamics[idx, DynRotW] = rot.W -} - -func DynamicVel(idx int32) math32.Vector3 { - return math32.Vec3(Dynamics[idx, DynVelX], Dynamics[idx, DynVelY], Dynamics[idx, DynVelZ]) -} - -func SetDynamicVel(idx int32, vel math32.Vector3) { - Dynamics[idx, DynVelX] = vel.X - Dynamics[idx, DynVelY] = vel.Y - Dynamics[idx, DynVelZ] = vel.Z -} - -func DynamicAcc(idx int32) math32.Vector3 { - return math32.Vec3(Dynamics[idx, DynAccX], Dynamics[idx, DynAccY], Dynamics[idx, DynAccZ]) -} - -func SetDynamicAcc(idx int32, acc math32.Vector3) { - Dynamics[idx, DynAccX] = acc.X - Dynamics[idx, DynAccY] = acc.Y - Dynamics[idx, DynAccZ] = acc.Z -} - -func DynamicForce(idx int32) math32.Vector3 { - return math32.Vec3(Dynamics[idx, DynForceX], Dynamics[idx, DynForceY], Dynamics[idx, DynForceZ]) -} - -func SetDynamicForce(idx int32, force math32.Vector3) { - Dynamics[idx, DynForceX] = force.X - Dynamics[idx, DynForceY] = force.Y - Dynamics[idx, DynForceZ] = force.Z -} - -func DynamicTorque(idx int32) math32.Vector3 { - return math32.Vec3(Dynamics[idx, DynTorqueX], Dynamics[idx, DynTorqueY], Dynamics[idx, DynTorqueZ]) -} - -func SetDynamicTorque(idx int32, torque math32.Vector3) { - Dynamics[idx, DynTorqueX] = torque.X - Dynamics[idx, DynTorqueY] = torque.Y - Dynamics[idx, DynTorqueZ] = torque.Z -} - -func DynamicAngVel(idx int32) math32.Vector3 { - return math32.Vec3(Dynamics[idx, DynAngVelX], Dynamics[idx, DynAngVelY], Dynamics[idx, DynAngVelZ]) -} - -func SetDynamicAngVel(idx int32, angVel math32.Vector3) { - Dynamics[idx, DynAngVelX] = angVel.X - Dynamics[idx, DynAngVelY] = angVel.Y - Dynamics[idx, DynAngVelZ] = angVel.Z -} - -func DynamicAngAcc(idx int32) math32.Vector3 { - return math32.Vec3(Dynamics[idx, DynAngAccX], Dynamics[idx, DynAngAccY], Dynamics[idx, DynAngAccZ]) -} - -func SetDynamicAngAcc(idx int32, angAcc math32.Vector3) { - Dynamics[idx, DynAngAccX] = angAcc.X - Dynamics[idx, DynAngAccY] = angAcc.Y - Dynamics[idx, DynAngAccZ] = angAcc.Z -} - -func DynamicDelta(idx int32) math32.Vector3 { - return math32.Vec3(Dynamics[idx, DynDeltaX], Dynamics[idx, DynDeltaY], Dynamics[idx, DynDeltaZ]) -} - -func SetDynamicDelta(idx int32, delta math32.Vector3) { - Dynamics[idx, DynDeltaX] = delta.X - Dynamics[idx, DynDeltaY] = delta.Y - Dynamics[idx, DynDeltaZ] = delta.Z -} - -func DynamicAngDelta(idx int32) math32.Vector3 { - return math32.Vec3(Dynamics[idx, DynAngDeltaX], Dynamics[idx, DynAngDeltaY], Dynamics[idx, DynAngDeltaZ]) -} - -func SetDynamicAngDelta(idx int32, angDelta math32.Vector3) { - Dynamics[idx, DynAngDeltaX] = angDelta.X - Dynamics[idx, DynAngDeltaY] = angDelta.Y - Dynamics[idx, DynAngDeltaZ] = angDelta.Z -} - //gosl:end diff --git a/physics/control.go b/physics/control.go new file mode 100644 index 00000000..cfb24612 --- /dev/null +++ b/physics/control.go @@ -0,0 +1,107 @@ +// Code generated by "goal build"; DO NOT EDIT. +//line control.goal:1 +// Copyright (c) 2025, Cogent Core. All rights reserved. +// Use of this source code is governed by a BSD-style +// license that can be found in the LICENSE file. + +package physics + +import "cogentcore.org/core/math32" + +//gosl:start + +// JointControlVars are external joint control input variables stored in tensor.Float32. +// These must be in one-to-one correspondence with the joints. +type JointControlVars int32 //enums:enum + +const ( + // Joint force and torque inputs + JointCtrlForceX JointControlVars = iota + JointCtrlForceY + JointCtrlForceZ + JointCtrlTorqueX + JointCtrlTorqueY + JointCtrlTorqueZ + + // target values (1 DoF use JointTargetPosX) + JointTargetPosX + JointTargetPosY + JointTargetPosZ + + JointTargetRotX + JointTargetRotY + JointTargetRotZ + JointTargetRotW + + // target velocity + JointTargetVelX + JointTargetVelY + JointTargetVelZ + + // target angular velocity + JointTargetAngVelX + JointTargetAngVelY + JointTargetAngVelZ +) + +func JointControlForce(idx int32) math32.Vector3 { + return math32.Vec3(JointControls.Value(int(idx), int(JointCtrlForceX)), JointControls.Value(int(idx), int(JointCtrlForceY)), JointControls.Value(int(idx), int(JointCtrlForceZ))) +} + +func SetJointControlForce(idx int32, f math32.Vector3) { + JointControls.Set(f.X, int(idx), int(JointCtrlForceX)) + JointControls.Set(f.Y, int(idx), int(JointCtrlForceY)) + JointControls.Set(f.Z, int(idx), int(JointCtrlForceZ)) +} + +func JointControlTorque(idx int32) math32.Vector3 { + return math32.Vec3(JointControls.Value(int(idx), int(JointCtrlTorqueX)), JointControls.Value(int(idx), int(JointCtrlTorqueY)), JointControls.Value(int(idx), int(JointCtrlTorqueZ))) +} + +func SetJointControlTorque(idx int32, f math32.Vector3) { + JointControls.Set(f.X, int(idx), int(JointCtrlTorqueX)) + JointControls.Set(f.Y, int(idx), int(JointCtrlTorqueY)) + JointControls.Set(f.Z, int(idx), int(JointCtrlTorqueZ)) +} + +func JointTargetPos(idx int32) math32.Vector3 { + return math32.Vec3(JointControls.Value(int(idx), int(JointTargetPosX)), JointControls.Value(int(idx), int(JointTargetPosY)), JointControls.Value(int(idx), int(JointTargetPosZ))) +} + +func SetJointTargetPos(idx int32, f math32.Vector3) { + JointControls.Set(f.X, int(idx), int(JointTargetPosX)) + JointControls.Set(f.Y, int(idx), int(JointTargetPosY)) + JointControls.Set(f.Z, int(idx), int(JointTargetPosZ)) +} + +func JointTargetRot(idx int32) math32.Vector3 { + return math32.Vec3(JointControls.Value(int(idx), int(JointTargetRotX)), JointControls.Value(int(idx), int(JointTargetRotY)), JointControls.Value(int(idx), int(JointTargetRotZ))) +} + +func SetJointTargetRot(idx int32, f math32.Vector3) { + JointControls.Set(f.X, int(idx), int(JointTargetRotX)) + JointControls.Set(f.Y, int(idx), int(JointTargetRotY)) + JointControls.Set(f.Z, int(idx), int(JointTargetRotZ)) +} + +func JointTargetVel(idx int32) math32.Vector3 { + return math32.Vec3(JointControls.Value(int(idx), int(JointTargetVelX)), JointControls.Value(int(idx), int(JointTargetVelY)), JointControls.Value(int(idx), int(JointTargetVelZ))) +} + +func SetJointTargetVel(idx int32, f math32.Vector3) { + JointControls.Set(f.X, int(idx), int(JointTargetVelX)) + JointControls.Set(f.Y, int(idx), int(JointTargetVelY)) + JointControls.Set(f.Z, int(idx), int(JointTargetVelZ)) +} + +func JointTargetAngVel(idx int32) math32.Vector3 { + return math32.Vec3(JointControls.Value(int(idx), int(JointTargetAngVelX)), JointControls.Value(int(idx), int(JointTargetAngVelY)), JointControls.Value(int(idx), int(JointTargetAngVelZ))) +} + +func SetJointTargetAngVel(idx int32, f math32.Vector3) { + JointControls.Set(f.X, int(idx), int(JointTargetAngVelX)) + JointControls.Set(f.Y, int(idx), int(JointTargetAngVelY)) + JointControls.Set(f.Z, int(idx), int(JointTargetAngVelZ)) +} + +//gosl:end diff --git a/physics/control.goal b/physics/control.goal index 688d3439..b29dec56 100644 --- a/physics/control.goal +++ b/physics/control.goal @@ -14,12 +14,12 @@ type JointControlVars int32 //enums:enum const ( // Joint force and torque inputs - JointForceX JointControlVars = iota - JointForceY - JointForceZ - JointTorqueX - JointTorqueY - JointTorqueZ + JointCtrlForceX JointControlVars = iota + JointCtrlForceY + JointCtrlForceZ + JointCtrlTorqueX + JointCtrlTorqueY + JointCtrlTorqueZ // target values (1 DoF use JointTargetPosX) JointTargetPosX @@ -42,32 +42,64 @@ const ( JointTargetAngVelZ ) -func JointForce(idx int32) math32.Vector3 { - var f math32.Vector3 - f.X = Joints[idx, JointForceX] - f.Y = Joints[idx, JointForceY] - f.Z = Joints[idx, JointForceZ] - return f +func JointControlForce(idx int32) math32.Vector3 { + return math32.Vec3(JointControls[idx, JointCtrlForceX], JointControls[idx, JointCtrlForceY], JointControls[idx, JointCtrlForceZ]) } -func SetJointForce(idx int32, f math32.Vector3) { - Joints[idx, JointForceX] = f.X - Joints[idx, JointForceY] = f.Y - Joints[idx, JointForceZ] = f.Z +func SetJointControlForce(idx int32, f math32.Vector3) { + JointControls[idx, JointCtrlForceX] = f.X + JointControls[idx, JointCtrlForceY] = f.Y + JointControls[idx, JointCtrlForceZ] = f.Z } -func JointTorque(idx int32) math32.Vector3 { - var f math32.Vector3 - f.X = Joints[idx, JointTorqueX] - f.Y = Joints[idx, JointTorqueY] - f.Z = Joints[idx, JointTorqueZ] - return f +func JointControlTorque(idx int32) math32.Vector3 { + return math32.Vec3(JointControls[idx, JointCtrlTorqueX], JointControls[idx, JointCtrlTorqueY], JointControls[idx, JointCtrlTorqueZ]) } -func SetJointTorque(idx int32, f math32.Vector3) { - Joints[idx, JointTorqueX] = f.X - Joints[idx, JointTorqueY] = f.Y - Joints[idx, JointTorqueZ] = f.Z +func SetJointControlTorque(idx int32, f math32.Vector3) { + JointControls[idx, JointCtrlTorqueX] = f.X + JointControls[idx, JointCtrlTorqueY] = f.Y + JointControls[idx, JointCtrlTorqueZ] = f.Z +} + +func JointTargetPos(idx int32) math32.Vector3 { + return math32.Vec3(JointControls[idx, JointTargetPosX], JointControls[idx, JointTargetPosY], JointControls[idx, JointTargetPosZ]) +} + +func SetJointTargetPos(idx int32, f math32.Vector3) { + JointControls[idx, JointTargetPosX] = f.X + JointControls[idx, JointTargetPosY] = f.Y + JointControls[idx, JointTargetPosZ] = f.Z +} + +func JointTargetRot(idx int32) math32.Vector3 { + return math32.Vec3(JointControls[idx, JointTargetRotX], JointControls[idx, JointTargetRotY], JointControls[idx, JointTargetRotZ]) +} + +func SetJointTargetRot(idx int32, f math32.Vector3) { + JointControls[idx, JointTargetRotX] = f.X + JointControls[idx, JointTargetRotY] = f.Y + JointControls[idx, JointTargetRotZ] = f.Z +} + +func JointTargetVel(idx int32) math32.Vector3 { + return math32.Vec3(JointControls[idx, JointTargetVelX], JointControls[idx, JointTargetVelY], JointControls[idx, JointTargetVelZ]) +} + +func SetJointTargetVel(idx int32, f math32.Vector3) { + JointControls[idx, JointTargetVelX] = f.X + JointControls[idx, JointTargetVelY] = f.Y + JointControls[idx, JointTargetVelZ] = f.Z +} + +func JointTargetAngVel(idx int32) math32.Vector3 { + return math32.Vec3(JointControls[idx, JointTargetAngVelX], JointControls[idx, JointTargetAngVelY], JointControls[idx, JointTargetAngVelZ]) +} + +func SetJointTargetAngVel(idx int32, f math32.Vector3) { + JointControls[idx, JointTargetAngVelX] = f.X + JointControls[idx, JointTargetAngVelY] = f.Y + JointControls[idx, JointTargetAngVelZ] = f.Z } //gosl:end diff --git a/physics/dynamics.go b/physics/dynamics.go new file mode 100644 index 00000000..f45cb2fa --- /dev/null +++ b/physics/dynamics.go @@ -0,0 +1,185 @@ +// Code generated by "goal build"; DO NOT EDIT. +//line dynamics.goal:1 +// Copyright (c) 2025, Cogent Core. All rights reserved. +// Use of this source code is governed by a BSD-style +// license that can be found in the LICENSE file. + +package physics + +import ( + "math" + + "cogentcore.org/core/math32" +) + +//gosl:start + +// DynamicVars are dynamic body variables stored in tensor.Float32. +type DynamicVars int32 //enums:enum + +const ( + // Index of body in list of bodies. + DynIndex DynamicVars = iota + + // 3D position of center of mass. + DynPosX + DynPosY + DynPosZ + + // Quaternion rotation. + DynRotX + DynRotY + DynRotZ + DynRotW + + // Linear velocity. + DynVelX + DynVelY + DynVelZ + + // Angular velocity. + DynAngVelX + DynAngVelY + DynAngVelZ + + // Linear acceleration. + DynAccX + DynAccY + DynAccZ + + // Angular acceleration due to applied torques. + DynAngAccX + DynAngAccY + DynAngAccZ + + // Linear force driving linear acceleration (from joints, etc). + DynForceX + DynForceY + DynForceZ + + // Torque driving angular acceleration (from joints, etc). + DynTorqueX + DynTorqueY + DynTorqueZ + + // Linear deltas. + DynDeltaX + DynDeltaY + DynDeltaZ + + // Angular deltas. + DynAngDeltaX + DynAngDeltaY + DynAngDeltaZ +) + +func SetDynamicIndex(idx, cni, bodyIdx int32) { + Dynamics.Set(math.Float32frombits(uint32(bodyIdx)), int(idx), int(cni), int(DynIndex)) +} + +func DynamicIndex(idx, cni int32) int32 { + return int32(math.Float32bits(Dynamics.Value(int(idx), int(cni), int(DynIndex)))) +} + +func DynamicPos(idx, cni int32) math32.Vector3 { + return math32.Vec3(Dynamics.Value(int(idx), int(cni), int(DynPosX)), Dynamics.Value(int(idx), int(cni), int(DynPosY)), Dynamics.Value(int(idx), int(cni), int(DynPosZ))) +} + +func SetDynamicPos(idx, cni int32, pos math32.Vector3) { + Dynamics.Set(pos.X, int(idx), int(cni), int(DynPosX)) + Dynamics.Set(pos.Y, int(idx), int(cni), int(DynPosY)) + Dynamics.Set(pos.Z, int(idx), int(cni), int(DynPosZ)) +} + +func DynamicRot(idx, cni int32) math32.Quat { + return math32.NewQuat(Dynamics.Value(int(idx), int(cni), int(DynRotX)), Dynamics.Value(int(idx), int(cni), int(DynRotY)), Dynamics.Value(int(idx), int(cni), int(DynRotZ)), Dynamics.Value(int(idx), int(cni), int(DynRotW))) +} + +func SetDynamicRot(idx, cni int32, rot math32.Quat) { + Dynamics.Set(rot.X, int(idx), int(cni), int(DynRotX)) + Dynamics.Set(rot.Y, int(idx), int(cni), int(DynRotY)) + Dynamics.Set(rot.Z, int(idx), int(cni), int(DynRotZ)) + Dynamics.Set(rot.W, int(idx), int(cni), int(DynRotW)) +} + +func DynamicVel(idx, cni int32) math32.Vector3 { + return math32.Vec3(Dynamics.Value(int(idx), int(cni), int(DynVelX)), Dynamics.Value(int(idx), int(cni), int(DynVelY)), Dynamics.Value(int(idx), int(cni), int(DynVelZ))) +} + +func SetDynamicVel(idx, cni int32, vel math32.Vector3) { + Dynamics.Set(vel.X, int(idx), int(cni), int(DynVelX)) + Dynamics.Set(vel.Y, int(idx), int(cni), int(DynVelY)) + Dynamics.Set(vel.Z, int(idx), int(cni), int(DynVelZ)) +} + +func DynamicAcc(idx, cni int32) math32.Vector3 { + return math32.Vec3(Dynamics.Value(int(idx), int(cni), int(DynAccX)), Dynamics.Value(int(idx), int(cni), int(DynAccY)), Dynamics.Value(int(idx), int(cni), int(DynAccZ))) +} + +func SetDynamicAcc(idx, cni int32, acc math32.Vector3) { + Dynamics.Set(acc.X, int(idx), int(cni), int(DynAccX)) + Dynamics.Set(acc.Y, int(idx), int(cni), int(DynAccY)) + Dynamics.Set(acc.Z, int(idx), int(cni), int(DynAccZ)) +} + +func DynamicForce(idx, cni int32) math32.Vector3 { + return math32.Vec3(Dynamics.Value(int(idx), int(cni), int(DynForceX)), Dynamics.Value(int(idx), int(cni), int(DynForceY)), Dynamics.Value(int(idx), int(cni), int(DynForceZ))) +} + +func SetDynamicForce(idx, cni int32, force math32.Vector3) { + Dynamics.Set(force.X, int(idx), int(cni), int(DynForceX)) + Dynamics.Set(force.Y, int(idx), int(cni), int(DynForceY)) + Dynamics.Set(force.Z, int(idx), int(cni), int(DynForceZ)) +} + +func DynamicTorque(idx, cni int32) math32.Vector3 { + return math32.Vec3(Dynamics.Value(int(idx), int(cni), int(DynTorqueX)), Dynamics.Value(int(idx), int(cni), int(DynTorqueY)), Dynamics.Value(int(idx), int(cni), int(DynTorqueZ))) +} + +func SetDynamicTorque(idx, cni int32, torque math32.Vector3) { + Dynamics.Set(torque.X, int(idx), int(cni), int(DynTorqueX)) + Dynamics.Set(torque.Y, int(idx), int(cni), int(DynTorqueY)) + Dynamics.Set(torque.Z, int(idx), int(cni), int(DynTorqueZ)) +} + +func DynamicAngVel(idx, cni int32) math32.Vector3 { + return math32.Vec3(Dynamics.Value(int(idx), int(cni), int(DynAngVelX)), Dynamics.Value(int(idx), int(cni), int(DynAngVelY)), Dynamics.Value(int(idx), int(cni), int(DynAngVelZ))) +} + +func SetDynamicAngVel(idx, cni int32, angVel math32.Vector3) { + Dynamics.Set(angVel.X, int(idx), int(cni), int(DynAngVelX)) + Dynamics.Set(angVel.Y, int(idx), int(cni), int(DynAngVelY)) + Dynamics.Set(angVel.Z, int(idx), int(cni), int(DynAngVelZ)) +} + +func DynamicAngAcc(idx, cni int32) math32.Vector3 { + return math32.Vec3(Dynamics.Value(int(idx), int(cni), int(DynAngAccX)), Dynamics.Value(int(idx), int(cni), int(DynAngAccY)), Dynamics.Value(int(idx), int(cni), int(DynAngAccZ))) +} + +func SetDynamicAngAcc(idx, cni int32, angAcc math32.Vector3) { + Dynamics.Set(angAcc.X, int(idx), int(cni), int(DynAngAccX)) + Dynamics.Set(angAcc.Y, int(idx), int(cni), int(DynAngAccY)) + Dynamics.Set(angAcc.Z, int(idx), int(cni), int(DynAngAccZ)) +} + +func DynamicDelta(idx, cni int32) math32.Vector3 { + return math32.Vec3(Dynamics.Value(int(idx), int(cni), int(DynDeltaX)), Dynamics.Value(int(idx), int(cni), int(DynDeltaY)), Dynamics.Value(int(idx), int(cni), int(DynDeltaZ))) +} + +func SetDynamicDelta(idx, cni int32, delta math32.Vector3) { + Dynamics.Set(delta.X, int(idx), int(cni), int(DynDeltaX)) + Dynamics.Set(delta.Y, int(idx), int(cni), int(DynDeltaY)) + Dynamics.Set(delta.Z, int(idx), int(cni), int(DynDeltaZ)) +} + +func DynamicAngDelta(idx, cni int32) math32.Vector3 { + return math32.Vec3(Dynamics.Value(int(idx), int(cni), int(DynAngDeltaX)), Dynamics.Value(int(idx), int(cni), int(DynAngDeltaY)), Dynamics.Value(int(idx), int(cni), int(DynAngDeltaZ))) +} + +func SetDynamicAngDelta(idx, cni int32, angDelta math32.Vector3) { + Dynamics.Set(angDelta.X, int(idx), int(cni), int(DynAngDeltaX)) + Dynamics.Set(angDelta.Y, int(idx), int(cni), int(DynAngDeltaY)) + Dynamics.Set(angDelta.Z, int(idx), int(cni), int(DynAngDeltaZ)) +} + +//gosl:end diff --git a/physics/dynamics.goal b/physics/dynamics.goal new file mode 100644 index 00000000..dc2a7e7d --- /dev/null +++ b/physics/dynamics.goal @@ -0,0 +1,183 @@ +// Copyright (c) 2025, Cogent Core. All rights reserved. +// Use of this source code is governed by a BSD-style +// license that can be found in the LICENSE file. + +package physics + +import ( + "math" + + "cogentcore.org/core/math32" +) + +//gosl:start + +// DynamicVars are dynamic body variables stored in tensor.Float32. +type DynamicVars int32 //enums:enum + +const ( + // Index of body in list of bodies. + DynIndex DynamicVars = iota + + // 3D position of center of mass. + DynPosX + DynPosY + DynPosZ + + // Quaternion rotation. + DynRotX + DynRotY + DynRotZ + DynRotW + + // Linear velocity. + DynVelX + DynVelY + DynVelZ + + // Angular velocity. + DynAngVelX + DynAngVelY + DynAngVelZ + + // Linear acceleration. + DynAccX + DynAccY + DynAccZ + + // Angular acceleration due to applied torques. + DynAngAccX + DynAngAccY + DynAngAccZ + + // Linear force driving linear acceleration (from joints, etc). + DynForceX + DynForceY + DynForceZ + + // Torque driving angular acceleration (from joints, etc). + DynTorqueX + DynTorqueY + DynTorqueZ + + // Linear deltas. + DynDeltaX + DynDeltaY + DynDeltaZ + + // Angular deltas. + DynAngDeltaX + DynAngDeltaY + DynAngDeltaZ +) + +func SetDynamicIndex(idx, cni, bodyIdx int32) { + Dynamics[idx, cni, DynIndex] = math.Float32frombits(uint32(bodyIdx)) +} + +func DynamicIndex(idx, cni int32) int32 { + return int32(math.Float32bits(Dynamics[idx, cni, DynIndex])) +} + +func DynamicPos(idx, cni int32) math32.Vector3 { + return math32.Vec3(Dynamics[idx, cni, DynPosX], Dynamics[idx, cni, DynPosY], Dynamics[idx, cni, DynPosZ]) +} + +func SetDynamicPos(idx, cni int32, pos math32.Vector3) { + Dynamics[idx, cni, DynPosX] = pos.X + Dynamics[idx, cni, DynPosY] = pos.Y + Dynamics[idx, cni, DynPosZ] = pos.Z +} + +func DynamicRot(idx, cni int32) math32.Quat { + return math32.NewQuat(Dynamics[idx, cni, DynRotX], Dynamics[idx, cni, DynRotY], Dynamics[idx, cni, DynRotZ], Dynamics[idx, cni, DynRotW]) +} + +func SetDynamicRot(idx, cni int32, rot math32.Quat) { + Dynamics[idx, cni, DynRotX] = rot.X + Dynamics[idx, cni, DynRotY] = rot.Y + Dynamics[idx, cni, DynRotZ] = rot.Z + Dynamics[idx, cni, DynRotW] = rot.W +} + +func DynamicVel(idx, cni int32) math32.Vector3 { + return math32.Vec3(Dynamics[idx, cni, DynVelX], Dynamics[idx, cni, DynVelY], Dynamics[idx, cni, DynVelZ]) +} + +func SetDynamicVel(idx, cni int32, vel math32.Vector3) { + Dynamics[idx, cni, DynVelX] = vel.X + Dynamics[idx, cni, DynVelY] = vel.Y + Dynamics[idx, cni, DynVelZ] = vel.Z +} + +func DynamicAcc(idx, cni int32) math32.Vector3 { + return math32.Vec3(Dynamics[idx, cni, DynAccX], Dynamics[idx, cni, DynAccY], Dynamics[idx, cni, DynAccZ]) +} + +func SetDynamicAcc(idx, cni int32, acc math32.Vector3) { + Dynamics[idx, cni, DynAccX] = acc.X + Dynamics[idx, cni, DynAccY] = acc.Y + Dynamics[idx, cni, DynAccZ] = acc.Z +} + +func DynamicForce(idx, cni int32) math32.Vector3 { + return math32.Vec3(Dynamics[idx, cni, DynForceX], Dynamics[idx, cni, DynForceY], Dynamics[idx, cni, DynForceZ]) +} + +func SetDynamicForce(idx, cni int32, force math32.Vector3) { + Dynamics[idx, cni, DynForceX] = force.X + Dynamics[idx, cni, DynForceY] = force.Y + Dynamics[idx, cni, DynForceZ] = force.Z +} + +func DynamicTorque(idx, cni int32) math32.Vector3 { + return math32.Vec3(Dynamics[idx, cni, DynTorqueX], Dynamics[idx, cni, DynTorqueY], Dynamics[idx, cni, DynTorqueZ]) +} + +func SetDynamicTorque(idx, cni int32, torque math32.Vector3) { + Dynamics[idx, cni, DynTorqueX] = torque.X + Dynamics[idx, cni, DynTorqueY] = torque.Y + Dynamics[idx, cni, DynTorqueZ] = torque.Z +} + +func DynamicAngVel(idx, cni int32) math32.Vector3 { + return math32.Vec3(Dynamics[idx, cni, DynAngVelX], Dynamics[idx, cni, DynAngVelY], Dynamics[idx, cni, DynAngVelZ]) +} + +func SetDynamicAngVel(idx, cni int32, angVel math32.Vector3) { + Dynamics[idx, cni, DynAngVelX] = angVel.X + Dynamics[idx, cni, DynAngVelY] = angVel.Y + Dynamics[idx, cni, DynAngVelZ] = angVel.Z +} + +func DynamicAngAcc(idx, cni int32) math32.Vector3 { + return math32.Vec3(Dynamics[idx, cni, DynAngAccX], Dynamics[idx, cni, DynAngAccY], Dynamics[idx, cni, DynAngAccZ]) +} + +func SetDynamicAngAcc(idx, cni int32, angAcc math32.Vector3) { + Dynamics[idx, cni, DynAngAccX] = angAcc.X + Dynamics[idx, cni, DynAngAccY] = angAcc.Y + Dynamics[idx, cni, DynAngAccZ] = angAcc.Z +} + +func DynamicDelta(idx, cni int32) math32.Vector3 { + return math32.Vec3(Dynamics[idx, cni, DynDeltaX], Dynamics[idx, cni, DynDeltaY], Dynamics[idx, cni, DynDeltaZ]) +} + +func SetDynamicDelta(idx, cni int32, delta math32.Vector3) { + Dynamics[idx, cni, DynDeltaX] = delta.X + Dynamics[idx, cni, DynDeltaY] = delta.Y + Dynamics[idx, cni, DynDeltaZ] = delta.Z +} + +func DynamicAngDelta(idx, cni int32) math32.Vector3 { + return math32.Vec3(Dynamics[idx, cni, DynAngDeltaX], Dynamics[idx, cni, DynAngDeltaY], Dynamics[idx, cni, DynAngDeltaZ]) +} + +func SetDynamicAngDelta(idx, cni int32, angDelta math32.Vector3) { + Dynamics[idx, cni, DynAngDeltaX] = angDelta.X + Dynamics[idx, cni, DynAngDeltaY] = angDelta.Y + Dynamics[idx, cni, DynAngDeltaZ] = angDelta.Z +} + +//gosl:end diff --git a/physics/enumgen.go b/physics/enumgen.go index b309ba97..9ea7de78 100644 --- a/physics/enumgen.go +++ b/physics/enumgen.go @@ -51,53 +51,6 @@ func (i BodyVars) MarshalText() ([]byte, error) { return []byte(i.String()), nil // UnmarshalText implements the [encoding.TextUnmarshaler] interface. func (i *BodyVars) UnmarshalText(text []byte) error { return enums.UnmarshalText(i, text, "BodyVars") } -var _DynamicVarsValues = []DynamicVars{0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31} - -// DynamicVarsN is the highest valid value for type DynamicVars, plus one. -// -//gosl:start -const DynamicVarsN DynamicVars = 32 - -//gosl:end - -var _DynamicVarsValueMap = map[string]DynamicVars{`DynIndex`: 0, `DynPosX`: 1, `DynPosY`: 2, `DynPosZ`: 3, `DynRotX`: 4, `DynRotY`: 5, `DynRotZ`: 6, `DynRotW`: 7, `DynVelX`: 8, `DynVelY`: 9, `DynVelZ`: 10, `DynAngVelX`: 11, `DynAngVelY`: 12, `DynAngVelZ`: 13, `DynAccX`: 14, `DynAccY`: 15, `DynAccZ`: 16, `DynAngAccX`: 17, `DynAngAccY`: 18, `DynAngAccZ`: 19, `DynForceX`: 20, `DynForceY`: 21, `DynForceZ`: 22, `DynTorqueX`: 23, `DynTorqueY`: 24, `DynTorqueZ`: 25, `DynDeltaX`: 26, `DynDeltaY`: 27, `DynDeltaZ`: 28, `DynAngDeltaX`: 29, `DynAngDeltaY`: 30, `DynAngDeltaZ`: 31} - -var _DynamicVarsDescMap = map[DynamicVars]string{0: `Index of body in list of bodies.`, 1: `3D position of center of mass.`, 2: ``, 3: ``, 4: `Quaternion rotation.`, 5: ``, 6: ``, 7: ``, 8: `Linear velocity.`, 9: ``, 10: ``, 11: `Angular velocity.`, 12: ``, 13: ``, 14: `Linear acceleration.`, 15: ``, 16: ``, 17: `Angular acceleration due to applied torques.`, 18: ``, 19: ``, 20: `Linear force driving linear acceleration (from joints, etc).`, 21: ``, 22: ``, 23: `Torque driving angular acceleration (from joints, etc).`, 24: ``, 25: ``, 26: `Linear deltas.`, 27: ``, 28: ``, 29: `Angular deltas.`, 30: ``, 31: ``} - -var _DynamicVarsMap = map[DynamicVars]string{0: `DynIndex`, 1: `DynPosX`, 2: `DynPosY`, 3: `DynPosZ`, 4: `DynRotX`, 5: `DynRotY`, 6: `DynRotZ`, 7: `DynRotW`, 8: `DynVelX`, 9: `DynVelY`, 10: `DynVelZ`, 11: `DynAngVelX`, 12: `DynAngVelY`, 13: `DynAngVelZ`, 14: `DynAccX`, 15: `DynAccY`, 16: `DynAccZ`, 17: `DynAngAccX`, 18: `DynAngAccY`, 19: `DynAngAccZ`, 20: `DynForceX`, 21: `DynForceY`, 22: `DynForceZ`, 23: `DynTorqueX`, 24: `DynTorqueY`, 25: `DynTorqueZ`, 26: `DynDeltaX`, 27: `DynDeltaY`, 28: `DynDeltaZ`, 29: `DynAngDeltaX`, 30: `DynAngDeltaY`, 31: `DynAngDeltaZ`} - -// String returns the string representation of this DynamicVars value. -func (i DynamicVars) String() string { return enums.String(i, _DynamicVarsMap) } - -// SetString sets the DynamicVars value from its string representation, -// and returns an error if the string is invalid. -func (i *DynamicVars) SetString(s string) error { - return enums.SetString(i, s, _DynamicVarsValueMap, "DynamicVars") -} - -// Int64 returns the DynamicVars value as an int64. -func (i DynamicVars) Int64() int64 { return int64(i) } - -// SetInt64 sets the DynamicVars value from an int64. -func (i *DynamicVars) SetInt64(in int64) { *i = DynamicVars(in) } - -// Desc returns the description of the DynamicVars value. -func (i DynamicVars) Desc() string { return enums.Desc(i, _DynamicVarsDescMap) } - -// DynamicVarsValues returns all possible values for the type DynamicVars. -func DynamicVarsValues() []DynamicVars { return _DynamicVarsValues } - -// Values returns all possible values for the type DynamicVars. -func (i DynamicVars) Values() []enums.Enum { return enums.Values(_DynamicVarsValues) } - -// MarshalText implements the [encoding.TextMarshaler] interface. -func (i DynamicVars) MarshalText() ([]byte, error) { return []byte(i.String()), nil } - -// UnmarshalText implements the [encoding.TextUnmarshaler] interface. -func (i *DynamicVars) UnmarshalText(text []byte) error { - return enums.UnmarshalText(i, text, "DynamicVars") -} - var _ContactVarsValues = []ContactVars{0, 1, 2, 3, 4, 5, 6, 7, 8} // ContactVarsN is the highest valid value for type ContactVars, plus one. @@ -154,11 +107,11 @@ const JointControlVarsN JointControlVars = 19 //gosl:end -var _JointControlVarsValueMap = map[string]JointControlVars{`JointForceX`: 0, `JointForceY`: 1, `JointForceZ`: 2, `JointTorqueX`: 3, `JointTorqueY`: 4, `JointTorqueZ`: 5, `JointTargetPosX`: 6, `JointTargetPosY`: 7, `JointTargetPosZ`: 8, `JointTargetRotX`: 9, `JointTargetRotY`: 10, `JointTargetRotZ`: 11, `JointTargetRotW`: 12, `JointTargetVelX`: 13, `JointTargetVelY`: 14, `JointTargetVelZ`: 15, `JointTargetAngVelX`: 16, `JointTargetAngVelY`: 17, `JointTargetAngVelZ`: 18} +var _JointControlVarsValueMap = map[string]JointControlVars{`JointCtrlForceX`: 0, `JointCtrlForceY`: 1, `JointCtrlForceZ`: 2, `JointCtrlTorqueX`: 3, `JointCtrlTorqueY`: 4, `JointCtrlTorqueZ`: 5, `JointTargetPosX`: 6, `JointTargetPosY`: 7, `JointTargetPosZ`: 8, `JointTargetRotX`: 9, `JointTargetRotY`: 10, `JointTargetRotZ`: 11, `JointTargetRotW`: 12, `JointTargetVelX`: 13, `JointTargetVelY`: 14, `JointTargetVelZ`: 15, `JointTargetAngVelX`: 16, `JointTargetAngVelY`: 17, `JointTargetAngVelZ`: 18} var _JointControlVarsDescMap = map[JointControlVars]string{0: `Joint force and torque inputs`, 1: ``, 2: ``, 3: ``, 4: ``, 5: ``, 6: `target values (1 DoF use JointTargetPosX)`, 7: ``, 8: ``, 9: ``, 10: ``, 11: ``, 12: ``, 13: `target velocity`, 14: ``, 15: ``, 16: `target angular velocity`, 17: ``, 18: ``} -var _JointControlVarsMap = map[JointControlVars]string{0: `JointForceX`, 1: `JointForceY`, 2: `JointForceZ`, 3: `JointTorqueX`, 4: `JointTorqueY`, 5: `JointTorqueZ`, 6: `JointTargetPosX`, 7: `JointTargetPosY`, 8: `JointTargetPosZ`, 9: `JointTargetRotX`, 10: `JointTargetRotY`, 11: `JointTargetRotZ`, 12: `JointTargetRotW`, 13: `JointTargetVelX`, 14: `JointTargetVelY`, 15: `JointTargetVelZ`, 16: `JointTargetAngVelX`, 17: `JointTargetAngVelY`, 18: `JointTargetAngVelZ`} +var _JointControlVarsMap = map[JointControlVars]string{0: `JointCtrlForceX`, 1: `JointCtrlForceY`, 2: `JointCtrlForceZ`, 3: `JointCtrlTorqueX`, 4: `JointCtrlTorqueY`, 5: `JointCtrlTorqueZ`, 6: `JointTargetPosX`, 7: `JointTargetPosY`, 8: `JointTargetPosZ`, 9: `JointTargetRotX`, 10: `JointTargetRotY`, 11: `JointTargetRotZ`, 12: `JointTargetRotW`, 13: `JointTargetVelX`, 14: `JointTargetVelY`, 15: `JointTargetVelZ`, 16: `JointTargetAngVelX`, 17: `JointTargetAngVelY`, 18: `JointTargetAngVelZ`} // String returns the string representation of this JointControlVars value. func (i JointControlVars) String() string { return enums.String(i, _JointControlVarsMap) } @@ -192,6 +145,53 @@ func (i *JointControlVars) UnmarshalText(text []byte) error { return enums.UnmarshalText(i, text, "JointControlVars") } +var _DynamicVarsValues = []DynamicVars{0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31} + +// DynamicVarsN is the highest valid value for type DynamicVars, plus one. +// +//gosl:start +const DynamicVarsN DynamicVars = 32 + +//gosl:end + +var _DynamicVarsValueMap = map[string]DynamicVars{`DynIndex`: 0, `DynPosX`: 1, `DynPosY`: 2, `DynPosZ`: 3, `DynRotX`: 4, `DynRotY`: 5, `DynRotZ`: 6, `DynRotW`: 7, `DynVelX`: 8, `DynVelY`: 9, `DynVelZ`: 10, `DynAngVelX`: 11, `DynAngVelY`: 12, `DynAngVelZ`: 13, `DynAccX`: 14, `DynAccY`: 15, `DynAccZ`: 16, `DynAngAccX`: 17, `DynAngAccY`: 18, `DynAngAccZ`: 19, `DynForceX`: 20, `DynForceY`: 21, `DynForceZ`: 22, `DynTorqueX`: 23, `DynTorqueY`: 24, `DynTorqueZ`: 25, `DynDeltaX`: 26, `DynDeltaY`: 27, `DynDeltaZ`: 28, `DynAngDeltaX`: 29, `DynAngDeltaY`: 30, `DynAngDeltaZ`: 31} + +var _DynamicVarsDescMap = map[DynamicVars]string{0: `Index of body in list of bodies.`, 1: `3D position of center of mass.`, 2: ``, 3: ``, 4: `Quaternion rotation.`, 5: ``, 6: ``, 7: ``, 8: `Linear velocity.`, 9: ``, 10: ``, 11: `Angular velocity.`, 12: ``, 13: ``, 14: `Linear acceleration.`, 15: ``, 16: ``, 17: `Angular acceleration due to applied torques.`, 18: ``, 19: ``, 20: `Linear force driving linear acceleration (from joints, etc).`, 21: ``, 22: ``, 23: `Torque driving angular acceleration (from joints, etc).`, 24: ``, 25: ``, 26: `Linear deltas.`, 27: ``, 28: ``, 29: `Angular deltas.`, 30: ``, 31: ``} + +var _DynamicVarsMap = map[DynamicVars]string{0: `DynIndex`, 1: `DynPosX`, 2: `DynPosY`, 3: `DynPosZ`, 4: `DynRotX`, 5: `DynRotY`, 6: `DynRotZ`, 7: `DynRotW`, 8: `DynVelX`, 9: `DynVelY`, 10: `DynVelZ`, 11: `DynAngVelX`, 12: `DynAngVelY`, 13: `DynAngVelZ`, 14: `DynAccX`, 15: `DynAccY`, 16: `DynAccZ`, 17: `DynAngAccX`, 18: `DynAngAccY`, 19: `DynAngAccZ`, 20: `DynForceX`, 21: `DynForceY`, 22: `DynForceZ`, 23: `DynTorqueX`, 24: `DynTorqueY`, 25: `DynTorqueZ`, 26: `DynDeltaX`, 27: `DynDeltaY`, 28: `DynDeltaZ`, 29: `DynAngDeltaX`, 30: `DynAngDeltaY`, 31: `DynAngDeltaZ`} + +// String returns the string representation of this DynamicVars value. +func (i DynamicVars) String() string { return enums.String(i, _DynamicVarsMap) } + +// SetString sets the DynamicVars value from its string representation, +// and returns an error if the string is invalid. +func (i *DynamicVars) SetString(s string) error { + return enums.SetString(i, s, _DynamicVarsValueMap, "DynamicVars") +} + +// Int64 returns the DynamicVars value as an int64. +func (i DynamicVars) Int64() int64 { return int64(i) } + +// SetInt64 sets the DynamicVars value from an int64. +func (i *DynamicVars) SetInt64(in int64) { *i = DynamicVars(in) } + +// Desc returns the description of the DynamicVars value. +func (i DynamicVars) Desc() string { return enums.Desc(i, _DynamicVarsDescMap) } + +// DynamicVarsValues returns all possible values for the type DynamicVars. +func DynamicVarsValues() []DynamicVars { return _DynamicVarsValues } + +// Values returns all possible values for the type DynamicVars. +func (i DynamicVars) Values() []enums.Enum { return enums.Values(_DynamicVarsValues) } + +// MarshalText implements the [encoding.TextMarshaler] interface. +func (i DynamicVars) MarshalText() ([]byte, error) { return []byte(i.String()), nil } + +// UnmarshalText implements the [encoding.TextUnmarshaler] interface. +func (i *DynamicVars) UnmarshalText(text []byte) error { + return enums.UnmarshalText(i, text, "DynamicVars") +} + var _GPUVarsValues = []GPUVars{0, 1, 2, 3, 4, 5, 6} // GPUVarsN is the highest valid value for type GPUVars, plus one. @@ -237,20 +237,20 @@ func (i GPUVars) MarshalText() ([]byte, error) { return []byte(i.String()), nil // UnmarshalText implements the [encoding.TextUnmarshaler] interface. func (i *GPUVars) UnmarshalText(text []byte) error { return enums.UnmarshalText(i, text, "GPUVars") } -var _JointVarsValues = []JointVars{0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34} +var _JointVarsValues = []JointVars{0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40} // JointVarsN is the highest valid value for type JointVars, plus one. // //gosl:start -const JointVarsN JointVars = 35 +const JointVarsN JointVars = 41 //gosl:end -var _JointVarsValueMap = map[string]JointVars{`JointType`: 0, `JointEnabled`: 1, `JointParent`: 2, `JointChild`: 3, `JointPPosX`: 4, `JointPPosY`: 5, `JointPPosZ`: 6, `JointPRotX`: 7, `JointPRotY`: 8, `JointPRotZ`: 9, `JointPRotW`: 10, `JointCPosX`: 11, `JointCPosY`: 12, `JointCPosZ`: 13, `JointCRotX`: 14, `JointCRotY`: 15, `JointCRotZ`: 16, `JointCRotW`: 17, `JointAxisX`: 18, `JointAxisY`: 19, `JointAxisZ`: 20, `JointLimitLower`: 21, `JointLimitUpper`: 22, `JointPForceX`: 23, `JointPForceY`: 24, `JointPForceZ`: 25, `JointPTorqueX`: 26, `JointPTorqueY`: 27, `JointPTorqueZ`: 28, `JointCForceX`: 29, `JointCForceY`: 30, `JointCForceZ`: 31, `JointCTorqueX`: 32, `JointCTorqueY`: 33, `JointCTorqueZ`: 34} +var _JointVarsValueMap = map[string]JointVars{`JointType`: 0, `JointEnabled`: 1, `JointParent`: 2, `JointChild`: 3, `JointPPosX`: 4, `JointPPosY`: 5, `JointPPosZ`: 6, `JointPRotX`: 7, `JointPRotY`: 8, `JointPRotZ`: 9, `JointPRotW`: 10, `JointCPosX`: 11, `JointCPosY`: 12, `JointCPosZ`: 13, `JointCRotX`: 14, `JointCRotY`: 15, `JointCRotZ`: 16, `JointCRotW`: 17, `JointAxisX`: 18, `JointAxisY`: 19, `JointAxisZ`: 20, `JointLimitLower`: 21, `JointLimitUpper`: 22, `JointStiffX`: 23, `JointStiffY`: 24, `JointStiffZ`: 25, `JointDampX`: 26, `JointDampY`: 27, `JointDampZ`: 28, `JointPForceX`: 29, `JointPForceY`: 30, `JointPForceZ`: 31, `JointPTorqueX`: 32, `JointPTorqueY`: 33, `JointPTorqueZ`: 34, `JointCForceX`: 35, `JointCForceY`: 36, `JointCForceZ`: 37, `JointCTorqueX`: 38, `JointCTorqueY`: 39, `JointCTorqueZ`: 40} -var _JointVarsDescMap = map[JointVars]string{0: `JointType (as an int32 from bits).`, 1: `JointEnabled allows joints to be dynamically enabled.`, 2: `JointParent is the dynamic body index for parent body. Can be -1 for a fixed parent for absolute anchor.`, 3: `JointChild is the dynamic body index for child body.`, 4: `position of joint, in parent frame.`, 5: ``, 6: ``, 7: `orientation of joint, in parent frame.`, 8: ``, 9: ``, 10: ``, 11: `position of joint, in child frame.`, 12: ``, 13: ``, 14: `orientation of joint, in child frame.`, 15: ``, 16: ``, 17: ``, 18: ``, 19: ``, 20: ``, 21: `joint limits`, 22: ``, 23: `Computed parent joint force value.`, 24: ``, 25: ``, 26: `Computed parent joint torque value.`, 27: ``, 28: ``, 29: `Computed child joint force value.`, 30: ``, 31: ``, 32: `Computed child joint torque value.`, 33: ``, 34: ``} +var _JointVarsDescMap = map[JointVars]string{0: `JointType (as an int32 from bits).`, 1: `JointEnabled allows joints to be dynamically enabled.`, 2: `JointParent is the dynamic body index for parent body. Can be -1 for a fixed parent for absolute anchor.`, 3: `JointChild is the dynamic body index for child body.`, 4: `position of joint, in parent frame.`, 5: ``, 6: ``, 7: `orientation of joint, in parent frame.`, 8: ``, 9: ``, 10: ``, 11: `position of joint, in child frame.`, 12: ``, 13: ``, 14: `orientation of joint, in child frame.`, 15: ``, 16: ``, 17: ``, 18: ``, 19: ``, 20: ``, 21: `joint limits`, 22: ``, 23: `joint stiffness target (ke)`, 24: ``, 25: ``, 26: `joint damping target (kd)`, 27: ``, 28: ``, 29: `Computed parent joint force value.`, 30: ``, 31: ``, 32: `Computed parent joint torque value.`, 33: ``, 34: ``, 35: `Computed child joint force value.`, 36: ``, 37: ``, 38: `Computed child joint torque value.`, 39: ``, 40: ``} -var _JointVarsMap = map[JointVars]string{0: `JointType`, 1: `JointEnabled`, 2: `JointParent`, 3: `JointChild`, 4: `JointPPosX`, 5: `JointPPosY`, 6: `JointPPosZ`, 7: `JointPRotX`, 8: `JointPRotY`, 9: `JointPRotZ`, 10: `JointPRotW`, 11: `JointCPosX`, 12: `JointCPosY`, 13: `JointCPosZ`, 14: `JointCRotX`, 15: `JointCRotY`, 16: `JointCRotZ`, 17: `JointCRotW`, 18: `JointAxisX`, 19: `JointAxisY`, 20: `JointAxisZ`, 21: `JointLimitLower`, 22: `JointLimitUpper`, 23: `JointPForceX`, 24: `JointPForceY`, 25: `JointPForceZ`, 26: `JointPTorqueX`, 27: `JointPTorqueY`, 28: `JointPTorqueZ`, 29: `JointCForceX`, 30: `JointCForceY`, 31: `JointCForceZ`, 32: `JointCTorqueX`, 33: `JointCTorqueY`, 34: `JointCTorqueZ`} +var _JointVarsMap = map[JointVars]string{0: `JointType`, 1: `JointEnabled`, 2: `JointParent`, 3: `JointChild`, 4: `JointPPosX`, 5: `JointPPosY`, 6: `JointPPosZ`, 7: `JointPRotX`, 8: `JointPRotY`, 9: `JointPRotZ`, 10: `JointPRotW`, 11: `JointCPosX`, 12: `JointCPosY`, 13: `JointCPosZ`, 14: `JointCRotX`, 15: `JointCRotY`, 16: `JointCRotZ`, 17: `JointCRotW`, 18: `JointAxisX`, 19: `JointAxisY`, 20: `JointAxisZ`, 21: `JointLimitLower`, 22: `JointLimitUpper`, 23: `JointStiffX`, 24: `JointStiffY`, 25: `JointStiffZ`, 26: `JointDampX`, 27: `JointDampY`, 28: `JointDampZ`, 29: `JointPForceX`, 30: `JointPForceY`, 31: `JointPForceZ`, 32: `JointPTorqueX`, 33: `JointPTorqueY`, 34: `JointPTorqueZ`, 35: `JointCForceX`, 36: `JointCForceY`, 37: `JointCForceZ`, 38: `JointCTorqueX`, 39: `JointCTorqueY`, 40: `JointCTorqueZ`} // String returns the string representation of this JointVars value. func (i JointVars) String() string { return enums.String(i, _JointVarsMap) } diff --git a/physics/gosl.go b/physics/gosl.go index e396127b..4ff1ce2f 100644 --- a/physics/gosl.go +++ b/physics/gosl.go @@ -106,6 +106,10 @@ func GPUInit() { sgp.SetNValues(1) } var pl *gpu.ComputePipeline + pl = gpu.NewComputePipelineShaderFS(shaders, "shaders/DynamicsCurToNext.wgsl", sy) + pl.AddVarUsed(0, "TensorStrides") + pl.AddVarUsed(2, "Dynamics") + pl.AddVarUsed(0, "Params") pl = gpu.NewComputePipelineShaderFS(shaders, "shaders/InitDynamics.wgsl", sy) pl.AddVarUsed(0, "TensorStrides") pl.AddVarUsed(1, "Bodies") @@ -116,10 +120,18 @@ func GPUInit() { pl.AddVarUsed(1, "Bodies") pl.AddVarUsed(2, "Dynamics") pl.AddVarUsed(0, "Params") - pl = gpu.NewComputePipelineShaderFS(shaders, "shaders/StepJoints.wgsl", sy) + pl = gpu.NewComputePipelineShaderFS(shaders, "shaders/StepJointForces.wgsl", sy) + pl.AddVarUsed(0, "TensorStrides") + pl.AddVarUsed(1, "Bodies") + pl.AddVarUsed(2, "Dynamics") + pl.AddVarUsed(3, "JointControls") + pl.AddVarUsed(1, "Joints") + pl.AddVarUsed(0, "Params") + pl = gpu.NewComputePipelineShaderFS(shaders, "shaders/StepSolveJoints.wgsl", sy) pl.AddVarUsed(0, "TensorStrides") pl.AddVarUsed(1, "Bodies") pl.AddVarUsed(2, "Dynamics") + pl.AddVarUsed(3, "JointControls") pl.AddVarUsed(1, "Joints") pl.AddVarUsed(0, "Params") sy.Config() @@ -141,6 +153,48 @@ func GPURelease() { ComputeGPU = nil } +// RunDynamicsCurToNext runs the DynamicsCurToNext kernel with given number of elements, +// on either the CPU or GPU depending on the UseGPU variable. +// Can call multiple Run* kernels in a row, which are then all launched +// in the same command submission on the GPU, which is by far the most efficient. +// MUST call RunDone (with optional vars to sync) after all Run calls. +// Alternatively, a single-shot RunOneDynamicsCurToNext call does Run and Done for a +// single run-and-sync case. +func RunDynamicsCurToNext(n int) { + if UseGPU { + RunDynamicsCurToNextGPU(n) + } else { + RunDynamicsCurToNextCPU(n) + } +} + +// RunDynamicsCurToNextGPU runs the DynamicsCurToNext kernel on the GPU. See [RunDynamicsCurToNext] for more info. +func RunDynamicsCurToNextGPU(n int) { + sy := GPUSystem + pl := sy.ComputePipelines["DynamicsCurToNext"] + ce, _ := sy.BeginComputePass() + pl.Dispatch1D(ce, n, 64) +} + +// RunDynamicsCurToNextCPU runs the DynamicsCurToNext kernel on the CPU. +func RunDynamicsCurToNextCPU(n int) { + gpu.VectorizeFunc(0, n, DynamicsCurToNext) +} + +// RunOneDynamicsCurToNext runs the DynamicsCurToNext kernel with given number of elements, +// on either the CPU or GPU depending on the UseGPU variable. +// This version then calls RunDone with the given variables to sync +// after the Run, for a single-shot Run-and-Done call. If multiple kernels +// can be run in sequence, it is much more efficient to do multiple Run* +// calls followed by a RunDone call. +func RunOneDynamicsCurToNext(n int, syncVars ...GPUVars) { + if UseGPU { + RunDynamicsCurToNextGPU(n) + RunDone(syncVars...) + } else { + RunDynamicsCurToNextCPU(n) + } +} // RunInitDynamics runs the InitDynamics kernel with given number of elements, // on either the CPU or GPU depending on the UseGPU variable. // Can call multiple Run* kernels in a row, which are then all launched @@ -225,46 +279,88 @@ func RunOneStepIntegrateBodies(n int, syncVars ...GPUVars) { RunStepIntegrateBodiesCPU(n) } } -// RunStepJoints runs the StepJoints kernel with given number of elements, +// RunStepJointForces runs the StepJointForces kernel with given number of elements, +// on either the CPU or GPU depending on the UseGPU variable. +// Can call multiple Run* kernels in a row, which are then all launched +// in the same command submission on the GPU, which is by far the most efficient. +// MUST call RunDone (with optional vars to sync) after all Run calls. +// Alternatively, a single-shot RunOneStepJointForces call does Run and Done for a +// single run-and-sync case. +func RunStepJointForces(n int) { + if UseGPU { + RunStepJointForcesGPU(n) + } else { + RunStepJointForcesCPU(n) + } +} + +// RunStepJointForcesGPU runs the StepJointForces kernel on the GPU. See [RunStepJointForces] for more info. +func RunStepJointForcesGPU(n int) { + sy := GPUSystem + pl := sy.ComputePipelines["StepJointForces"] + ce, _ := sy.BeginComputePass() + pl.Dispatch1D(ce, n, 64) +} + +// RunStepJointForcesCPU runs the StepJointForces kernel on the CPU. +func RunStepJointForcesCPU(n int) { + gpu.VectorizeFunc(0, n, StepJointForces) +} + +// RunOneStepJointForces runs the StepJointForces kernel with given number of elements, +// on either the CPU or GPU depending on the UseGPU variable. +// This version then calls RunDone with the given variables to sync +// after the Run, for a single-shot Run-and-Done call. If multiple kernels +// can be run in sequence, it is much more efficient to do multiple Run* +// calls followed by a RunDone call. +func RunOneStepJointForces(n int, syncVars ...GPUVars) { + if UseGPU { + RunStepJointForcesGPU(n) + RunDone(syncVars...) + } else { + RunStepJointForcesCPU(n) + } +} +// RunStepSolveJoints runs the StepSolveJoints kernel with given number of elements, // on either the CPU or GPU depending on the UseGPU variable. // Can call multiple Run* kernels in a row, which are then all launched // in the same command submission on the GPU, which is by far the most efficient. // MUST call RunDone (with optional vars to sync) after all Run calls. -// Alternatively, a single-shot RunOneStepJoints call does Run and Done for a +// Alternatively, a single-shot RunOneStepSolveJoints call does Run and Done for a // single run-and-sync case. -func RunStepJoints(n int) { +func RunStepSolveJoints(n int) { if UseGPU { - RunStepJointsGPU(n) + RunStepSolveJointsGPU(n) } else { - RunStepJointsCPU(n) + RunStepSolveJointsCPU(n) } } -// RunStepJointsGPU runs the StepJoints kernel on the GPU. See [RunStepJoints] for more info. -func RunStepJointsGPU(n int) { +// RunStepSolveJointsGPU runs the StepSolveJoints kernel on the GPU. See [RunStepSolveJoints] for more info. +func RunStepSolveJointsGPU(n int) { sy := GPUSystem - pl := sy.ComputePipelines["StepJoints"] + pl := sy.ComputePipelines["StepSolveJoints"] ce, _ := sy.BeginComputePass() pl.Dispatch1D(ce, n, 64) } -// RunStepJointsCPU runs the StepJoints kernel on the CPU. -func RunStepJointsCPU(n int) { - gpu.VectorizeFunc(0, n, StepJoints) +// RunStepSolveJointsCPU runs the StepSolveJoints kernel on the CPU. +func RunStepSolveJointsCPU(n int) { + gpu.VectorizeFunc(0, n, StepSolveJoints) } -// RunOneStepJoints runs the StepJoints kernel with given number of elements, +// RunOneStepSolveJoints runs the StepSolveJoints kernel with given number of elements, // on either the CPU or GPU depending on the UseGPU variable. // This version then calls RunDone with the given variables to sync // after the Run, for a single-shot Run-and-Done call. If multiple kernels // can be run in sequence, it is much more efficient to do multiple Run* // calls followed by a RunDone call. -func RunOneStepJoints(n int, syncVars ...GPUVars) { +func RunOneStepSolveJoints(n int, syncVars ...GPUVars) { if UseGPU { - RunStepJointsGPU(n) + RunStepSolveJointsGPU(n) RunDone(syncVars...) } else { - RunStepJointsCPU(n) + RunStepSolveJointsCPU(n) } } // RunDone must be called after Run* calls to start compute kernels. @@ -344,6 +440,7 @@ func ToGPUTensorStrides() { TensorStrides.SetInt1D(BodyJoints.Shape().Strides[2], 22) TensorStrides.SetInt1D(Dynamics.Shape().Strides[0], 30) TensorStrides.SetInt1D(Dynamics.Shape().Strides[1], 31) + TensorStrides.SetInt1D(Dynamics.Shape().Strides[2], 32) TensorStrides.SetInt1D(Contacts.Shape().Strides[0], 40) TensorStrides.SetInt1D(Contacts.Shape().Strides[1], 41) TensorStrides.SetInt1D(JointControls.Shape().Strides[0], 50) diff --git a/physics/joint.go b/physics/joint.go index 35b424c2..1c6cc250 100644 --- a/physics/joint.go +++ b/physics/joint.go @@ -63,6 +63,16 @@ const ( JointLimitLower JointLimitUpper + // joint stiffness target (ke) + JointStiffX + JointStiffY + JointStiffZ + + // joint damping target (kd) + JointDampX + JointDampY + JointDampZ + // Computed forces (temp storage until aggregated by bodies). // Computed parent joint force value. @@ -111,11 +121,7 @@ func JointChildIndex(idx int32) int32 { } func JointPPos(idx int32) math32.Vector3 { - var pos math32.Vector3 - pos.X = Joints.Value(int(idx), int(JointPPosX)) - pos.Y = Joints.Value(int(idx), int(JointPPosY)) - pos.Z = Joints.Value(int(idx), int(JointPPosZ)) - return pos + return math32.Vec3(Joints.Value(int(idx), int(JointPPosX)), Joints.Value(int(idx), int(JointPPosY)), Joints.Value(int(idx), int(JointPPosZ))) } func SetJointPPos(idx int32, pos math32.Vector3) { @@ -125,12 +131,7 @@ func SetJointPPos(idx int32, pos math32.Vector3) { } func JointPRot(idx int32) math32.Quat { - var rot math32.Quat - rot.X = Joints.Value(int(idx), int(JointPRotX)) - rot.Y = Joints.Value(int(idx), int(JointPRotY)) - rot.Z = Joints.Value(int(idx), int(JointPRotZ)) - rot.W = Joints.Value(int(idx), int(JointPRotW)) - return rot + return math32.NewQuat(Joints.Value(int(idx), int(JointPRotX)), Joints.Value(int(idx), int(JointPRotY)), Joints.Value(int(idx), int(JointPRotZ)), Joints.Value(int(idx), int(JointPRotW))) } func SetJointPRot(idx int32, rot math32.Quat) { @@ -140,12 +141,29 @@ func SetJointPRot(idx int32, rot math32.Quat) { Joints.Set(rot.W, int(idx), int(JointPRotW)) } +func JointCPos(idx int32) math32.Vector3 { + return math32.Vec3(Joints.Value(int(idx), int(JointCPosX)), Joints.Value(int(idx), int(JointCPosY)), Joints.Value(int(idx), int(JointCPosZ))) +} + +func SetJointCPos(idx int32, pos math32.Vector3) { + Joints.Set(pos.X, int(idx), int(JointCPosX)) + Joints.Set(pos.Y, int(idx), int(JointCPosY)) + Joints.Set(pos.Z, int(idx), int(JointCPosZ)) +} + +func JointCRot(idx int32) math32.Quat { + return math32.NewQuat(Joints.Value(int(idx), int(JointCRotX)), Joints.Value(int(idx), int(JointCRotY)), Joints.Value(int(idx), int(JointCRotZ)), Joints.Value(int(idx), int(JointCRotW))) +} + +func SetJointCRot(idx int32, rot math32.Quat) { + Joints.Set(rot.X, int(idx), int(JointCRotX)) + Joints.Set(rot.Y, int(idx), int(JointCRotY)) + Joints.Set(rot.Z, int(idx), int(JointCRotZ)) + Joints.Set(rot.W, int(idx), int(JointCRotW)) +} + func JointAxis(idx int32) math32.Vector3 { - var axis math32.Vector3 - axis.X = Joints.Value(int(idx), int(JointAxisX)) - axis.Y = Joints.Value(int(idx), int(JointAxisY)) - axis.Z = Joints.Value(int(idx), int(JointAxisZ)) - return axis + return math32.Vec3(Joints.Value(int(idx), int(JointAxisX)), Joints.Value(int(idx), int(JointAxisY)), Joints.Value(int(idx), int(JointAxisZ))) } func SetJointAxis(idx int32, axis math32.Vector3) { @@ -154,12 +172,28 @@ func SetJointAxis(idx int32, axis math32.Vector3) { Joints.Set(axis.Z, int(idx), int(JointAxisZ)) } +func JointStiff(idx int32) math32.Vector3 { + return math32.Vec3(Joints.Value(int(idx), int(JointStiffX)), Joints.Value(int(idx), int(JointStiffY)), Joints.Value(int(idx), int(JointStiffZ))) +} + +func SetJointStiff(idx int32, stiff math32.Vector3) { + Joints.Set(stiff.X, int(idx), int(JointStiffX)) + Joints.Set(stiff.Y, int(idx), int(JointStiffY)) + Joints.Set(stiff.Z, int(idx), int(JointStiffZ)) +} + +func JointDamp(idx int32) math32.Vector3 { + return math32.Vec3(Joints.Value(int(idx), int(JointDampX)), Joints.Value(int(idx), int(JointDampY)), Joints.Value(int(idx), int(JointDampZ))) +} + +func SetJointDamp(idx int32, damp math32.Vector3) { + Joints.Set(damp.X, int(idx), int(JointDampX)) + Joints.Set(damp.Y, int(idx), int(JointDampY)) + Joints.Set(damp.Z, int(idx), int(JointDampZ)) +} + func JointPForce(idx int32) math32.Vector3 { - var f math32.Vector3 - f.X = Joints.Value(int(idx), int(JointPForceX)) - f.Y = Joints.Value(int(idx), int(JointPForceY)) - f.Z = Joints.Value(int(idx), int(JointPForceZ)) - return f + return math32.Vec3(Joints.Value(int(idx), int(JointPForceX)), Joints.Value(int(idx), int(JointPForceY)), Joints.Value(int(idx), int(JointPForceZ))) } func SetJointPForce(idx int32, f math32.Vector3) { @@ -169,11 +203,7 @@ func SetJointPForce(idx int32, f math32.Vector3) { } func JointPTorque(idx int32) math32.Vector3 { - var t math32.Vector3 - t.X = Joints.Value(int(idx), int(JointPTorqueX)) - t.Y = Joints.Value(int(idx), int(JointPTorqueY)) - t.Z = Joints.Value(int(idx), int(JointPTorqueZ)) - return t + return math32.Vec3(Joints.Value(int(idx), int(JointPTorqueX)), Joints.Value(int(idx), int(JointPTorqueY)), Joints.Value(int(idx), int(JointPTorqueZ))) } func SetJointPTorque(idx int32, t math32.Vector3) { @@ -183,11 +213,7 @@ func SetJointPTorque(idx int32, t math32.Vector3) { } func JointCForce(idx int32) math32.Vector3 { - var f math32.Vector3 - f.X = Joints.Value(int(idx), int(JointCForceX)) - f.Y = Joints.Value(int(idx), int(JointCForceY)) - f.Z = Joints.Value(int(idx), int(JointCForceZ)) - return f + return math32.Vec3(Joints.Value(int(idx), int(JointCForceX)), Joints.Value(int(idx), int(JointCForceY)), Joints.Value(int(idx), int(JointCForceZ))) } func SetJointCForce(idx int32, f math32.Vector3) { @@ -197,11 +223,7 @@ func SetJointCForce(idx int32, f math32.Vector3) { } func JointCTorque(idx int32) math32.Vector3 { - var t math32.Vector3 - t.X = Joints.Value(int(idx), int(JointCTorqueX)) - t.Y = Joints.Value(int(idx), int(JointCTorqueY)) - t.Z = Joints.Value(int(idx), int(JointCTorqueZ)) - return t + return math32.Vec3(Joints.Value(int(idx), int(JointCTorqueX)), Joints.Value(int(idx), int(JointCTorqueY)), Joints.Value(int(idx), int(JointCTorqueZ))) } func SetJointCTorque(idx int32, t math32.Vector3) { diff --git a/physics/joint.goal b/physics/joint.goal index 1d9bf5a2..d2f5f635 100644 --- a/physics/joint.goal +++ b/physics/joint.goal @@ -61,6 +61,16 @@ const ( JointLimitLower JointLimitUpper + // joint stiffness target (ke) + JointStiffX + JointStiffY + JointStiffZ + + // joint damping target (kd) + JointDampX + JointDampY + JointDampZ + // Computed forces (temp storage until aggregated by bodies). // Computed parent joint force value. @@ -109,11 +119,7 @@ func JointChildIndex(idx int32) int32 { } func JointPPos(idx int32) math32.Vector3 { - var pos math32.Vector3 - pos.X = Joints[idx, JointPPosX] - pos.Y = Joints[idx, JointPPosY] - pos.Z = Joints[idx, JointPPosZ] - return pos + return math32.Vec3(Joints[idx, JointPPosX], Joints[idx, JointPPosY], Joints[idx, JointPPosZ]) } func SetJointPPos(idx int32, pos math32.Vector3) { @@ -123,12 +129,7 @@ func SetJointPPos(idx int32, pos math32.Vector3) { } func JointPRot(idx int32) math32.Quat { - var rot math32.Quat - rot.X = Joints[idx, JointPRotX] - rot.Y = Joints[idx, JointPRotY] - rot.Z = Joints[idx, JointPRotZ] - rot.W = Joints[idx, JointPRotW] - return rot + return math32.NewQuat(Joints[idx, JointPRotX], Joints[idx, JointPRotY], Joints[idx, JointPRotZ], Joints[idx, JointPRotW]) } func SetJointPRot(idx int32, rot math32.Quat) { @@ -138,12 +139,29 @@ func SetJointPRot(idx int32, rot math32.Quat) { Joints[idx, JointPRotW] = rot.W } +func JointCPos(idx int32) math32.Vector3 { + return math32.Vec3(Joints[idx, JointCPosX], Joints[idx, JointCPosY], Joints[idx, JointCPosZ]) +} + +func SetJointCPos(idx int32, pos math32.Vector3) { + Joints[idx, JointCPosX] = pos.X + Joints[idx, JointCPosY] = pos.Y + Joints[idx, JointCPosZ] = pos.Z +} + +func JointCRot(idx int32) math32.Quat { + return math32.NewQuat(Joints[idx, JointCRotX], Joints[idx, JointCRotY], Joints[idx, JointCRotZ], Joints[idx, JointCRotW]) +} + +func SetJointCRot(idx int32, rot math32.Quat) { + Joints[idx, JointCRotX] = rot.X + Joints[idx, JointCRotY] = rot.Y + Joints[idx, JointCRotZ] = rot.Z + Joints[idx, JointCRotW] = rot.W +} + func JointAxis(idx int32) math32.Vector3 { - var axis math32.Vector3 - axis.X = Joints[idx, JointAxisX] - axis.Y = Joints[idx, JointAxisY] - axis.Z = Joints[idx, JointAxisZ] - return axis + return math32.Vec3(Joints[idx, JointAxisX], Joints[idx, JointAxisY], Joints[idx, JointAxisZ]) } func SetJointAxis(idx int32, axis math32.Vector3) { @@ -152,12 +170,28 @@ func SetJointAxis(idx int32, axis math32.Vector3) { Joints[idx, JointAxisZ] = axis.Z } +func JointStiff(idx int32) math32.Vector3 { + return math32.Vec3(Joints[idx, JointStiffX], Joints[idx, JointStiffY], Joints[idx, JointStiffZ]) +} + +func SetJointStiff(idx int32, stiff math32.Vector3) { + Joints[idx, JointStiffX] = stiff.X + Joints[idx, JointStiffY] = stiff.Y + Joints[idx, JointStiffZ] = stiff.Z +} + +func JointDamp(idx int32) math32.Vector3 { + return math32.Vec3(Joints[idx, JointDampX], Joints[idx, JointDampY], Joints[idx, JointDampZ]) +} + +func SetJointDamp(idx int32, damp math32.Vector3) { + Joints[idx, JointDampX] = damp.X + Joints[idx, JointDampY] = damp.Y + Joints[idx, JointDampZ] = damp.Z +} + func JointPForce(idx int32) math32.Vector3 { - var f math32.Vector3 - f.X = Joints[idx, JointPForceX] - f.Y = Joints[idx, JointPForceY] - f.Z = Joints[idx, JointPForceZ] - return f + return math32.Vec3(Joints[idx, JointPForceX], Joints[idx, JointPForceY], Joints[idx, JointPForceZ]) } func SetJointPForce(idx int32, f math32.Vector3) { @@ -167,11 +201,7 @@ func SetJointPForce(idx int32, f math32.Vector3) { } func JointPTorque(idx int32) math32.Vector3 { - var t math32.Vector3 - t.X = Joints[idx, JointPTorqueX] - t.Y = Joints[idx, JointPTorqueY] - t.Z = Joints[idx, JointPTorqueZ] - return t + return math32.Vec3(Joints[idx, JointPTorqueX], Joints[idx, JointPTorqueY], Joints[idx, JointPTorqueZ]) } func SetJointPTorque(idx int32, t math32.Vector3) { @@ -181,11 +211,7 @@ func SetJointPTorque(idx int32, t math32.Vector3) { } func JointCForce(idx int32) math32.Vector3 { - var f math32.Vector3 - f.X = Joints[idx, JointCForceX] - f.Y = Joints[idx, JointCForceY] - f.Z = Joints[idx, JointCForceZ] - return f + return math32.Vec3(Joints[idx, JointCForceX], Joints[idx, JointCForceY], Joints[idx, JointCForceZ]) } func SetJointCForce(idx int32, f math32.Vector3) { @@ -195,11 +221,7 @@ func SetJointCForce(idx int32, f math32.Vector3) { } func JointCTorque(idx int32) math32.Vector3 { - var t math32.Vector3 - t.X = Joints[idx, JointCTorqueX] - t.Y = Joints[idx, JointCTorqueY] - t.Z = Joints[idx, JointCTorqueZ] - return t + return math32.Vec3(Joints[idx, JointCTorqueX], Joints[idx, JointCTorqueY], Joints[idx, JointCTorqueZ]) } func SetJointCTorque(idx int32, t math32.Vector3) { diff --git a/physics/params.go b/physics/params.go index d5811510..b30bff76 100644 --- a/physics/params.go +++ b/physics/params.go @@ -19,6 +19,12 @@ type PhysParams struct { // JointsN is number of joints. JointsN int32 + // Index for the current state (0 or 1, alternates with Next). + Cur int32 + + // Index for the next state (1 or 0, alternates with Cur). + Next int32 + // Iters is the number of iterations to perform. Iters int32 `default:"2"` @@ -52,7 +58,7 @@ type PhysParams struct { // Restitution Restitution slbool.Bool `default:"false"` - pad, pad1, pad2 float32 + pad float32 // Gravity is the gravity acceleration function Gravity slvec.Vector3 diff --git a/physics/shaders/InitDynamics.wgsl b/physics/shaders/InitDynamics.wgsl index 06553dcd..902aeae4 100644 --- a/physics/shaders/InitDynamics.wgsl +++ b/physics/shaders/InitDynamics.wgsl @@ -9,7 +9,7 @@ var Params: array; // // Bodies are the rigid body elements (dynamic and static), // specifying the constant, non-dynamic properties, // which is initial state for dynamics. // [body][BodyVarsN] @group(1) @binding(0) var Bodies: array; -// // Dynamics are the dynamic rigid body elements: these actually move. // [dyn body][DynamicVarsN] +// // Dynamics are the dynamic rigid body elements: these actually move. // Two alternating states are used: Params.Cur and Params.Next. // [dyn body][cur/next][DynamicVarsN] @group(2) @binding(0) var Dynamics: array; // // JointControls are dynamic joint control inputs. // [joint][JointControlVarsN] @@ -26,6 +26,10 @@ fn Index2D(s0: u32, s1: u32, i0: u32, i1: u32) -> u32 { return s0 * i0 + s1 * i1; } +fn Index3D(s0: u32, s1: u32, s2: u32, i0: u32, i1: u32, i2: u32) -> u32 { + return s0 * i0 + s1 * i1 + s2 * i2; +} + //////// import: "vars.go" @@ -68,6 +72,42 @@ const BodyInvInertiaZY: BodyVars = 33; const BodyInvInertiaXZ: BodyVars = 34; const BodyInvInertiaYZ: BodyVars = 35; const BodyInvInertiaZZ: BodyVars = 36; + +//////// import: "contact.go" +alias ContactVars = i32; //enums:enum +const ContactA: ContactVars = 0; +const ContactB: ContactVars = 1; +const ContactNormX: ContactVars = 2; +const ContactNormY: ContactVars = 3; +const ContactNormZ: ContactVars = 4; +const ContactPointX: ContactVars = 5; +const ContactPointY: ContactVars = 6; +const ContactPointZ: ContactVars = 7; +const ContactDist: ContactVars = 8; + +//////// import: "control.go" +alias JointControlVars = i32; //enums:enum +const JointCtrlForceX: JointControlVars = 0; +const JointCtrlForceY: JointControlVars = 1; +const JointCtrlForceZ: JointControlVars = 2; +const JointCtrlTorqueX: JointControlVars = 3; +const JointCtrlTorqueY: JointControlVars = 4; +const JointCtrlTorqueZ: JointControlVars = 5; +const JointTargetPosX: JointControlVars = 6; +const JointTargetPosY: JointControlVars = 7; +const JointTargetPosZ: JointControlVars = 8; +const JointTargetRotX: JointControlVars = 9; +const JointTargetRotY: JointControlVars = 10; +const JointTargetRotZ: JointControlVars = 11; +const JointTargetRotW: JointControlVars = 12; +const JointTargetVelX: JointControlVars = 13; +const JointTargetVelY: JointControlVars = 14; +const JointTargetVelZ: JointControlVars = 15; +const JointTargetAngVelX: JointControlVars = 16; +const JointTargetAngVelY: JointControlVars = 17; +const JointTargetAngVelZ: JointControlVars = 18; + +//////// import: "dynamics.go" alias DynamicVars = i32; //enums:enum const DynIndex: DynamicVars = 0; const DynPosX: DynamicVars = 1; @@ -101,51 +141,17 @@ const DynDeltaZ: DynamicVars = 28; const DynAngDeltaX: DynamicVars = 29; const DynAngDeltaY: DynamicVars = 30; const DynAngDeltaZ: DynamicVars = 31; -fn DynamicIndex(idx: i32) -> i32 { - return i32(bitcast(Dynamics[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(DynIndex))])); +fn DynamicIndex(idx: i32,cni: i32) -> i32 { + return i32(bitcast(Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynIndex))])); } -//////// import: "contact.go" -alias ContactVars = i32; //enums:enum -const ContactA: ContactVars = 0; -const ContactB: ContactVars = 1; -const ContactNormX: ContactVars = 2; -const ContactNormY: ContactVars = 3; -const ContactNormZ: ContactVars = 4; -const ContactPointX: ContactVars = 5; -const ContactPointY: ContactVars = 6; -const ContactPointZ: ContactVars = 7; -const ContactDist: ContactVars = 8; - -//////// import: "control.go" -alias JointControlVars = i32; //enums:enum -const JointForceX: JointControlVars = 0; -const JointForceY: JointControlVars = 1; -const JointForceZ: JointControlVars = 2; -const JointTorqueX: JointControlVars = 3; -const JointTorqueY: JointControlVars = 4; -const JointTorqueZ: JointControlVars = 5; -const JointTargetPosX: JointControlVars = 6; -const JointTargetPosY: JointControlVars = 7; -const JointTargetPosZ: JointControlVars = 8; -const JointTargetRotX: JointControlVars = 9; -const JointTargetRotY: JointControlVars = 10; -const JointTargetRotZ: JointControlVars = 11; -const JointTargetRotW: JointControlVars = 12; -const JointTargetVelX: JointControlVars = 13; -const JointTargetVelY: JointControlVars = 14; -const JointTargetVelZ: JointControlVars = 15; -const JointTargetAngVelX: JointControlVars = 16; -const JointTargetAngVelY: JointControlVars = 17; -const JointTargetAngVelZ: JointControlVars = 18; - //////// import: "enumgen.go" const BodyVarsN: BodyVars = 37; -const DynamicVarsN: DynamicVars = 32; const ContactVarsN: ContactVars = 9; const JointControlVarsN: JointControlVars = 19; +const DynamicVarsN: DynamicVars = 32; const GPUVarsN: GPUVars = 7; -const JointVarsN: JointVars = 35; +const JointVarsN: JointVars = 41; const JointTypesN: JointTypes = 7; const ShapesN: Shapes = 4; @@ -174,18 +180,24 @@ const JointAxisY: JointVars = 19; const JointAxisZ: JointVars = 20; const JointLimitLower: JointVars = 21; const JointLimitUpper: JointVars = 22; -const JointPForceX: JointVars = 23; -const JointPForceY: JointVars = 24; -const JointPForceZ: JointVars = 25; -const JointPTorqueX: JointVars = 26; -const JointPTorqueY: JointVars = 27; -const JointPTorqueZ: JointVars = 28; -const JointCForceX: JointVars = 29; -const JointCForceY: JointVars = 30; -const JointCForceZ: JointVars = 31; -const JointCTorqueX: JointVars = 32; -const JointCTorqueY: JointVars = 33; -const JointCTorqueZ: JointVars = 34; +const JointStiffX: JointVars = 23; +const JointStiffY: JointVars = 24; +const JointStiffZ: JointVars = 25; +const JointDampX: JointVars = 26; +const JointDampY: JointVars = 27; +const JointDampZ: JointVars = 28; +const JointPForceX: JointVars = 29; +const JointPForceY: JointVars = 30; +const JointPForceZ: JointVars = 31; +const JointPTorqueX: JointVars = 32; +const JointPTorqueY: JointVars = 33; +const JointPTorqueZ: JointVars = 34; +const JointCForceX: JointVars = 35; +const JointCForceY: JointVars = 36; +const JointCForceZ: JointVars = 37; +const JointCTorqueX: JointVars = 38; +const JointCTorqueY: JointVars = 39; +const JointCTorqueZ: JointVars = 40; alias JointTypes = i32; //enums:enum const Prismatic: JointTypes = 0; const Revolute: JointTypes = 1; @@ -199,6 +211,8 @@ const D6: JointTypes = 6; struct PhysParams { DynamicsN: i32, JointsN: i32, + Cur: i32, + Next: i32, Iters: i32, Dt: f32, SoftRelax: f32, @@ -211,8 +225,6 @@ struct PhysParams { ContactWeighting: i32, Restitution: i32, pad: f32, - pad1: f32, - pad2: f32, Gravity: vec4, } @@ -230,22 +242,28 @@ const Capsule: Shapes = 3; //////// import: "slmath-vector3.go" //////// import: "step.go" + +//////// import: "step_body.go" fn InitDynamics(i: u32) { //gosl:kernel let pars = Params[0]; var ii = i32(i); if (ii >= pars.DynamicsN) { return; } - var bi = DynamicIndex(ii); - Dynamics[Index2D(TensorStrides[30], TensorStrides[31], u32(ii), u32(DynPosX))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyPosX))]; - Dynamics[Index2D(TensorStrides[30], TensorStrides[31], u32(ii), u32(DynPosY))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyPosY))]; - Dynamics[Index2D(TensorStrides[30], TensorStrides[31], u32(ii), u32(DynPosZ))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyPosZ))]; - Dynamics[Index2D(TensorStrides[30], TensorStrides[31], u32(ii), u32(DynRotX))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyRotX))]; - Dynamics[Index2D(TensorStrides[30], TensorStrides[31], u32(ii), u32(DynRotY))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyRotY))]; - Dynamics[Index2D(TensorStrides[30], TensorStrides[31], u32(ii), u32(DynRotZ))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyRotZ))]; - Dynamics[Index2D(TensorStrides[30], TensorStrides[31], u32(ii), u32(DynRotW))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyRotW))]; - for (var v = DynVelX; v < DynamicVarsN; v++) { - Dynamics[Index2D(TensorStrides[30], TensorStrides[31], - u32(ii), u32(v))] = 0.0; + for (var cni=0; cni<2; cni++) { + var bi = DynamicIndex(ii, i32(cni)); + Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(ii), u32(cni), u32(DynPosX))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyPosX))]; + Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(ii), u32(cni), u32(DynPosY))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyPosY))]; + Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(ii), u32(cni), u32(DynPosZ))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyPosZ))]; + Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(ii), u32(cni), u32(DynRotX))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyRotX))]; + Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(ii), u32(cni), u32(DynRotY))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyRotY))]; + Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(ii), u32(cni), u32(DynRotZ))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyRotZ))]; + Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(ii), u32(cni), u32(DynRotW))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyRotW))]; + for (var v = DynVelX; v < DynamicVarsN; v++) { + Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], + u32(ii), u32(cni), u32(v))] = 0.0; + } } -} \ No newline at end of file +} + +//////// import: "step_joint.go" \ No newline at end of file diff --git a/physics/shaders/StepJoints.wgsl b/physics/shaders/StepJoints.wgsl deleted file mode 100644 index 5d90f625..00000000 --- a/physics/shaders/StepJoints.wgsl +++ /dev/null @@ -1,374 +0,0 @@ -// Code generated by "gosl"; DO NOT EDIT -// kernel: StepJoints - -// // Params are global parameters. -@group(0) @binding(0) -var TensorStrides: array; -@group(0) @binding(1) -var Params: array; -// // Bodies are the rigid body elements (dynamic and static), // specifying the constant, non-dynamic properties, // which is initial state for dynamics. // [body][BodyVarsN] -@group(1) @binding(0) -var Bodies: array; -@group(1) @binding(1) -var Joints: array; -// // Dynamics are the dynamic rigid body elements: these actually move. // [dyn body][DynamicVarsN] -@group(2) @binding(0) -var Dynamics: array; -// // JointControls are dynamic joint control inputs. // [joint][JointControlVarsN] - -alias GPUVars = i32; - -@compute @workgroup_size(64, 1, 1) -fn main(@builtin(workgroup_id) wgid: vec3, @builtin(num_workgroups) nwg: vec3, @builtin(local_invocation_index) loci: u32) { - let idx = loci + (wgid.x + wgid.y * nwg.x + wgid.z * nwg.x * nwg.y) * 64; - StepJoints(idx); -} - -fn Index2D(s0: u32, s1: u32, i0: u32, i1: u32) -> u32 { - return s0 * i0 + s1 * i1; -} - - -//////// import: "vars.go" - -//////// import: "body.go" -alias BodyVars = i32; //enums:enum -const BodyShape: BodyVars = 0; -const BodyWorldIndex: BodyVars = 1; -const BodySizeX: BodyVars = 2; -const BodySizeY: BodyVars = 3; -const BodySizeZ: BodyVars = 4; -const BodyMass: BodyVars = 5; -const BodyInvMass: BodyVars = 6; -const BodyBounce: BodyVars = 7; -const BodyFriction: BodyVars = 8; -const BodyPosX: BodyVars = 9; -const BodyPosY: BodyVars = 10; -const BodyPosZ: BodyVars = 11; -const BodyRotX: BodyVars = 12; -const BodyRotY: BodyVars = 13; -const BodyRotZ: BodyVars = 14; -const BodyRotW: BodyVars = 15; -const BodyComX: BodyVars = 16; -const BodyComY: BodyVars = 17; -const BodyComZ: BodyVars = 18; -const BodyInertiaXX: BodyVars = 19; -const BodyInertiaYX: BodyVars = 20; -const BodyInertiaZX: BodyVars = 21; -const BodyInertiaXY: BodyVars = 22; -const BodyInertiaYY: BodyVars = 23; -const BodyInertiaZY: BodyVars = 24; -const BodyInertiaXZ: BodyVars = 25; -const BodyInertiaYZ: BodyVars = 26; -const BodyInertiaZZ: BodyVars = 27; -const BodyInvInertiaXX: BodyVars = 28; -const BodyInvInertiaYX: BodyVars = 29; -const BodyInvInertiaZX: BodyVars = 30; -const BodyInvInertiaXY: BodyVars = 31; -const BodyInvInertiaYY: BodyVars = 32; -const BodyInvInertiaZY: BodyVars = 33; -const BodyInvInertiaXZ: BodyVars = 34; -const BodyInvInertiaYZ: BodyVars = 35; -const BodyInvInertiaZZ: BodyVars = 36; -fn BodyCom(idx: i32) -> vec3 { - return vec3(Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComX))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComY))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComZ))]); -} -alias DynamicVars = i32; //enums:enum -const DynIndex: DynamicVars = 0; -const DynPosX: DynamicVars = 1; -const DynPosY: DynamicVars = 2; -const DynPosZ: DynamicVars = 3; -const DynRotX: DynamicVars = 4; -const DynRotY: DynamicVars = 5; -const DynRotZ: DynamicVars = 6; -const DynRotW: DynamicVars = 7; -const DynVelX: DynamicVars = 8; -const DynVelY: DynamicVars = 9; -const DynVelZ: DynamicVars = 10; -const DynAngVelX: DynamicVars = 11; -const DynAngVelY: DynamicVars = 12; -const DynAngVelZ: DynamicVars = 13; -const DynAccX: DynamicVars = 14; -const DynAccY: DynamicVars = 15; -const DynAccZ: DynamicVars = 16; -const DynAngAccX: DynamicVars = 17; -const DynAngAccY: DynamicVars = 18; -const DynAngAccZ: DynamicVars = 19; -const DynForceX: DynamicVars = 20; -const DynForceY: DynamicVars = 21; -const DynForceZ: DynamicVars = 22; -const DynTorqueX: DynamicVars = 23; -const DynTorqueY: DynamicVars = 24; -const DynTorqueZ: DynamicVars = 25; -const DynDeltaX: DynamicVars = 26; -const DynDeltaY: DynamicVars = 27; -const DynDeltaZ: DynamicVars = 28; -const DynAngDeltaX: DynamicVars = 29; -const DynAngDeltaY: DynamicVars = 30; -const DynAngDeltaZ: DynamicVars = 31; -fn DynamicIndex(idx: i32) -> i32 { - return i32(bitcast(Dynamics[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(DynIndex))])); -} -fn DynamicPos(idx: i32) -> vec3 { - return vec3(Dynamics[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(DynPosX))], Dynamics[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(DynPosY))], Dynamics[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(DynPosZ))]); -} -fn DynamicRot(idx: i32) -> vec4 { - return vec4(Dynamics[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(DynRotX))], Dynamics[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(DynRotY))], Dynamics[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(DynRotZ))], Dynamics[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(DynRotW))]); -} - -//////// import: "contact.go" -alias ContactVars = i32; //enums:enum -const ContactA: ContactVars = 0; -const ContactB: ContactVars = 1; -const ContactNormX: ContactVars = 2; -const ContactNormY: ContactVars = 3; -const ContactNormZ: ContactVars = 4; -const ContactPointX: ContactVars = 5; -const ContactPointY: ContactVars = 6; -const ContactPointZ: ContactVars = 7; -const ContactDist: ContactVars = 8; - -//////// import: "control.go" -alias JointControlVars = i32; //enums:enum -const JointForceX: JointControlVars = 0; -const JointForceY: JointControlVars = 1; -const JointForceZ: JointControlVars = 2; -const JointTorqueX: JointControlVars = 3; -const JointTorqueY: JointControlVars = 4; -const JointTorqueZ: JointControlVars = 5; -const JointTargetPosX: JointControlVars = 6; -const JointTargetPosY: JointControlVars = 7; -const JointTargetPosZ: JointControlVars = 8; -const JointTargetRotX: JointControlVars = 9; -const JointTargetRotY: JointControlVars = 10; -const JointTargetRotZ: JointControlVars = 11; -const JointTargetRotW: JointControlVars = 12; -const JointTargetVelX: JointControlVars = 13; -const JointTargetVelY: JointControlVars = 14; -const JointTargetVelZ: JointControlVars = 15; -const JointTargetAngVelX: JointControlVars = 16; -const JointTargetAngVelY: JointControlVars = 17; -const JointTargetAngVelZ: JointControlVars = 18; -fn JointForce(idx: i32) -> vec3 { var f: vec3;; f.x = Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointForceX))];; f.y = Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointForceY))];; f.z = Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointForceZ))];; return f; } -fn JointTorque(idx: i32) -> vec3 { var f: vec3;; f.x = Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointTorqueX))];; f.y = Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointTorqueY))];; f.z = Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointTorqueZ))];; return f; } - -//////// import: "enumgen.go" -const BodyVarsN: BodyVars = 37; -const DynamicVarsN: DynamicVars = 32; -const ContactVarsN: ContactVars = 9; -const JointControlVarsN: JointControlVars = 19; -const GPUVarsN: GPUVars = 7; -const JointVarsN: JointVars = 35; -const JointTypesN: JointTypes = 7; -const ShapesN: Shapes = 4; - -//////// import: "joint.go" -alias JointVars = i32; //enums:enum -const JointType: JointVars = 0; -const JointEnabled: JointVars = 1; -const JointParent: JointVars = 2; -const JointChild: JointVars = 3; -const JointPPosX: JointVars = 4; -const JointPPosY: JointVars = 5; -const JointPPosZ: JointVars = 6; -const JointPRotX: JointVars = 7; -const JointPRotY: JointVars = 8; -const JointPRotZ: JointVars = 9; -const JointPRotW: JointVars = 10; -const JointCPosX: JointVars = 11; -const JointCPosY: JointVars = 12; -const JointCPosZ: JointVars = 13; -const JointCRotX: JointVars = 14; -const JointCRotY: JointVars = 15; -const JointCRotZ: JointVars = 16; -const JointCRotW: JointVars = 17; -const JointAxisX: JointVars = 18; -const JointAxisY: JointVars = 19; -const JointAxisZ: JointVars = 20; -const JointLimitLower: JointVars = 21; -const JointLimitUpper: JointVars = 22; -const JointPForceX: JointVars = 23; -const JointPForceY: JointVars = 24; -const JointPForceZ: JointVars = 25; -const JointPTorqueX: JointVars = 26; -const JointPTorqueY: JointVars = 27; -const JointPTorqueZ: JointVars = 28; -const JointCForceX: JointVars = 29; -const JointCForceY: JointVars = 30; -const JointCForceZ: JointVars = 31; -const JointCTorqueX: JointVars = 32; -const JointCTorqueY: JointVars = 33; -const JointCTorqueZ: JointVars = 34; -fn GetJointType(idx: i32) -> JointTypes { - return JointTypes(bitcast(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointType))])); -} -fn JointParentIndex(idx: i32) -> i32 { - return i32(bitcast(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointParent))])); -} -fn JointChildIndex(idx: i32) -> i32 { - return i32(bitcast(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointChild))])); -} -fn JointPPos(idx: i32) -> vec3 { - var pos: vec3; - pos.x = Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPPosX))]; - pos.y = Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPPosY))]; - pos.z = Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPPosZ))];return pos; -} -fn JointPRot(idx: i32) -> vec4 { - var rot: vec4; - rot.x = Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPRotX))]; - rot.y = Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPRotY))]; - rot.z = Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPRotZ))]; - rot.w = Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPRotW))];return rot; -} -fn JointAxis(idx: i32) -> vec3 { - var axis: vec3; - axis.x = Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointAxisX))]; - axis.y = Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointAxisY))]; - axis.z = Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointAxisZ))];return axis; -} -fn SetJointPForce(idx: i32, f: vec3) { - Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPForceX))] = f.x; - Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPForceY))] = f.y; - Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPForceZ))] = f.z; -} -fn SetJointPTorque(idx: i32, t: vec3) { - Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPTorqueX))] = t.x; - Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPTorqueY))] = t.y; - Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPTorqueZ))] = t.z; -} -fn SetJointCForce(idx: i32, f: vec3) { - Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCForceX))] = f.x; - Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCForceY))] = f.y; - Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCForceZ))] = f.z; -} -fn SetJointCTorque(idx: i32, t: vec3) { - Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCTorqueX))] = t.x; - Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCTorqueY))] = t.y; - Joints[Index2D(TensorStrides[10], TensorStrides[11], - u32(idx), u32(JointCTorqueZ))] = t.z; -} -alias JointTypes = i32; //enums:enum -const Prismatic: JointTypes = 0; -const Revolute: JointTypes = 1; -const Ball: JointTypes = 2; -const Fixed: JointTypes = 3; -const Free: JointTypes = 4; -const Distance: JointTypes = 5; -const D6: JointTypes = 6; - -//////// import: "params.go" -struct PhysParams { - DynamicsN: i32, - JointsN: i32, - Iters: i32, - Dt: f32, - SoftRelax: f32, - JointLinearRelax: f32, - JointAngularRelax: f32, - JointLinearComply: f32, - JointAngularComply: f32, - ContactRelax: f32, - AngularDamping: f32, - ContactWeighting: i32, - Restitution: i32, - pad: f32, - pad1: f32, - pad2: f32, - Gravity: vec4, -} - -//////// import: "shapes.go" -alias Shapes = i32; //enums:enum -const Box: Shapes = 0; -const Sphere: Shapes = 1; -const Cylinder: Shapes = 2; -const Capsule: Shapes = 3; - -//////// import: "slmath-matrix3.go" - -//////// import: "slmath-quaternion.go" -fn MulQuatVector(q: vec4, v: vec3) -> vec3 { - var xyz = vec3(q.x, q.y, q.z); - var t = MulScalar3(Cross3(xyz, v), f32(f32(2)));return v+(MulScalar3(t, q.w))+(Cross3(xyz, t)); -} -fn MulQuats(a: vec4,b: vec4) -> vec4 { - var q: vec4; - q.x = a.x*b.w + a.w*b.x + a.y*b.z - a.z*b.y; - q.y = a.y*b.w + a.w*b.y + a.z*b.x - a.x*b.z; - q.z = a.z*b.w + a.w*b.z + a.x*b.y - a.y*b.x; - q.w = a.w*b.w - a.x*b.x - a.y*b.y - a.z*b.z;return q; -} -fn MulQPTransforms(aP: vec3, aQ: vec4, bP: vec3, bQ: vec4, oP: ptr>, oQ: ptr>) { - var br = MulQuatVector(aQ, bP); - *oP = br+(aP); - *oQ = MulQuats(aQ, bQ); -} -fn MulQPPoint(xP: vec3, xQ: vec4, p: vec3) -> vec3 { - var dp = MulQuatVector(xQ, p);return dp+(xP); -} - -//////// import: "slmath-vector3.go" -fn MulScalar3(v: vec3, s: f32) -> vec3 { - return vec3(v.x*s, v.y*s, v.z*s); -} -fn Cross3(v: vec3,o: vec3) -> vec3 { - return vec3(v.y*o.z-v.z*o.y, v.z*o.x-v.x*o.z, v.x*o.y-v.y*o.x); -} - -//////// import: "step.go" -fn StepJoints(i: u32) { //gosl:kernel - let pars = Params[0]; - var ji = i32(i); - if (ji >= pars.JointsN) { - return; - } - var jpi = JointParentIndex(ji); - var jpbi = DynamicIndex(jpi); - var jci = JointChildIndex(ji); - var jcbi = DynamicIndex(jci); - var jt = GetJointType(ji); - var jpP = JointPPos(ji); - var jpQ = JointPRot(ji); - var xwpP = jpP; - var xwpQ = jpQ; - var posepP = jpP; - var posepQ = jpQ; - var comp: vec3; - if (jpi >= 0) { // can be fixed - posepP = DynamicPos(jpi); - posepQ = DynamicRot(jpi); - MulQPTransforms(posepP, posepQ, jpP, jpQ, &xwpP, &xwpQ); - comp = BodyCom(jpbi); - } - var rp = xwpP-(MulQPPoint(posepP, posepQ, comp)); // parent moment arm - var posecP = DynamicPos(jci); - var posecQ = DynamicRot(jci); - var xwcP = posecP; - var comc = BodyCom(jcbi); - var rc = xwcP-(MulQPPoint(posecP, posecQ, comc)); // child moment arm - var jf = JointForce(ji); - var jtq = JointTorque(ji); - var f: vec3; - var t: vec3; - switch (jt) { - case Free, Distance: { - f = jf; - t = jtq; - } - case Ball: { - t = jtq; - } - case Revolute, Prismatic: { - var axis = JointAxis(ji); - var ap = MulQuatVector(xwpQ, axis); - f = f+(MulScalar3(ap, jf.x)); - } - default: { - } - } - SetJointPForce(ji, f); - SetJointCForce(ji, f); - SetJointPTorque(ji, t+(Cross3(rp, f))); - SetJointCTorque(ji, t+(Cross3(rc, f))); -} \ No newline at end of file diff --git a/physics/step.go b/physics/step.go index 3c31f7d5..351d28fe 100644 --- a/physics/step.go +++ b/physics/step.go @@ -4,12 +4,10 @@ // Use of this source code is governed by a BSD-style // license that can be found in the LICENSE file. -package physics +// This code is adapted directly from https://github.com/newton-physics/newton +// Copyright (c) 2025 The Newton Developers, Released under an Apache-2.0 license -import ( - "cogentcore.org/core/math32" - "cogentcore.org/lab/gosl/slmath" -) +package physics //gosl:start //gosl:import "cogentcore.org/lab/gosl/slmath" @@ -21,28 +19,6 @@ func OneIfNonzero(f float32) float32 { return 0.0 } -// InitDynamics copies Body initial state to dynamic state. -func InitDynamics(i uint32) { //gosl:kernel - pars := GetParams(0) - ii := int32(i) - if ii >= pars.DynamicsN { - return - } - bi := DynamicIndex(ii) - Dynamics.Set(Bodies.Value(int(bi), int(BodyPosX)), int(ii), int(DynPosX)) - Dynamics.Set(Bodies.Value(int(bi), int(BodyPosY)), int(ii), int(DynPosY)) - Dynamics.Set(Bodies.Value(int(bi), int(BodyPosZ)), int(ii), int(DynPosZ)) - - Dynamics.Set(Bodies.Value(int(bi), int(BodyRotX)), int(ii), int(DynRotX)) - Dynamics.Set(Bodies.Value(int(bi), int(BodyRotY)), int(ii), int(DynRotY)) - Dynamics.Set(Bodies.Value(int(bi), int(BodyRotZ)), int(ii), int(DynRotZ)) - Dynamics.Set(Bodies.Value(int(bi), int(BodyRotW)), int(ii), int(DynRotW)) - - for v := DynVelX; v < DynamicVarsN; v++ { - Dynamics.Set(0.0, int(ii), int(v)) - } -} - // step does the following: // if self.compute_body_velocity_from_position_delta or self.enable_restitution: // body_q_init = wp.clone(state_in.body_q) @@ -69,132 +45,9 @@ func InitDynamics(i uint32) { //gosl:kernel // kernel=apply_rigid_restitution, // kernel=apply_body_delta_velocities, -// StepJoints does joint-based update. -func StepJoints(i uint32) { //gosl:kernel - pars := GetParams(0) - ji := int32(i) - if ji >= pars.JointsN { - return - } - // todo: enabled - jpi := JointParentIndex(ji) - jpbi := DynamicIndex(jpi) - jci := JointChildIndex(ji) - jcbi := DynamicIndex(jci) - jt := GetJointType(ji) - - jpP := JointPPos(ji) - jpQ := JointPRot(ji) - - // parent world transform - xwpP := jpP - xwpQ := jpQ - posepP := jpP - posepQ := jpQ - var comp math32.Vector3 - - if jpi >= 0 { // can be fixed - posepP = DynamicPos(jpi) - posepQ = DynamicRot(jpi) - slmath.MulQPTransforms(posepP, posepQ, jpP, jpQ, &xwpP, &xwpQ) - comp = BodyCom(jpbi) - } - rp := xwpP.Sub(slmath.MulQPPoint(posepP, posepQ, comp)) // parent moment arm - - // child world transform - posecP := DynamicPos(jci) - posecQ := DynamicRot(jci) - xwcP := posecP - // xwcQ := posecQ - comc := BodyCom(jcbi) - rc := xwcP.Sub(slmath.MulQPPoint(posecP, posecQ, comc)) // child moment arm - - // from controls: - jf := JointForce(ji) - jtq := JointTorque(ji) - - var f, t math32.Vector3 - switch jt { - case Free, Distance: - // todo: distance doesn't seem to be supported here? - f = jf - t = jtq - case Ball: - t = jtq - case Revolute, Prismatic: - axis := JointAxis(ji) - ap := slmath.MulQuatVector(xwpQ, axis) - f = f.Add(slmath.MulScalar3(ap, jf.X)) - default: - // todo: D6 requires more iteration! - } - SetJointPForce(ji, f) - SetJointCForce(ji, f) - SetJointPTorque(ji, t.Add(slmath.Cross3(rp, f))) - SetJointCTorque(ji, t.Add(slmath.Cross3(rc, f))) -} - -// todo: aggregate forces - -// StepIntegrateBodies -func StepIntegrateBodies(i uint32) { //gosl:kernel - pars := GetParams(0) - di := int32(i) - if di >= pars.DynamicsN { - return - } - bi := DynamicIndex(di) - - invMass := Bodies.Value(int(bi), int(BodyInvMass)) - inertia := BodyInertia(bi) - invInertia := BodyInvInertia(bi) - - com := BodyCom(bi) - - // unpack transform - x0 := DynamicPos(di) - r0 := DynamicRot(di) - - // unpack spatial twist - v0 := DynamicDelta(di) - w0 := DynamicAngDelta(di) - - // unpack spatial wrench - f0 := DynamicForce(di) - t0 := DynamicTorque(di) - - xcom := slmath.MulQuatVector(r0, com).Add(x0) - - // linear part - v1 := v0.Add(f0.MulScalar(invMass).Add(pars.Gravity.V().MulScalar(OneIfNonzero(invMass))).MulScalar(pars.Dt)) - x1 := xcom.Add(v1.MulScalar(pars.Dt)) - - // angular part (compute in body frame) - wb := slmath.MulQuatVectorInverse(r0, w0) - tb := slmath.MulQuatVectorInverse(r0, t0).Sub(slmath.Cross3(wb, inertia.MulVector3(wb))) // coriolis forces - - w1 := slmath.MulQuatVector(r0, wb.Add(invInertia.MulVector3(tb).MulScalar(pars.Dt))) - r1 := slmath.MulQuats(math32.NewQuat(w1.X, w1.Y, w1.Z, 0), r0).MulScalar(0.5 * pars.Dt) - r1 = slmath.QuatNormalize(r1) - - // angular damping - w1 = w1.MulScalar(1.0 - pars.AngularDamping*pars.Dt) - - newP := x1.Sub(slmath.MulQuatVector(r1, com)) // pos - newQ := r1 // rot - - newV := v1 // delta - newW := w1 // angDelta - - // todo: write new values - - // q_new = wp.transform(x1 - wp.quat_rotate(r1, com), r1) - // qd_new = wp.spatial_vector(v1, w1) -} - //gosl:end -func (wl *World) StepJoints() { +func (wl *World) StepJointForces() { pars := GetParams(0) - RunStepJoints(int(pars.JointsN)) + RunStepJointForces(int(pars.JointsN)) } diff --git a/physics/step.goal b/physics/step.goal index 23bffde6..c4b501c9 100644 --- a/physics/step.goal +++ b/physics/step.goal @@ -2,19 +2,14 @@ // Use of this source code is governed by a BSD-style // license that can be found in the LICENSE file. -package physics +// This code is adapted directly from https://github.com/newton-physics/newton +// Copyright (c) 2025 The Newton Developers, Released under an Apache-2.0 license -import ( - "cogentcore.org/core/math32" - "cogentcore.org/lab/gosl/slmath" -) +package physics //gosl:start //gosl:import "cogentcore.org/lab/gosl/slmath" -// todo: Dynamics has an extra 2 index for Cur and Next, -// switch between those. - func OneIfNonzero(f float32) float32 { if f != 0.0 { return 1.0 @@ -22,28 +17,6 @@ func OneIfNonzero(f float32) float32 { return 0.0 } -// InitDynamics copies Body initial state to dynamic state. -func InitDynamics(i uint32) { //gosl:kernel - pars := GetParams(0) - ii := int32(i) - if ii >= pars.DynamicsN { - return - } - bi := DynamicIndex(ii) - Dynamics[ii, DynPosX] = Bodies[bi, BodyPosX] - Dynamics[ii, DynPosY] = Bodies[bi, BodyPosY] - Dynamics[ii, DynPosZ] = Bodies[bi, BodyPosZ] - - Dynamics[ii, DynRotX] = Bodies[bi, BodyRotX] - Dynamics[ii, DynRotY] = Bodies[bi, BodyRotY] - Dynamics[ii, DynRotZ] = Bodies[bi, BodyRotZ] - Dynamics[ii, DynRotW] = Bodies[bi, BodyRotW] - - for v := DynVelX; v < DynamicVarsN; v++ { - Dynamics[ii, v] = 0.0 - } -} - // step does the following: // if self.compute_body_velocity_from_position_delta or self.enable_restitution: // body_q_init = wp.clone(state_in.body_q) @@ -70,132 +43,9 @@ func InitDynamics(i uint32) { //gosl:kernel // kernel=apply_rigid_restitution, // kernel=apply_body_delta_velocities, -// StepJoints does joint-based update. -func StepJoints(i uint32) { //gosl:kernel - pars := GetParams(0) - ji := int32(i) - if ji >= pars.JointsN { - return - } - // todo: enabled - jpi := JointParentIndex(ji) - jpbi := DynamicIndex(jpi) - jci := JointChildIndex(ji) - jcbi := DynamicIndex(jci) - jt := GetJointType(ji) - - jpP := JointPPos(ji) - jpQ := JointPRot(ji) - - // parent world transform - xwpP := jpP - xwpQ := jpQ - posepP := jpP - posepQ := jpQ - var comp math32.Vector3 - - if jpi >= 0 { // can be fixed - posepP = DynamicPos(jpi) - posepQ = DynamicRot(jpi) - slmath.MulQPTransforms(posepP, posepQ, jpP, jpQ, &xwpP, &xwpQ) - comp = BodyCom(jpbi) - } - rp := xwpP.Sub(slmath.MulQPPoint(posepP, posepQ, comp)) // parent moment arm - - // child world transform - posecP := DynamicPos(jci) - posecQ := DynamicRot(jci) - xwcP := posecP - // xwcQ := posecQ - comc := BodyCom(jcbi) - rc := xwcP.Sub(slmath.MulQPPoint(posecP, posecQ, comc)) // child moment arm - - // from controls: - jf := JointForce(ji) - jtq := JointTorque(ji) - - var f, t math32.Vector3 - switch jt { - case Free, Distance: - // todo: distance doesn't seem to be supported here? - f = jf - t = jtq - case Ball: - t = jtq - case Revolute, Prismatic: - axis := JointAxis(ji) - ap := slmath.MulQuatVector(xwpQ, axis) - f = f.Add(slmath.MulScalar3(ap, jf.X)) - default: - // todo: D6 requires more iteration! - } - SetJointPForce(ji, f) - SetJointCForce(ji, f) - SetJointPTorque(ji, t.Add(slmath.Cross3(rp, f))) - SetJointCTorque(ji, t.Add(slmath.Cross3(rc, f))) -} - -// todo: aggregate forces - -// StepIntegrateBodies -func StepIntegrateBodies(i uint32) { //gosl:kernel - pars := GetParams(0) - di := int32(i) - if di >= pars.DynamicsN { - return - } - bi := DynamicIndex(di) - - invMass := Bodies[bi, BodyInvMass] - inertia := BodyInertia(bi) - invInertia := BodyInvInertia(bi) - - com := BodyCom(bi) - - // unpack transform - x0 := DynamicPos(di) - r0 := DynamicRot(di) - - // unpack spatial twist - v0 := DynamicDelta(di) - w0 := DynamicAngDelta(di) - - // unpack spatial wrench - f0 := DynamicForce(di) - t0 := DynamicTorque(di) - - xcom := slmath.MulQuatVector(r0, com).Add(x0) - - // linear part - v1 := v0.Add(f0.MulScalar(invMass).Add(pars.Gravity.V().MulScalar(OneIfNonzero(invMass))).MulScalar(pars.Dt)) - x1 := xcom.Add(v1.MulScalar(pars.Dt)) - - // angular part (compute in body frame) - wb := slmath.MulQuatVectorInverse(r0, w0) - tb := slmath.MulQuatVectorInverse(r0, t0).Sub(slmath.Cross3(wb, inertia.MulVector3(wb))) // coriolis forces - - w1 := slmath.MulQuatVector(r0, wb.Add(invInertia.MulVector3(tb).MulScalar(pars.Dt))) - r1 := slmath.MulQuats(math32.NewQuat(w1.X, w1.Y, w1.Z, 0), r0).MulScalar(0.5 * pars.Dt) - r1 = slmath.QuatNormalize(r1) - - // angular damping - w1 = w1.MulScalar(1.0 - pars.AngularDamping*pars.Dt) - - newP := x1.Sub(slmath.MulQuatVector(r1, com)) // pos - newQ := r1 // rot - - newV := v1 // delta - newW := w1 // angDelta - - // todo: write new values - - // q_new = wp.transform(x1 - wp.quat_rotate(r1, com), r1) - // qd_new = wp.spatial_vector(v1, w1) -} - //gosl:end -func (wl *World) StepJoints() { +func (wl *World) StepJointForces() { pars := GetParams(0) - RunStepJoints(int(pars.JointsN)) + RunStepJointForces(int(pars.JointsN)) } diff --git a/physics/step_body.go b/physics/step_body.go new file mode 100644 index 00000000..7b7740dc --- /dev/null +++ b/physics/step_body.go @@ -0,0 +1,111 @@ +// Code generated by "goal build"; DO NOT EDIT. +//line step_body.goal:1 +// Copyright (c) 2025, Cogent Core. All rights reserved. +// Use of this source code is governed by a BSD-style +// license that can be found in the LICENSE file. + +// This code is adapted directly from https://github.com/newton-physics/newton +// Copyright (c) 2025 The Newton Developers, Released under an Apache-2.0 license + +package physics + +import ( + "cogentcore.org/core/math32" + "cogentcore.org/lab/gosl/slmath" +) + +//gosl:start +//gosl:import "cogentcore.org/lab/gosl/slmath" + +// InitDynamics copies Body initial state to dynamic state (cur and next). +func InitDynamics(i uint32) { //gosl:kernel + pars := GetParams(0) + ii := int32(i) + if ii >= pars.DynamicsN { + return + } + for cni := range 2 { + bi := DynamicIndex(ii, int32(cni)) + Dynamics.Set(Bodies.Value(int(bi), int(BodyPosX)), int(ii), int(cni), int(DynPosX)) + Dynamics.Set(Bodies.Value(int(bi), int(BodyPosY)), int(ii), int(cni), int(DynPosY)) + Dynamics.Set(Bodies.Value(int(bi), int(BodyPosZ)), int(ii), int(cni), int(DynPosZ)) + + Dynamics.Set(Bodies.Value(int(bi), int(BodyRotX)), int(ii), int(cni), int(DynRotX)) + Dynamics.Set(Bodies.Value(int(bi), int(BodyRotY)), int(ii), int(cni), int(DynRotY)) + Dynamics.Set(Bodies.Value(int(bi), int(BodyRotZ)), int(ii), int(cni), int(DynRotZ)) + Dynamics.Set(Bodies.Value(int(bi), int(BodyRotW)), int(ii), int(cni), int(DynRotW)) + + for v := DynVelX; v < DynamicVarsN; v++ { + Dynamics.Set(0.0, int(ii), int(cni), int(v)) + } + } +} + +// DynamicsCurToNext copies [Dynamics] state from Cur to Next. +func DynamicsCurToNext(i uint32) { //gosl:kernel + pars := GetParams(0) + ii := int32(i) + if ii >= pars.DynamicsN { + return + } + for di := DynIndex; di < DynamicVarsN; di++ { + Dynamics.Set(Dynamics.Value(int(ii), int(pars.Cur), int(di)), int(ii), int(pars.Next), int(di)) + } +} + +// todo: aggregate forces + +// StepIntegrateBodies +func StepIntegrateBodies(i uint32) { //gosl:kernel + pars := GetParams(0) + di := int32(i) + if di >= pars.DynamicsN { + return + } + bi := DynamicIndex(di, pars.Cur) + + invMass := Bodies.Value(int(bi), int(BodyInvMass)) + inertia := BodyInertia(bi) + invInertia := BodyInvInertia(bi) + + com := BodyCom(bi) + + // unpack transform + x0 := DynamicPos(di, pars.Cur) + r0 := DynamicRot(di, pars.Cur) + + // unpack spatial twist + v0 := DynamicDelta(di, pars.Cur) + w0 := DynamicAngDelta(di, pars.Cur) + + // unpack spatial wrench + f0 := DynamicForce(di, pars.Next) + t0 := DynamicTorque(di, pars.Next) + + xcom := slmath.MulQuatVector(r0, com).Add(x0) + + // linear part + v1 := v0.Add(f0.MulScalar(invMass).Add(pars.Gravity.V().MulScalar(OneIfNonzero(invMass))).MulScalar(pars.Dt)) + x1 := xcom.Add(v1.MulScalar(pars.Dt)) + + // angular part (compute in body frame) + wb := slmath.MulQuatVectorInverse(r0, w0) + tb := slmath.MulQuatVectorInverse(r0, t0).Sub(slmath.Cross3(wb, inertia.MulVector3(wb))) // coriolis forces + + w1 := slmath.MulQuatVector(r0, wb.Add(invInertia.MulVector3(tb).MulScalar(pars.Dt))) + r1 := slmath.MulQuats(math32.NewQuat(w1.X, w1.Y, w1.Z, 0), r0).MulScalar(0.5 * pars.Dt) + r1 = slmath.QuatNormalize(r1) + + // angular damping + w1 = w1.MulScalar(1.0 - pars.AngularDamping*pars.Dt) + + x1a := x1.Sub(slmath.MulQuatVector(r1, com)) // pos corrected to nominal center. + + SetDynamicPos(di, pars.Next, x1a) + SetDynamicRot(di, pars.Next, r1) + + SetDynamicDelta(di, pars.Next, v1) + SetDynamicAngDelta(di, pars.Next, w1) +} + +//gosl:end diff --git a/physics/step_body.goal b/physics/step_body.goal new file mode 100644 index 00000000..54297b73 --- /dev/null +++ b/physics/step_body.goal @@ -0,0 +1,109 @@ +// Copyright (c) 2025, Cogent Core. All rights reserved. +// Use of this source code is governed by a BSD-style +// license that can be found in the LICENSE file. + +// This code is adapted directly from https://github.com/newton-physics/newton +// Copyright (c) 2025 The Newton Developers, Released under an Apache-2.0 license + +package physics + +import ( + "cogentcore.org/core/math32" + "cogentcore.org/lab/gosl/slmath" +) + +//gosl:start +//gosl:import "cogentcore.org/lab/gosl/slmath" + +// InitDynamics copies Body initial state to dynamic state (cur and next). +func InitDynamics(i uint32) { //gosl:kernel + pars := GetParams(0) + ii := int32(i) + if ii >= pars.DynamicsN { + return + } + for cni := range 2 { + bi := DynamicIndex(ii, int32(cni)) + Dynamics[ii, cni, DynPosX] = Bodies[bi, BodyPosX] + Dynamics[ii, cni, DynPosY] = Bodies[bi, BodyPosY] + Dynamics[ii, cni, DynPosZ] = Bodies[bi, BodyPosZ] + + Dynamics[ii, cni, DynRotX] = Bodies[bi, BodyRotX] + Dynamics[ii, cni, DynRotY] = Bodies[bi, BodyRotY] + Dynamics[ii, cni, DynRotZ] = Bodies[bi, BodyRotZ] + Dynamics[ii, cni, DynRotW] = Bodies[bi, BodyRotW] + + for v := DynVelX; v < DynamicVarsN; v++ { + Dynamics[ii, cni, v] = 0.0 + } + } +} + +// DynamicsCurToNext copies [Dynamics] state from Cur to Next. +func DynamicsCurToNext(i uint32) { //gosl:kernel + pars := GetParams(0) + ii := int32(i) + if ii >= pars.DynamicsN { + return + } + for di := DynIndex; di < DynamicVarsN; di++ { + Dynamics[ii, pars.Next, di] = Dynamics[ii, pars.Cur, di] + } +} + +// todo: aggregate forces + +// StepIntegrateBodies +func StepIntegrateBodies(i uint32) { //gosl:kernel + pars := GetParams(0) + di := int32(i) + if di >= pars.DynamicsN { + return + } + bi := DynamicIndex(di, pars.Cur) + + invMass := Bodies[bi, BodyInvMass] + inertia := BodyInertia(bi) + invInertia := BodyInvInertia(bi) + + com := BodyCom(bi) + + // unpack transform + x0 := DynamicPos(di, pars.Cur) + r0 := DynamicRot(di, pars.Cur) + + // unpack spatial twist + v0 := DynamicDelta(di, pars.Cur) + w0 := DynamicAngDelta(di, pars.Cur) + + // unpack spatial wrench + f0 := DynamicForce(di, pars.Next) + t0 := DynamicTorque(di, pars.Next) + + xcom := slmath.MulQuatVector(r0, com).Add(x0) + + // linear part + v1 := v0.Add(f0.MulScalar(invMass).Add(pars.Gravity.V().MulScalar(OneIfNonzero(invMass))).MulScalar(pars.Dt)) + x1 := xcom.Add(v1.MulScalar(pars.Dt)) + + // angular part (compute in body frame) + wb := slmath.MulQuatVectorInverse(r0, w0) + tb := slmath.MulQuatVectorInverse(r0, t0).Sub(slmath.Cross3(wb, inertia.MulVector3(wb))) // coriolis forces + + w1 := slmath.MulQuatVector(r0, wb.Add(invInertia.MulVector3(tb).MulScalar(pars.Dt))) + r1 := slmath.MulQuats(math32.NewQuat(w1.X, w1.Y, w1.Z, 0), r0).MulScalar(0.5 * pars.Dt) + r1 = slmath.QuatNormalize(r1) + + // angular damping + w1 = w1.MulScalar(1.0 - pars.AngularDamping*pars.Dt) + + x1a := x1.Sub(slmath.MulQuatVector(r1, com)) // pos corrected to nominal center. + + SetDynamicPos(di, pars.Next, x1a) + SetDynamicRot(di, pars.Next, r1) + + SetDynamicDelta(di, pars.Next, v1) + SetDynamicAngDelta(di, pars.Next, w1) +} + +//gosl:end diff --git a/physics/step_joint.go b/physics/step_joint.go new file mode 100644 index 00000000..b6913248 --- /dev/null +++ b/physics/step_joint.go @@ -0,0 +1,588 @@ +// Code generated by "goal build"; DO NOT EDIT. +//line step_joint.goal:1 +// Copyright (c) 2025, Cogent Core. All rights reserved. +// Use of this source code is governed by a BSD-style +// license that can be found in the LICENSE file. + +// This code is adapted directly from https://github.com/newton-physics/newton +// Copyright (c) 2025 The Newton Developers, Released under an Apache-2.0 license + +package physics + +import ( + "cogentcore.org/core/math32" + "cogentcore.org/lab/gosl/slmath" +) + +//gosl:start +//gosl:import "cogentcore.org/lab/gosl/slmath" + +// StepJointForces computes joint forces. +func StepJointForces(i uint32) { //gosl:kernel + pars := GetParams(0) + ji := int32(i) + if ji >= pars.JointsN { + return + } + // todo: enabled + jpi := JointParentIndex(ji) + jpbi := DynamicIndex(jpi, pars.Cur) + jci := JointChildIndex(ji) + jcbi := DynamicIndex(jci, pars.Cur) + jt := GetJointType(ji) + + jpP := JointPPos(ji) + jpQ := JointPRot(ji) + + // parent world transform + xwpP := jpP + xwpQ := jpQ + posepP := jpP + posepQ := jpQ + var comp math32.Vector3 + + if jpi >= 0 { // can be fixed + posepP = DynamicPos(jpi, pars.Cur) + posepQ = DynamicRot(jpi, pars.Cur) + slmath.MulQPTransforms(posepP, posepQ, jpP, jpQ, &xwpP, &xwpQ) + comp = BodyCom(jpbi) + } + rp := xwpP.Sub(slmath.MulQPPoint(posepP, posepQ, comp)) // parent moment arm + + // child world transform + posecP := DynamicPos(jci, pars.Cur) + posecQ := DynamicRot(jci, pars.Cur) + xwcP := posecP + // xwcQ := posecQ + comc := BodyCom(jcbi) + rc := xwcP.Sub(slmath.MulQPPoint(posecP, posecQ, comc)) // child moment arm + + // from controls: + jf := JointControlForce(ji) + jtq := JointControlTorque(ji) + + var f, t math32.Vector3 + switch jt { + case Free, Distance: + // todo: distance doesn't seem to be supported here? + f = jf + t = jtq + case Ball: + t = jtq + case Revolute, Prismatic: + axis := JointAxis(ji) + ap := slmath.MulQuatVector(xwpQ, axis) + f = f.Add(slmath.MulScalar3(ap, jf.X)) + default: + // todo: D6 requires more iteration! + } + SetJointPForce(ji, f) + SetJointCForce(ji, f) + SetJointPTorque(ji, t.Add(slmath.Cross3(rp, f))) + SetJointCTorque(ji, t.Add(slmath.Cross3(rc, f))) +} + +// StepSolveJoints fixes joints after updating bodies. +func StepSolveJoints(i uint32) { //gosl:kernel + pars := GetParams(0) + ji := int32(i) + if ji >= pars.JointsN { + return + } + + // todo: enabled + jpi := JointParentIndex(ji) + jpbi := DynamicIndex(jpi, pars.Cur) + jci := JointChildIndex(ji) + jcbi := DynamicIndex(jci, pars.Cur) + jt := GetJointType(ji) + + if jt == Free { + return + } + + jpP := JointPPos(ji) + jpQ := JointPRot(ji) + xwpP := jpP // world xform, parent, pos + xwpQ := jpQ // quat + mInvp := float32(0.0) + iInvp := math32.Mat3(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0) + posepP := jpP + posepQ := jpQ + + var comp, velp, omegap math32.Vector3 + + // parent transform and moment arm + if jpi >= 0 { + posepP = DynamicPos(jpi, pars.Next) // now using next + posepQ = DynamicRot(jpi, pars.Next) + slmath.MulQPTransforms(posepP, posepQ, jpP, jpQ, &xwpP, &xwpQ) + comp = BodyCom(jpbi) + mInvp = Bodies.Value(int(jpbi), int(BodyInvMass)) + iInvp = BodyInvInertia(jpbi) + velp = DynamicDelta(jpi, pars.Next) + omegap = DynamicAngDelta(jpi, pars.Next) + } + + // child transform and moment arm + posecP := DynamicPos(jci, pars.Next) + posecQ := DynamicRot(jci, pars.Next) + jcP := JointCPos(ji) + jcQ := JointCRot(ji) + xwcP := jcP + xwcQ := jcQ + slmath.MulQPTransforms(posecP, posecQ, jcP, jcQ, &xwcP, &xwcQ) + comc := BodyCom(jcbi) + mInvc := Bodies.Value(int(jcbi), int(BodyInvMass)) + iInvc := BodyInvInertia(jcbi) + velc := DynamicDelta(jci, pars.Next) + omegac := DynamicAngDelta(jci, pars.Next) + + if mInvp == 0.0 && mInvc == 0.0 { // connection between two immovable bodies + return + } + + // accumulate constraint deltas + var linDeltaP, angDeltaP, linDeltaC, angDeltaC math32.Vector3 + + relPoseP := xwpP + relPoseQ := xwpQ + slmath.QPTransformInverse(xwpP, xwpQ, &relPoseP, &relPoseQ) + slmath.MulQPTransforms(relPoseP, relPoseQ, xwcP, xwcQ, &relPoseP, &relPoseQ) + + // joint connection points + xc := xwcP + + // axis_start = joint_qd_start[tid] + // lin_axis_count = joint_dof_dim[tid, 0] + // ang_axis_count = joint_dof_dim[tid, 1] + + worldComp := slmath.MulQPPoint(posepP, posepQ, comp) + worldComc := slmath.MulQPPoint(posecP, posecQ, comc) + _ = worldComc + + // handle positional constraints + if jt == Distance { + // r_p = wp.transform_get_translation(X_wp) - world_com_p + // r_c = wp.transform_get_translation(X_wc) - world_com_c + // lower = joint_limit_lower[axis_start] + // upper = joint_limit_upper[axis_start] + // if lower < 0.0 and upper < 0.0: + // + // # no limits + // return + // + // d = wp.length(rel_p) + // err = 0.0 + // if lower >= 0.0 and d < lower: + // + // err = d - lower + // # use a more descriptive direction vector for the constraint + // # in case the joint parent and child anchors are very close + // rel_p = err * wp.normalize(world_com_c - world_com_p) + // + // elif upper >= 0.0 and d > upper: + // + // err = d - upper + // + // if wp.abs(err) > 1e-9: + // + // # compute gradients + // linear_c = rel_p + // linear_p = -linear_c + // r_c = x_c - world_com_c + // angular_p = -wp.cross(r_p, linear_c) + // angular_c = wp.cross(r_c, linear_c) + // # constraint time derivative + // derr = ( + // wp.dot(linear_p, vel_p) + // + wp.dot(linear_c, vel_c) + // + wp.dot(angular_p, omega_p) + // + wp.dot(angular_c, omega_c) + // ) + // lambda_in = 0.0 + // compliance = linear_compliance + // ke = joint_target_ke[axis_start] + // if ke > 0.0: + // compliance = 1.0 / ke + // damping = joint_target_kd[axis_start] + // d_lambda = compute_positional_correction( + // err, + // derr, + // pose_p, + // pose_c, + // m_inv_p, + // m_inv_c, + // I_inv_p, + // I_inv_c, + // linear_p, + // linear_c, + // angular_p, + // angular_c, + // lambda_in, + // compliance, + // damping, + // dt, + // ) + // + // linDelta_p += linear_p * (d_lambda * pars.JointLinearRelax) + // angDelta_p += angular_p * (d_lambda * angular_relaxation) + // linDelta_c += linear_c * (d_lambda * pars.JointLinearRelax) + // angDelta_c += angular_c * (d_lambda * angular_relaxation) + } else { // compute joint target, stiffness, damping + var axisLimitsD, axisLimitsA math32.Vector3 + var axisTargetPosKeD, axisTargetPosKeA math32.Vector3 + var axisTargetVelKdD, axisTargetVelKdA math32.Vector3 + + // axis_target_pos_ke = wp.spatial_vector() + // axis_target_vel_kd = wp.spatial_vector() + // + // avoid a for loop here since local variables would need to be modified + // which is not yet differentiable + axis := JointAxis(ji) + loTemp := axis.MulScalar(Joints.Value(int(ji), int(JointLimitLower))) + upTemp := axis.MulScalar(Joints.Value(int(ji), int(JointLimitUpper))) + axisLimitsD = slmath.Min3(loTemp, upTemp) + axisLimitsA = slmath.Max3(loTemp, upTemp) + ke := JointStiff(ji) + kd := JointDamp(ji) + targetPos := JointTargetPos(ji) + targetVel := JointTargetVel(ji) + if ke.X > 0.0 { // has position control + UpdateJointAxisWeightedTarget(axis, targetPos.X, ke.X, &axisTargetPosKeD, &axisTargetPosKeA) + } + if kd.X > 0.0 { // has velocity control + UpdateJointAxisWeightedTarget(axis, targetVel.X, kd.X, &axisTargetVelKdD, &axisTargetVelKdA) + } + // if lin_axis_count > 1: + // axis_idx = axis_start + 1 + // axis = joint_axis[axis_idx] + // lower = joint_limit_lower[axis_idx] + // upper = joint_limit_upper[axis_idx] + // axis_limits = update_joint_axis_limits(axis, lower, upper, axis_limits) + // ke = joint_target_ke[axis_idx] + // kd = joint_target_kd[axis_idx] + // target_pos = joint_target_pos[axis_idx] + // target_vel = joint_target_vel[axis_idx] + // if ke > 0.0: # has position control + // axis_target_pos_ke = update_joint_axis_weighted_target(axis, target_pos, ke, axis_target_pos_ke) + // if kd > 0.0: # has velocity control + // axis_target_vel_kd = update_joint_axis_weighted_target(axis, target_vel, kd, axis_target_vel_kd) + // if lin_axis_count > 2: + // axis_idx = axis_start + 2 + // axis = joint_axis[axis_idx] + // lower = joint_limit_lower[axis_idx] + // upper = joint_limit_upper[axis_idx] + // axis_limits = update_joint_axis_limits(axis, lower, upper, axis_limits) + // ke = joint_target_ke[axis_idx] + // kd = joint_target_kd[axis_idx] + // target_pos = joint_target_pos[axis_idx] + // target_vel = joint_target_vel[axis_idx] + // if ke > 0.0: # has position control + // axis_target_pos_ke = update_joint_axis_weighted_target(axis, target_pos, ke, axis_target_pos_ke) + // if kd > 0.0: # has velocity control + // axis_target_vel_kd = update_joint_axis_weighted_target(axis, target_vel, kd, axis_target_vel_kd) + + axisStiffness := axisTargetPosKeA + axisDamping := axisTargetVelKdA + for i := range 3 { + if axisStiffness.X > 0.0 { // todo: Dim(i) access + axisTargetPosKeD.X /= axisStiffness.X + } + } + for i := range 3 { + if axisDamping.X > 0.0 { // todo Dim + axisTargetVelKdD.X /= axisDamping.X + } + } + axisLimitsLower := axisLimitsD + axisLimitsUpper := axisLimitsA + // todo: + // + // frame_p = wp.quat_to_matrix(wp.transform_get_rotation(X_wp)) + // + // note that xc appearing in both is correct: + rp := xc.Sub(worldComp) + rc := xc.Sub(slmath.MulQPPoint(posecP, posecQ, comc)) + + // for loop will be unrolled, so we can modify local variables + for dim := range 3 { + e := relPoseP.X // rel_p[dim] + + // compute gradients + linearc := math32.Vec3(frame_p.Value(int(0), int(dim)), frame_p.Value(int(1), int(dim)), frame_p.Value(int(2), int(dim))) + linearp := slmath.Negate3(linearc) + angularp := slmath.Cross3(rp, linearc) + angularc := slmath.Cross3(rc, linearc) + // constraint time derivative + derr := slmath.Dot3(linearp, velp) + slmath.Dot3(linearc, velc) + slmath.Dot3(angularp, omegap) + slmath.Dot3(angularc, omegac) + + err := float32(0.0) + compliance := pars.JointLinearComply + damping := float32(0.0) + + targetVel := axisTargetVelKdD.X // [dim] + derrRel := derr - targetVel + + // consider joint limits irrespective of axis mode + lower := axisLimitsLower.X // [dim] + upper := axisLimitsUpper.X // [dim] + if e < lower { + err = e - lower + } else if e > upper { + err = e - upper + } else { + targetPos := axisTargetPosKeD.X // [dim] + targetPos = math32.Clamp(targetPos, lower, upper) + + if axisStiffness.X > 0.0 { + err = e - targetPos + compliance = 1.0 / axisStiffness.X // [dim] + damping = axisDamping.X // [dim] + } else if axisDamping.X > 0.0 { + compliance = 1.0 / axisDamping.X // [dim] + damping = axisDamping.X // [dim] + } + } + if math32.Abs(err) > 1e-9 || math32.Abs(derrRel) > 1e-9 { + lambdaIn := float32(0.0) + dLambda := PositionalCorrection(err, derrRel, posepQ, posecQ, mInvp, mInvc, + iInvp, iInvc, linearp, linearc, angularp, angularc, lambdaIn, compliance, damping, pars.Dt) + + linDeltaP = linDeltaP.Add(linearp.MulScalar(dLambda * pars.JointLinearRelax)) + angDeltaP = angDeltaP.Add(angularp.MulScalar(dLambda * pars.JointAngularRelax)) + linDeltaC = linDeltaC.Add(linearc.MulScalar(dLambda * pars.JointLinearRelax)) + angDeltaC = angDeltaC.Add(angularc.MulScalar(dLambda * pars.JointAngularRelax)) + } + } + } + // + // if type == JointType.FIXED or type == JointType.PRISMATIC or type == JointType.REVOLUTE or type == JointType.D6: + // # handle angular constraints + // + // # local joint rotations + // q_p = wp.transform_get_rotation(X_wp) + // q_c = wp.transform_get_rotation(X_wc) + // + // # make quats lie in same hemisphere + // if slmath.Dot3(q_p, q_c) < 0.0: + // q_c *= -1.0 + // + // rel_q = wp.quat_inverse(q_p) * q_c + // + // qtwist = wp.normalize(wp.quat(rel_q[0], 0.0, 0.0, rel_q[3])) + // qswing = rel_q * wp.quat_inverse(qtwist) + // + // # decompose to a compound rotation each axis + // s = wp.sqrt(rel_q[0] * rel_q[0] + rel_q[3] * rel_q[3]) + // invs = 1.0 / s + // invscube = invs * invs * invs + // + // # handle axis-angle joints + // + // # rescale twist from quaternion space to angular + // err_0 = 2.0 * wp.asin(wp.clamp(qtwist[0], -1.0, 1.0)) + // err_1 = qswing[1] + // err_2 = qswing[2] + // # analytic gradients of swing-twist decomposition + // grad_0 = wp.quat(invs - rel_q[0] * rel_q[0] * invscube, 0.0, 0.0, -(rel_q[3] * rel_q[0]) * invscube) + // grad_1 = wp.quat( + // -rel_q[3] * (rel_q[3] * rel_q[2] + rel_q[0] * rel_q[1]) * invscube, + // rel_q[3] * invs, + // -rel_q[0] * invs, + // rel_q[0] * (rel_q[3] * rel_q[2] + rel_q[0] * rel_q[1]) * invscube, + // ) + // grad_2 = wp.quat( + // rel_q[3] * (rel_q[3] * rel_q[1] - rel_q[0] * rel_q[2]) * invscube, + // rel_q[0] * invs, + // rel_q[3] * invs, + // rel_q[0] * (rel_q[2] * rel_q[0] - rel_q[3] * rel_q[1]) * invscube, + // ) + // grad_0 *= 2.0 / wp.abs(qtwist[3]) + // # grad_0 *= 2.0 / wp.sqrt(1.0-qtwist[0]*qtwist[0]) # derivative of asin(x) = 1/sqrt(1-x^2) + // + // # rescale swing + // swing_sq = qswing[3] * qswing[3] + // # if swing axis magnitude close to zero vector, just treat in quaternion space + // angularEps = 1.0e-4 + // if swing_sq + angularEps < 1.0: + // d = wp.sqrt(1.0 - qswing[3] * qswing[3]) + // theta = 2.0 * wp.acos(wp.clamp(qswing[3], -1.0, 1.0)) + // scale = theta / d + // + // err_1 *= scale + // err_2 *= scale + // + // grad_1 *= scale + // grad_2 *= scale + // + // errs = wp.vec3(err_0, err_1, err_2) + // grad_x = wp.vec3(grad_0[0], grad_1[0], grad_2[0]) + // grad_y = wp.vec3(grad_0[1], grad_1[1], grad_2[1]) + // grad_z = wp.vec3(grad_0[2], grad_1[2], grad_2[2]) + // grad_w = wp.vec3(grad_0[3], grad_1[3], grad_2[3]) + // + // # compute joint target, stiffness, damping + // axis_limits = wp.spatial_vector(0.0, 0.0, 0.0, 0.0, 0.0, 0.0) + // + // axis_target_pos_ke = wp.spatial_vector() # [weighted_target_pos, ke_weights] + // axis_target_vel_kd = wp.spatial_vector() # [weighted_target_vel, kd_weights] + // # avoid a for loop here since local variables would need to be modified which is not yet differentiable + // if ang_axis_count > 0: + // axis_idx = axis_start + lin_axis_count + // axis = joint_axis[axis_idx] + // lo_temp = axis * joint_limit_lower[axis_idx] + // up_temp = axis * joint_limit_upper[axis_idx] + // axis_limits = wp.spatial_vector(vec_min(lo_temp, up_temp), vec_max(lo_temp, up_temp)) + // ke = joint_target_ke[axis_idx] + // kd = joint_target_kd[axis_idx] + // target_pos = joint_target_pos[axis_idx] + // target_vel = joint_target_vel[axis_idx] + // if ke > 0.0: # has position control + // axis_target_pos_ke = update_joint_axis_weighted_target(axis, target_pos, ke, axis_target_pos_ke) + // if kd > 0.0: # has velocity control + // axis_target_vel_kd = update_joint_axis_weighted_target(axis, target_vel, kd, axis_target_vel_kd) + + // if ang_axis_count > 1: + // axis_idx = axis_start + lin_axis_count + 1 + // axis = joint_axis[axis_idx] + // lower = joint_limit_lower[axis_idx] + // upper = joint_limit_upper[axis_idx] + // axis_limits = update_joint_axis_limits(axis, lower, upper, axis_limits) + // ke = joint_target_ke[axis_idx] + // kd = joint_target_kd[axis_idx] + // target_pos = joint_target_pos[axis_idx] + // target_vel = joint_target_vel[axis_idx] + // if ke > 0.0: # has position control + // axis_target_pos_ke = update_joint_axis_weighted_target(axis, target_pos, ke, axis_target_pos_ke) + // if kd > 0.0: # has velocity control + // axis_target_vel_kd = update_joint_axis_weighted_target(axis, target_vel, kd, axis_target_vel_kd) + // if ang_axis_count > 2: + // axis_idx = axis_start + lin_axis_count + 2 + // axis = joint_axis[axis_idx] + // lower = joint_limit_lower[axis_idx] + // upper = joint_limit_upper[axis_idx] + // axis_limits = update_joint_axis_limits(axis, lower, upper, axis_limits) + // ke = joint_target_ke[axis_idx] + // kd = joint_target_kd[axis_idx] + // target_pos = joint_target_pos[axis_idx] + // target_vel = joint_target_vel[axis_idx] + // if ke > 0.0: # has position control + // axis_target_pos_ke = update_joint_axis_weighted_target(axis, target_pos, ke, axis_target_pos_ke) + // if kd > 0.0: # has velocity control + // axis_target_vel_kd = update_joint_axis_weighted_target(axis, target_vel, kd, axis_target_vel_kd) + // + + // axis_target_pos = wp.spatial_top(axis_target_pos_ke) + // axis_stiffness = wp.spatial_bottom(axis_target_pos_ke) + // axis_target_vel = wp.spatial_top(axis_target_vel_kd) + // axis_damping = wp.spatial_bottom(axis_target_vel_kd) + // for i in range(3): + // if axis_stiffness[i] > 0.0: + // axis_target_pos[i] /= axis_stiffness[i] + // for i in range(3): + // if axis_damping[i] > 0.0: + // axis_target_vel[i] /= axis_damping[i] + // axis_limits_lower = wp.spatial_top(axis_limits) + // axis_limits_upper = wp.spatial_bottom(axis_limits) + + // # if type == JointType.D6: + // # wp.printf("axis_target: %f %f %f\t axis_stiffness: %f %f %f\t axis_damping: %f %f %f\t axis_limits_lower: %f %f %f \t axis_limits_upper: %f %f %f\n", + // # axis_target[0], axis_target[1], axis_target[2], + // # axis_stiffness[0], axis_stiffness[1], axis_stiffness[2], + // # axis_damping[0], axis_damping[1], axis_damping[2], + // # axis_limits_lower[0], axis_limits_lower[1], axis_limits_lower[2], + // # axis_limits_upper[0], axis_limits_upper[1], axis_limits_upper[2]) + // # # wp.printf("wp.sqrt(1.0-qtwist[0]*qtwist[0]) = %f\n", wp.sqrt(1.0-qtwist[0]*qtwist[0])) + + // for dim in range(3): + // e = errs[dim] + // + // # analytic gradients of swing-twist decomposition + // grad = wp.quat(grad_x[dim], grad_y[dim], grad_z[dim], grad_w[dim]) + // + // quat_c = 0.5 * q_p * grad * wp.quat_inverse(q_c) + // angular_c = wp.vec3(quat_c[0], quat_c[1], quat_c[2]) + // angular_p = -angular_c + // # time derivative of the constraint + // derr = slmath.Dot3(angular_p, omega_p) + slmath.Dot3(angular_c, omega_c) + // + // err = 0.0 + // compliance = angular_compliance + // damping = 0.0 + // + // target_vel = axis_target_vel[dim] + // derr_rel = derr - target_vel + // + // # consider joint limits irrespective of mode + // lower = axis_limits_lower[dim] + // upper = axis_limits_upper[dim] + // if e < lower: + // err = e - lower + // elif e > upper: + // err = e - upper + // else: + // target_pos = axis_target_pos[dim] + // target_pos = wp.clamp(target_pos, lower, upper) + // + // if axis_stiffness[dim] > 0.0: + // err = e - target_pos + // compliance = 1.0 / axis_stiffness[dim] + // damping = axis_damping[dim] + // elif axis_damping[dim] > 0.0: + // damping = axis_damping[dim] + // compliance = 1.0 / axis_damping[dim] + // + // d_lambda = ( + // compute_angular_correction( + // err, derr_rel, pose_p, pose_c, I_inv_p, I_inv_c, angular_p, angular_c, 0.0, compliance, damping, dt + // ) + // * angular_relaxation + // ) + // + // # update deltas + // angDelta_p += angular_p * d_lambda + // angDelta_c += angular_c * d_lambda + + // if id_p >= 0: + // + // wp.atomic_add(deltas, id_p, wp.spatial_vector(linDelta_p, angDelta_p)) + // + // if id_c >= 0: + // + // wp.atomic_add(deltas, id_c, wp.spatial_vector(linDelta_c, angDelta_c)) +} + +func UpdateJointAxisWeightedTarget(axis math32.Vector3, target, weight float32, axisTargets, axisWeights *math32.Vector3) { + weightedAxis := axis.MulScalar(weight) + *axisTargets = axisTargets.Add(weightedAxis.MulScalar(target)) // weighted target (to be normalized later by sum of weights) + *axisWeights = axisWeights.Add(slmath.Abs3(weightedAxis)) +} + +func PositionalCorrection(err, derr float32, tfaQ, tfbQ math32.Quat, mInva, mInvb float32, Iinva, Iinvb math32.Matrix3, lineara, linearb, angulara, angularb math32.Vector3, lambdaIn, compliance, damping, dt float32) float32 { + denom := float32(0.0) + denom += slmath.LengthSquared3(lineara) * mInva + denom += slmath.LengthSquared3(linearb) * mInvb + + q1 := tfaQ + q2 := tfbQ + + // # Eq. 2-3 (make sure to project into the frame of the body) + rotAngulara := slmath.MulQuatVectorInverse(q1, angulara) + rotAngularb := slmath.MulQuatVectorInverse(q2, angularb) + + denom += slmath.Dot3(rotAngulara, Iinva.MulVector3(rotAngulara)) + denom += slmath.Dot3(rotAngularb, Iinvb.MulVector3(rotAngularb)) + + alpha := compliance + gamma := compliance * damping + + deltaLambda := -(err + alpha*lambdaIn + gamma*derr) + if denom+alpha > 0.0 { + deltaLambda /= (dt+gamma)*denom + alpha/dt + } + + return deltaLambda +} + +//gosl:end diff --git a/physics/step_joint.goal b/physics/step_joint.goal new file mode 100644 index 00000000..7edf0aa2 --- /dev/null +++ b/physics/step_joint.goal @@ -0,0 +1,575 @@ +// Copyright (c) 2025, Cogent Core. All rights reserved. +// Use of this source code is governed by a BSD-style +// license that can be found in the LICENSE file. + +// This code is adapted directly from https://github.com/newton-physics/newton +// Copyright (c) 2025 The Newton Developers, Released under an Apache-2.0 license + +package physics + +import ( + "cogentcore.org/core/math32" + "cogentcore.org/lab/gosl/slmath" +) + +//gosl:start +//gosl:import "cogentcore.org/lab/gosl/slmath" + +// StepJointForces computes joint forces. +func StepJointForces(i uint32) { //gosl:kernel + pars := GetParams(0) + ji := int32(i) + if ji >= pars.JointsN { + return + } + // todo: enabled + jpi := JointParentIndex(ji) + jpbi := DynamicIndex(jpi, pars.Cur) + jci := JointChildIndex(ji) + jcbi := DynamicIndex(jci, pars.Cur) + jt := GetJointType(ji) + + jpP := JointPPos(ji) + jpQ := JointPRot(ji) + + // parent world transform + xwpP := jpP + xwpQ := jpQ + posepP := jpP + posepQ := jpQ + var comp math32.Vector3 + + if jpi >= 0 { // can be fixed + posepP = DynamicPos(jpi, pars.Cur) + posepQ = DynamicRot(jpi, pars.Cur) + slmath.MulQPTransforms(posepP, posepQ, jpP, jpQ, &xwpP, &xwpQ) + comp = BodyCom(jpbi) + } + rp := xwpP.Sub(slmath.MulQPPoint(posepP, posepQ, comp)) // parent moment arm + + // child world transform + posecP := DynamicPos(jci, pars.Cur) + posecQ := DynamicRot(jci, pars.Cur) + xwcP := posecP + // xwcQ := posecQ + comc := BodyCom(jcbi) + rc := xwcP.Sub(slmath.MulQPPoint(posecP, posecQ, comc)) // child moment arm + + // from controls: + jf := JointControlForce(ji) + jtq := JointControlTorque(ji) + + var f, t math32.Vector3 + switch jt { + case Free, Distance: + // todo: distance doesn't seem to be supported here? + f = jf + t = jtq + case Ball: + t = jtq + case Revolute, Prismatic: + axis := JointAxis(ji) + ap := slmath.MulQuatVector(xwpQ, axis) + f = f.Add(slmath.MulScalar3(ap, jf.X)) + default: + // todo: D6 requires more iteration! + } + SetJointPForce(ji, f) + SetJointCForce(ji, f) + SetJointPTorque(ji, t.Add(slmath.Cross3(rp, f))) + SetJointCTorque(ji, t.Add(slmath.Cross3(rc, f))) +} + +// StepSolveJoints fixes joints after updating bodies. +func StepSolveJoints(i uint32) { //gosl:kernel + pars := GetParams(0) + ji := int32(i) + if ji >= pars.JointsN { + return + } + + // todo: enabled + jpi := JointParentIndex(ji) + jpbi := DynamicIndex(jpi, pars.Cur) + jci := JointChildIndex(ji) + jcbi := DynamicIndex(jci, pars.Cur) + jt := GetJointType(ji) + + if jt == Free { + return + } + + jpP := JointPPos(ji) + jpQ := JointPRot(ji) + xwpP := jpP // world xform, parent, pos + xwpQ := jpQ // quat + mInvp := float32(0.0) + iInvp := math32.Mat3(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0) + posepP := jpP + posepQ := jpQ + + var comp, velp, omegap math32.Vector3 + + // parent transform and moment arm + if jpi >= 0 { + posepP = DynamicPos(jpi, pars.Next) // now using next + posepQ = DynamicRot(jpi, pars.Next) + slmath.MulQPTransforms(posepP, posepQ, jpP, jpQ, &xwpP, &xwpQ) + comp = BodyCom(jpbi) + mInvp = Bodies[jpbi, BodyInvMass] + iInvp = BodyInvInertia(jpbi) + velp = DynamicDelta(jpi, pars.Next) + omegap = DynamicAngDelta(jpi, pars.Next) + } + + // child transform and moment arm + posecP := DynamicPos(jci, pars.Next) + posecQ := DynamicRot(jci, pars.Next) + jcP := JointCPos(ji) + jcQ := JointCRot(ji) + xwcP := jcP + xwcQ := jcQ + slmath.MulQPTransforms(posecP, posecQ, jcP, jcQ, &xwcP, &xwcQ) + comc := BodyCom(jcbi) + mInvc := Bodies[jcbi, BodyInvMass] + iInvc := BodyInvInertia(jcbi) + velc := DynamicDelta(jci, pars.Next) + omegac := DynamicAngDelta(jci, pars.Next) + + if mInvp == 0.0 && mInvc == 0.0 { // connection between two immovable bodies + return + } + + // accumulate constraint deltas + var linDeltaP, angDeltaP, linDeltaC, angDeltaC math32.Vector3 + + relPoseP := xwpP + relPoseQ := xwpQ + slmath.QPTransformInverse(xwpP, xwpQ, &relPoseP, &relPoseQ) + slmath.MulQPTransforms(relPoseP, relPoseQ, xwcP, xwcQ, &relPoseP, &relPoseQ) + + // joint connection points + xc := xwcP + + // axis_start = joint_qd_start[tid] + // lin_axis_count = joint_dof_dim[tid, 0] + // ang_axis_count = joint_dof_dim[tid, 1] + + worldComp := slmath.MulQPPoint(posepP, posepQ, comp) + worldComc := slmath.MulQPPoint(posecP, posecQ, comc) + _ = worldComc + + // handle positional constraints + if jt == Distance { + // r_p = wp.transform_get_translation(X_wp) - world_com_p + // r_c = wp.transform_get_translation(X_wc) - world_com_c + // lower = joint_limit_lower[axis_start] + // upper = joint_limit_upper[axis_start] + // if lower < 0.0 and upper < 0.0: + // # no limits + // return + // d = wp.length(rel_p) + // err = 0.0 + // if lower >= 0.0 and d < lower: + // err = d - lower + // # use a more descriptive direction vector for the constraint + // # in case the joint parent and child anchors are very close + // rel_p = err * wp.normalize(world_com_c - world_com_p) + // elif upper >= 0.0 and d > upper: + // err = d - upper + // + // if wp.abs(err) > 1e-9: + // # compute gradients + // linear_c = rel_p + // linear_p = -linear_c + // r_c = x_c - world_com_c + // angular_p = -wp.cross(r_p, linear_c) + // angular_c = wp.cross(r_c, linear_c) + // # constraint time derivative + // derr = ( + // wp.dot(linear_p, vel_p) + // + wp.dot(linear_c, vel_c) + // + wp.dot(angular_p, omega_p) + // + wp.dot(angular_c, omega_c) + // ) + // lambda_in = 0.0 + // compliance = linear_compliance + // ke = joint_target_ke[axis_start] + // if ke > 0.0: + // compliance = 1.0 / ke + // damping = joint_target_kd[axis_start] + // d_lambda = compute_positional_correction( + // err, + // derr, + // pose_p, + // pose_c, + // m_inv_p, + // m_inv_c, + // I_inv_p, + // I_inv_c, + // linear_p, + // linear_c, + // angular_p, + // angular_c, + // lambda_in, + // compliance, + // damping, + // dt, + // ) + // + // linDelta_p += linear_p * (d_lambda * pars.JointLinearRelax) + // angDelta_p += angular_p * (d_lambda * angular_relaxation) + // linDelta_c += linear_c * (d_lambda * pars.JointLinearRelax) + // angDelta_c += angular_c * (d_lambda * angular_relaxation) + // + } else { // compute joint target, stiffness, damping + var axisLimitsD, axisLimitsA math32.Vector3 + var axisTargetPosKeD, axisTargetPosKeA math32.Vector3 + var axisTargetVelKdD, axisTargetVelKdA math32.Vector3 + + // axis_target_pos_ke = wp.spatial_vector() + // axis_target_vel_kd = wp.spatial_vector() + // avoid a for loop here since local variables would need to be modified + // which is not yet differentiable + axis := JointAxis(ji) + loTemp := axis.MulScalar(Joints[ji, JointLimitLower]) + upTemp := axis.MulScalar(Joints[ji, JointLimitUpper]) + axisLimitsD = slmath.Min3(loTemp, upTemp) + axisLimitsA = slmath.Max3(loTemp, upTemp) + ke := JointStiff(ji) + kd := JointDamp(ji) + targetPos := JointTargetPos(ji) + targetVel := JointTargetVel(ji) + if ke.X > 0.0 { // has position control + UpdateJointAxisWeightedTarget(axis, targetPos.X, ke.X, &axisTargetPosKeD, &axisTargetPosKeA) + } + if kd.X > 0.0 { // has velocity control + UpdateJointAxisWeightedTarget(axis, targetVel.X, kd.X, &axisTargetVelKdD, &axisTargetVelKdA) + } + // if lin_axis_count > 1: + // axis_idx = axis_start + 1 + // axis = joint_axis[axis_idx] + // lower = joint_limit_lower[axis_idx] + // upper = joint_limit_upper[axis_idx] + // axis_limits = update_joint_axis_limits(axis, lower, upper, axis_limits) + // ke = joint_target_ke[axis_idx] + // kd = joint_target_kd[axis_idx] + // target_pos = joint_target_pos[axis_idx] + // target_vel = joint_target_vel[axis_idx] + // if ke > 0.0: # has position control + // axis_target_pos_ke = update_joint_axis_weighted_target(axis, target_pos, ke, axis_target_pos_ke) + // if kd > 0.0: # has velocity control + // axis_target_vel_kd = update_joint_axis_weighted_target(axis, target_vel, kd, axis_target_vel_kd) + // if lin_axis_count > 2: + // axis_idx = axis_start + 2 + // axis = joint_axis[axis_idx] + // lower = joint_limit_lower[axis_idx] + // upper = joint_limit_upper[axis_idx] + // axis_limits = update_joint_axis_limits(axis, lower, upper, axis_limits) + // ke = joint_target_ke[axis_idx] + // kd = joint_target_kd[axis_idx] + // target_pos = joint_target_pos[axis_idx] + // target_vel = joint_target_vel[axis_idx] + // if ke > 0.0: # has position control + // axis_target_pos_ke = update_joint_axis_weighted_target(axis, target_pos, ke, axis_target_pos_ke) + // if kd > 0.0: # has velocity control + // axis_target_vel_kd = update_joint_axis_weighted_target(axis, target_vel, kd, axis_target_vel_kd) + + axisStiffness := axisTargetPosKeA + axisDamping := axisTargetVelKdA + for i := range 3 { + if axisStiffness.X > 0.0 { // todo: Dim(i) access + axisTargetPosKeD.X /= axisStiffness.X + } + } + for i := range 3 { + if axisDamping.X > 0.0 { // todo Dim + axisTargetVelKdD.X /= axisDamping.X + } + } + axisLimitsLower := axisLimitsD + axisLimitsUpper := axisLimitsA + // todo: + // frame_p = wp.quat_to_matrix(wp.transform_get_rotation(X_wp)) + // note that xc appearing in both is correct: + rp := xc.Sub(worldComp) + rc := xc.Sub(slmath.MulQPPoint(posecP, posecQ, comc)) + + // for loop will be unrolled, so we can modify local variables + for dim := range 3 { + e := relPoseP.X // rel_p[dim] + + // compute gradients + linearc := math32.Vec3(frame_p[0, dim], frame_p[1, dim], frame_p[2, dim]) + linearp := slmath.Negate3(linearc) + angularp := slmath.Cross3(rp, linearc) + angularc := slmath.Cross3(rc, linearc) + // constraint time derivative + derr := slmath.Dot3(linearp, velp) + slmath.Dot3(linearc, velc) + slmath.Dot3(angularp, omegap) + slmath.Dot3(angularc, omegac) + + err := float32(0.0) + compliance := pars.JointLinearComply + damping := float32(0.0) + + targetVel := axisTargetVelKdD.X // [dim] + derrRel := derr - targetVel + + // consider joint limits irrespective of axis mode + lower := axisLimitsLower.X // [dim] + upper := axisLimitsUpper.X // [dim] + if e < lower { + err = e - lower + } else if e > upper { + err = e - upper + } else { + targetPos := axisTargetPosKeD.X // [dim] + targetPos = math32.Clamp(targetPos, lower, upper) + + if axisStiffness.X > 0.0 { + err = e - targetPos + compliance = 1.0 / axisStiffness.X // [dim] + damping = axisDamping.X // [dim] + } else if axisDamping.X > 0.0 { + compliance = 1.0 / axisDamping.X // [dim] + damping = axisDamping.X // [dim] + } + } + if math32.Abs(err) > 1e-9 || math32.Abs(derrRel) > 1e-9 { + lambdaIn := float32(0.0) + dLambda := PositionalCorrection(err, derrRel, posepQ, posecQ, mInvp, mInvc, + iInvp, iInvc, linearp, linearc, angularp, angularc, lambdaIn, compliance, damping, pars.Dt) + + linDeltaP = linDeltaP.Add(linearp.MulScalar(dLambda * pars.JointLinearRelax)) + angDeltaP = angDeltaP.Add(angularp.MulScalar(dLambda * pars.JointAngularRelax)) + linDeltaC = linDeltaC.Add(linearc.MulScalar(dLambda * pars.JointLinearRelax)) + angDeltaC = angDeltaC.Add(angularc.MulScalar(dLambda * pars.JointAngularRelax)) + } + } + } + // + // if type == JointType.FIXED or type == JointType.PRISMATIC or type == JointType.REVOLUTE or type == JointType.D6: + // # handle angular constraints + // + // # local joint rotations + // q_p = wp.transform_get_rotation(X_wp) + // q_c = wp.transform_get_rotation(X_wc) + // + // # make quats lie in same hemisphere + // if slmath.Dot3(q_p, q_c) < 0.0: + // q_c *= -1.0 + // + // rel_q = wp.quat_inverse(q_p) * q_c + // + // qtwist = wp.normalize(wp.quat(rel_q[0], 0.0, 0.0, rel_q[3])) + // qswing = rel_q * wp.quat_inverse(qtwist) + // + // # decompose to a compound rotation each axis + // s = wp.sqrt(rel_q[0] * rel_q[0] + rel_q[3] * rel_q[3]) + // invs = 1.0 / s + // invscube = invs * invs * invs + // + // # handle axis-angle joints + // + // # rescale twist from quaternion space to angular + // err_0 = 2.0 * wp.asin(wp.clamp(qtwist[0], -1.0, 1.0)) + // err_1 = qswing[1] + // err_2 = qswing[2] + // # analytic gradients of swing-twist decomposition + // grad_0 = wp.quat(invs - rel_q[0] * rel_q[0] * invscube, 0.0, 0.0, -(rel_q[3] * rel_q[0]) * invscube) + // grad_1 = wp.quat( + // -rel_q[3] * (rel_q[3] * rel_q[2] + rel_q[0] * rel_q[1]) * invscube, + // rel_q[3] * invs, + // -rel_q[0] * invs, + // rel_q[0] * (rel_q[3] * rel_q[2] + rel_q[0] * rel_q[1]) * invscube, + // ) + // grad_2 = wp.quat( + // rel_q[3] * (rel_q[3] * rel_q[1] - rel_q[0] * rel_q[2]) * invscube, + // rel_q[0] * invs, + // rel_q[3] * invs, + // rel_q[0] * (rel_q[2] * rel_q[0] - rel_q[3] * rel_q[1]) * invscube, + // ) + // grad_0 *= 2.0 / wp.abs(qtwist[3]) + // # grad_0 *= 2.0 / wp.sqrt(1.0-qtwist[0]*qtwist[0]) # derivative of asin(x) = 1/sqrt(1-x^2) + // + // # rescale swing + // swing_sq = qswing[3] * qswing[3] + // # if swing axis magnitude close to zero vector, just treat in quaternion space + // angularEps = 1.0e-4 + // if swing_sq + angularEps < 1.0: + // d = wp.sqrt(1.0 - qswing[3] * qswing[3]) + // theta = 2.0 * wp.acos(wp.clamp(qswing[3], -1.0, 1.0)) + // scale = theta / d + // + // err_1 *= scale + // err_2 *= scale + // + // grad_1 *= scale + // grad_2 *= scale + // + // errs = wp.vec3(err_0, err_1, err_2) + // grad_x = wp.vec3(grad_0[0], grad_1[0], grad_2[0]) + // grad_y = wp.vec3(grad_0[1], grad_1[1], grad_2[1]) + // grad_z = wp.vec3(grad_0[2], grad_1[2], grad_2[2]) + // grad_w = wp.vec3(grad_0[3], grad_1[3], grad_2[3]) + // + // # compute joint target, stiffness, damping + // axis_limits = wp.spatial_vector(0.0, 0.0, 0.0, 0.0, 0.0, 0.0) + // + // axis_target_pos_ke = wp.spatial_vector() # [weighted_target_pos, ke_weights] + // axis_target_vel_kd = wp.spatial_vector() # [weighted_target_vel, kd_weights] + // # avoid a for loop here since local variables would need to be modified which is not yet differentiable + // if ang_axis_count > 0: + // axis_idx = axis_start + lin_axis_count + // axis = joint_axis[axis_idx] + // lo_temp = axis * joint_limit_lower[axis_idx] + // up_temp = axis * joint_limit_upper[axis_idx] + // axis_limits = wp.spatial_vector(vec_min(lo_temp, up_temp), vec_max(lo_temp, up_temp)) + // ke = joint_target_ke[axis_idx] + // kd = joint_target_kd[axis_idx] + // target_pos = joint_target_pos[axis_idx] + // target_vel = joint_target_vel[axis_idx] + // if ke > 0.0: # has position control + // axis_target_pos_ke = update_joint_axis_weighted_target(axis, target_pos, ke, axis_target_pos_ke) + // if kd > 0.0: # has velocity control + // axis_target_vel_kd = update_joint_axis_weighted_target(axis, target_vel, kd, axis_target_vel_kd) + + // if ang_axis_count > 1: + // axis_idx = axis_start + lin_axis_count + 1 + // axis = joint_axis[axis_idx] + // lower = joint_limit_lower[axis_idx] + // upper = joint_limit_upper[axis_idx] + // axis_limits = update_joint_axis_limits(axis, lower, upper, axis_limits) + // ke = joint_target_ke[axis_idx] + // kd = joint_target_kd[axis_idx] + // target_pos = joint_target_pos[axis_idx] + // target_vel = joint_target_vel[axis_idx] + // if ke > 0.0: # has position control + // axis_target_pos_ke = update_joint_axis_weighted_target(axis, target_pos, ke, axis_target_pos_ke) + // if kd > 0.0: # has velocity control + // axis_target_vel_kd = update_joint_axis_weighted_target(axis, target_vel, kd, axis_target_vel_kd) + // if ang_axis_count > 2: + // axis_idx = axis_start + lin_axis_count + 2 + // axis = joint_axis[axis_idx] + // lower = joint_limit_lower[axis_idx] + // upper = joint_limit_upper[axis_idx] + // axis_limits = update_joint_axis_limits(axis, lower, upper, axis_limits) + // ke = joint_target_ke[axis_idx] + // kd = joint_target_kd[axis_idx] + // target_pos = joint_target_pos[axis_idx] + // target_vel = joint_target_vel[axis_idx] + // if ke > 0.0: # has position control + // axis_target_pos_ke = update_joint_axis_weighted_target(axis, target_pos, ke, axis_target_pos_ke) + // if kd > 0.0: # has velocity control + // axis_target_vel_kd = update_joint_axis_weighted_target(axis, target_vel, kd, axis_target_vel_kd) + // + + // axis_target_pos = wp.spatial_top(axis_target_pos_ke) + // axis_stiffness = wp.spatial_bottom(axis_target_pos_ke) + // axis_target_vel = wp.spatial_top(axis_target_vel_kd) + // axis_damping = wp.spatial_bottom(axis_target_vel_kd) + // for i in range(3): + // if axis_stiffness[i] > 0.0: + // axis_target_pos[i] /= axis_stiffness[i] + // for i in range(3): + // if axis_damping[i] > 0.0: + // axis_target_vel[i] /= axis_damping[i] + // axis_limits_lower = wp.spatial_top(axis_limits) + // axis_limits_upper = wp.spatial_bottom(axis_limits) + + // # if type == JointType.D6: + // # wp.printf("axis_target: %f %f %f\t axis_stiffness: %f %f %f\t axis_damping: %f %f %f\t axis_limits_lower: %f %f %f \t axis_limits_upper: %f %f %f\n", + // # axis_target[0], axis_target[1], axis_target[2], + // # axis_stiffness[0], axis_stiffness[1], axis_stiffness[2], + // # axis_damping[0], axis_damping[1], axis_damping[2], + // # axis_limits_lower[0], axis_limits_lower[1], axis_limits_lower[2], + // # axis_limits_upper[0], axis_limits_upper[1], axis_limits_upper[2]) + // # # wp.printf("wp.sqrt(1.0-qtwist[0]*qtwist[0]) = %f\n", wp.sqrt(1.0-qtwist[0]*qtwist[0])) + + // for dim in range(3): + // e = errs[dim] + // + // # analytic gradients of swing-twist decomposition + // grad = wp.quat(grad_x[dim], grad_y[dim], grad_z[dim], grad_w[dim]) + // + // quat_c = 0.5 * q_p * grad * wp.quat_inverse(q_c) + // angular_c = wp.vec3(quat_c[0], quat_c[1], quat_c[2]) + // angular_p = -angular_c + // # time derivative of the constraint + // derr = slmath.Dot3(angular_p, omega_p) + slmath.Dot3(angular_c, omega_c) + // + // err = 0.0 + // compliance = angular_compliance + // damping = 0.0 + // + // target_vel = axis_target_vel[dim] + // derr_rel = derr - target_vel + // + // # consider joint limits irrespective of mode + // lower = axis_limits_lower[dim] + // upper = axis_limits_upper[dim] + // if e < lower: + // err = e - lower + // elif e > upper: + // err = e - upper + // else: + // target_pos = axis_target_pos[dim] + // target_pos = wp.clamp(target_pos, lower, upper) + // + // if axis_stiffness[dim] > 0.0: + // err = e - target_pos + // compliance = 1.0 / axis_stiffness[dim] + // damping = axis_damping[dim] + // elif axis_damping[dim] > 0.0: + // damping = axis_damping[dim] + // compliance = 1.0 / axis_damping[dim] + // + // d_lambda = ( + // compute_angular_correction( + // err, derr_rel, pose_p, pose_c, I_inv_p, I_inv_c, angular_p, angular_c, 0.0, compliance, damping, dt + // ) + // * angular_relaxation + // ) + // + // # update deltas + // angDelta_p += angular_p * d_lambda + // angDelta_c += angular_c * d_lambda + + // if id_p >= 0: + // wp.atomic_add(deltas, id_p, wp.spatial_vector(linDelta_p, angDelta_p)) + // if id_c >= 0: + // wp.atomic_add(deltas, id_c, wp.spatial_vector(linDelta_c, angDelta_c)) +} + +func UpdateJointAxisWeightedTarget(axis math32.Vector3, target, weight float32, axisTargets, axisWeights *math32.Vector3) { + weightedAxis := axis.MulScalar(weight) + *axisTargets = axisTargets.Add(weightedAxis.MulScalar(target)) // weighted target (to be normalized later by sum of weights) + *axisWeights = axisWeights.Add(slmath.Abs3(weightedAxis)) +} + +func PositionalCorrection(err, derr float32, tfaQ, tfbQ math32.Quat, mInva, mInvb float32, Iinva, Iinvb math32.Matrix3, lineara, linearb, angulara, angularb math32.Vector3, lambdaIn, compliance, damping, dt float32) float32 { + denom := float32(0.0) + denom += slmath.LengthSquared3(lineara) * mInva + denom += slmath.LengthSquared3(linearb) * mInvb + + q1 := tfaQ + q2 := tfbQ + + // # Eq. 2-3 (make sure to project into the frame of the body) + rotAngulara := slmath.MulQuatVectorInverse(q1, angulara) + rotAngularb := slmath.MulQuatVectorInverse(q2, angularb) + + denom += slmath.Dot3(rotAngulara, Iinva.MulVector3(rotAngulara)) + denom += slmath.Dot3(rotAngularb, Iinvb.MulVector3(rotAngularb)) + + alpha := compliance + gamma := compliance * damping + + deltaLambda := -(err + alpha*lambdaIn + gamma*derr) + if denom+alpha > 0.0 { + deltaLambda /= (dt+gamma)*denom + alpha/dt + } + + return deltaLambda +} + +//gosl:end diff --git a/physics/typegen.go b/physics/typegen.go index 1c7bb9cc..fc7e0c72 100644 --- a/physics/typegen.go +++ b/physics/typegen.go @@ -10,22 +10,22 @@ var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.BBox", IDNam var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.BodyVars", IDName: "body-vars", Doc: "BodyVars are body state variables stored in tensor.Float32"}) -var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.DynamicVars", IDName: "dynamic-vars", Doc: "DynamicVars are dynamic body variables stored in tensor.Float32."}) - var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.ContactVars", IDName: "contact-vars", Doc: "Contact is one pairwise point of contact between two bodies.\nContacts are represented in spherical terms relative to the\nspherical BBox of A and B."}) var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.JointControlVars", IDName: "joint-control-vars", Doc: "JointControlVars are external joint control input variables stored in tensor.Float32.\nThese must be in one-to-one correspondence with the joints."}) +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.DynamicVars", IDName: "dynamic-vars", Doc: "DynamicVars are dynamic body variables stored in tensor.Float32."}) + var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.GPUVars", IDName: "gpu-vars", Doc: "GPUVars is an enum for GPU variables, for specifying what to sync."}) var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.JointVars", IDName: "joint-vars", Doc: "JointVars are joint state variables stored in tensor.Float32.\nThese are all static joint properties; dynamic control variables\nin [JointControlVars] and [JointControls]."}) var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.JointTypes", IDName: "joint-types", Doc: "JointTypes are joint types that determine nature of interaction."}) -var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.PhysParams", IDName: "phys-params", Doc: "PhysParams are the physics parameters", Directives: []types.Directive{{Tool: "gosl", Directive: "start"}}, Fields: []types.Field{{Name: "DynamicsN", Doc: "DynamicsN is number of dynamics bodies."}, {Name: "JointsN", Doc: "JointsN is number of joints."}, {Name: "Iters", Doc: "Iters is the number of iterations to perform."}, {Name: "Dt", Doc: "Dt is the integration stepsize."}, {Name: "SoftRelax", Doc: "SoftRelax is soft-body relaxation constant."}, {Name: "JointLinearRelax", Doc: "JointLinearRelax is joint linear relaxation constant."}, {Name: "JointAngularRelax", Doc: "JointAngularRelax is joint angular relaxation constant."}, {Name: "JointLinearComply", Doc: "JointLinearComply is joint linear compliance constant."}, {Name: "JointAngularComply", Doc: "JointAngularComply is joint angular compliance constant."}, {Name: "ContactRelax", Doc: "ContactRelax is rigid contact relaxation constant."}, {Name: "AngularDamping", Doc: "AngularDamping is damping of angular motion."}, {Name: "ContactWeighting", Doc: "Contact weighting"}, {Name: "Restitution", Doc: "Restitution"}, {Name: "pad"}, {Name: "pad1"}, {Name: "pad2"}, {Name: "Gravity", Doc: "Gravity is the gravity acceleration function"}}}) +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.PhysParams", IDName: "phys-params", Doc: "PhysParams are the physics parameters", Directives: []types.Directive{{Tool: "gosl", Directive: "start"}}, Fields: []types.Field{{Name: "DynamicsN", Doc: "DynamicsN is number of dynamics bodies."}, {Name: "JointsN", Doc: "JointsN is number of joints."}, {Name: "Cur", Doc: "Index for the current state (0 or 1, alternates with Next)."}, {Name: "Next", Doc: "Index for the next state (1 or 0, alternates with Cur)."}, {Name: "Iters", Doc: "Iters is the number of iterations to perform."}, {Name: "Dt", Doc: "Dt is the integration stepsize."}, {Name: "SoftRelax", Doc: "SoftRelax is soft-body relaxation constant."}, {Name: "JointLinearRelax", Doc: "JointLinearRelax is joint linear relaxation constant."}, {Name: "JointAngularRelax", Doc: "JointAngularRelax is joint angular relaxation constant."}, {Name: "JointLinearComply", Doc: "JointLinearComply is joint linear compliance constant."}, {Name: "JointAngularComply", Doc: "JointAngularComply is joint angular compliance constant."}, {Name: "ContactRelax", Doc: "ContactRelax is rigid contact relaxation constant."}, {Name: "AngularDamping", Doc: "AngularDamping is damping of angular motion."}, {Name: "ContactWeighting", Doc: "Contact weighting"}, {Name: "Restitution", Doc: "Restitution"}, {Name: "pad"}, {Name: "Gravity", Doc: "Gravity is the gravity acceleration function"}}}) var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.Shapes", IDName: "shapes", Doc: "Shapes are elemental shapes for rigid bodies."}) var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.State", IDName: "state", Doc: "State contains the basic physical state including position, orientation, velocity.\nThese are only the values that can be either relative or absolute -- other physical\nstate values such as Mass should go in Rigid.", Methods: []types.Method{{Name: "MoveOnAxis", Doc: "MoveOnAxis moves (translates) the specified distance on the specified local axis,\nrelative to the current rotation orientation.\nThe axis is normalized prior to aplying the distance factor.\nSets the LinVel to motion vector.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z", "dist"}}, {Name: "MoveOnAxisAbs", Doc: "MoveOnAxisAbs moves (translates) the specified distance on the specified local axis,\nin absolute X,Y,Z coordinates (does not apply the Quat rotation factor.\nThe axis is normalized prior to aplying the distance factor.\nSets the LinVel to motion vector.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z", "dist"}}, {Name: "SetEulerRotation", Doc: "SetEulerRotation sets the rotation in Euler angles (degrees).", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z"}}, {Name: "EulerRotation", Doc: "EulerRotation returns the current rotation in Euler angles (degrees).", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Returns: []string{"Vector3"}}, {Name: "SetAxisRotation", Doc: "SetAxisRotation sets rotation from local axis and angle in degrees.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z", "angle"}}, {Name: "RotateOnAxis", Doc: "RotateOnAxis rotates around the specified local axis the specified angle in degrees.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z", "angle"}}, {Name: "RotateEuler", Doc: "RotateEuler rotates by given Euler angles (in degrees) relative to existing rotation.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z"}}}, Fields: []types.Field{{Name: "Pos", Doc: "Pos is the position of center of mass of object."}, {Name: "Quat", Doc: "Quat is the rotation specified as a quaternion."}, {Name: "LinVel", Doc: "LinVel is the linear velocity."}, {Name: "AngVel", Doc: "AngVel is the angular velocity."}}}) -var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.World", IDName: "world", Doc: "World contains and manages all of the physics elements.", Directives: []types.Directive{{Tool: "go", Directive: "generate", Args: []string{"core", "generate", "-add-types", "-gosl"}}}, Fields: []types.Field{{Name: "GPU", Doc: "GPU determines whether to use GPU (else CPU)."}, {Name: "Params", Doc: "Params are global parameters."}, {Name: "Bodies", Doc: "Bodies are the rigid body elements (dynamic and static),\nspecifying the constant, non-dynamic properties,\nwhich is initial state for dynamics.\n[body][BodyVarsN]"}, {Name: "Joints", Doc: "Joints is a list of permanent joints connecting bodies,\nwhich do not change (no dynamic variables).\n[joint][JointVarsN]"}, {Name: "BodyJoints", Doc: "BodyJoints is a list of joint indexes for each dynamic body, for aggregating.\n[dyn body][parent, child][maxjointsperbody]"}, {Name: "Dynamics", Doc: "Dynamics are the dynamic rigid body elements: these actually move.\nThe first set of variables are for initial values, and the second current.\n[body][DynamicVarsN]"}, {Name: "Contacts", Doc: "Contacts are points of contact between bodies.\n[contact][ContactVarsN]"}, {Name: "JointControls", Doc: "JointControls are dynamic joint control inputs.\n[joint][JointControlVarsN]"}}}) +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.World", IDName: "world", Doc: "World contains and manages all of the physics elements.", Directives: []types.Directive{{Tool: "go", Directive: "generate", Args: []string{"core", "generate", "-add-types", "-gosl"}}}, Fields: []types.Field{{Name: "GPU", Doc: "GPU determines whether to use GPU (else CPU)."}, {Name: "Params", Doc: "Params are global parameters."}, {Name: "Bodies", Doc: "Bodies are the rigid body elements (dynamic and static),\nspecifying the constant, non-dynamic properties,\nwhich is initial state for dynamics.\n[body][BodyVarsN]"}, {Name: "Joints", Doc: "Joints is a list of permanent joints connecting bodies,\nwhich do not change (no dynamic variables).\n[joint][JointVarsN]"}, {Name: "BodyJoints", Doc: "BodyJoints is a list of joint indexes for each dynamic body, for aggregating.\n[dyn body][parent, child][maxjointsperbody]"}, {Name: "Dynamics", Doc: "Dynamics are the dynamic rigid body elements: these actually move.\nThe first set of variables are for initial values, and the second current.\n[body][cur/next][DynamicVarsN]"}, {Name: "Contacts", Doc: "Contacts are points of contact between bodies.\n[contact][ContactVarsN]"}, {Name: "JointControls", Doc: "JointControls are dynamic joint control inputs.\n[joint][JointControlVarsN]"}}}) diff --git a/physics/vars.go b/physics/vars.go index f2f5a6a1..31df9788 100644 --- a/physics/vars.go +++ b/physics/vars.go @@ -39,9 +39,10 @@ var ( BodyJoints *tensor.Int32 // Dynamics are the dynamic rigid body elements: these actually move. - // [dyn body][DynamicVarsN] + // Two alternating states are used: Params.Cur and Params.Next. + // [dyn body][cur/next][DynamicVarsN] //gosl:group Bodies - //gosl:dims 2 + //gosl:dims 3 Dynamics *tensor.Float32 // Contacts are points of contact between bodies. diff --git a/physics/world.go b/physics/world.go index d8b52b25..9c88c0ba 100644 --- a/physics/world.go +++ b/physics/world.go @@ -38,7 +38,7 @@ type World struct { // Dynamics are the dynamic rigid body elements: these actually move. // The first set of variables are for initial values, and the second current. - // [body][DynamicVarsN] + // [body][cur/next][DynamicVarsN] Dynamics *tensor.Float32 // Contacts are points of contact between bodies. @@ -62,7 +62,7 @@ func (wl *World) Init() { wl.Bodies = tensor.NewFloat32(0, int(BodyVarsN)) wl.Joints = tensor.NewFloat32(0, int(JointVarsN)) wl.BodyJoints = tensor.NewInt32(0, 2, 2) - wl.Dynamics = tensor.NewFloat32(0, int(DynamicVarsN)) + wl.Dynamics = tensor.NewFloat32(0, 2, int(DynamicVarsN)) wl.Contacts = tensor.NewFloat32(0, int(ContactVarsN)) wl.JointControls = tensor.NewFloat32(0, int(JointControlVarsN)) wl.SetAsCurrentVars() @@ -86,8 +86,9 @@ func (wl *World) NewDynamic(shape Shapes, size, pos math32.Vector3, rot math32.Q bodyIdx = wl.NewBody(shape, size, pos, rot) sizes := wl.Dynamics.ShapeSizes() dynIdx = int32(sizes[0]) - wl.Dynamics.SetShapeSizes(int(dynIdx+1), int(DynamicVarsN)) - SetDynamicIndex(dynIdx, bodyIdx) + wl.Dynamics.SetShapeSizes(int(dynIdx+1), 2, int(DynamicVarsN)) + SetDynamicIndex(dynIdx, 0, bodyIdx) + SetDynamicIndex(dynIdx, 1, bodyIdx) wl.Params[0].DynamicsN = dynIdx + 1 return } diff --git a/physics/world.goal b/physics/world.goal index e1e11454..8fe38c7d 100644 --- a/physics/world.goal +++ b/physics/world.goal @@ -36,7 +36,7 @@ type World struct { // Dynamics are the dynamic rigid body elements: these actually move. // The first set of variables are for initial values, and the second current. - // [body][DynamicVarsN] + // [body][cur/next][DynamicVarsN] Dynamics *tensor.Float32 // Contacts are points of contact between bodies. @@ -60,7 +60,7 @@ func (wl *World) Init() { wl.Bodies = tensor.NewFloat32(0, int(BodyVarsN)) wl.Joints = tensor.NewFloat32(0, int(JointVarsN)) wl.BodyJoints = tensor.NewInt32(0, 2, 2) - wl.Dynamics = tensor.NewFloat32(0, int(DynamicVarsN)) + wl.Dynamics = tensor.NewFloat32(0, 2, int(DynamicVarsN)) wl.Contacts = tensor.NewFloat32(0, int(ContactVarsN)) wl.JointControls = tensor.NewFloat32(0, int(JointControlVarsN)) wl.SetAsCurrentVars() @@ -84,8 +84,9 @@ func (wl *World) NewDynamic(shape Shapes, size, pos math32.Vector3, rot math32.Q bodyIdx = wl.NewBody(shape, size, pos, rot) sizes := wl.Dynamics.ShapeSizes() dynIdx = int32(sizes[0]) - wl.Dynamics.SetShapeSizes(int(dynIdx+1), int(DynamicVarsN)) - SetDynamicIndex(dynIdx, bodyIdx) + wl.Dynamics.SetShapeSizes(int(dynIdx+1), 2, int(DynamicVarsN)) + SetDynamicIndex(dynIdx, 0, bodyIdx) + SetDynamicIndex(dynIdx, 1, bodyIdx) wl.Params[0].DynamicsN = dynIdx + 1 return } From 968bec83942cc3b3ef4bca6a6e96ddf96953e4ed Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Tue, 16 Dec 2025 07:58:53 +0100 Subject: [PATCH 17/97] physics: body deltas --- physics/enumgen.go | 10 +- physics/gosl.go | 47 ++ physics/joint.go | 60 +++ physics/joint.goal | 60 +++ physics/shaders/DynamicsCurToNext.wgsl | 261 ++++++++++ physics/shaders/InitDynamics.wgsl | 14 +- physics/shaders/StepBodyDeltas.wgsl | 392 +++++++++++++++ physics/shaders/StepIntegrateBodies.wgsl | 389 +++++++++++++++ physics/shaders/StepJointForces.wgsl | 396 +++++++++++++++ physics/shaders/StepSolveJoints.wgsl | 582 +++++++++++++++++++++++ physics/step_body.go | 101 +++- physics/step_body.goal | 101 +++- physics/step_joint.go | 38 +- physics/step_joint.goal | 35 +- 14 files changed, 2421 insertions(+), 65 deletions(-) create mode 100644 physics/shaders/DynamicsCurToNext.wgsl create mode 100644 physics/shaders/StepBodyDeltas.wgsl create mode 100644 physics/shaders/StepIntegrateBodies.wgsl create mode 100644 physics/shaders/StepJointForces.wgsl create mode 100644 physics/shaders/StepSolveJoints.wgsl diff --git a/physics/enumgen.go b/physics/enumgen.go index 9ea7de78..600b0471 100644 --- a/physics/enumgen.go +++ b/physics/enumgen.go @@ -237,20 +237,20 @@ func (i GPUVars) MarshalText() ([]byte, error) { return []byte(i.String()), nil // UnmarshalText implements the [encoding.TextUnmarshaler] interface. func (i *GPUVars) UnmarshalText(text []byte) error { return enums.UnmarshalText(i, text, "GPUVars") } -var _JointVarsValues = []JointVars{0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40} +var _JointVarsValues = []JointVars{0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 51, 52} // JointVarsN is the highest valid value for type JointVars, plus one. // //gosl:start -const JointVarsN JointVars = 41 +const JointVarsN JointVars = 53 //gosl:end -var _JointVarsValueMap = map[string]JointVars{`JointType`: 0, `JointEnabled`: 1, `JointParent`: 2, `JointChild`: 3, `JointPPosX`: 4, `JointPPosY`: 5, `JointPPosZ`: 6, `JointPRotX`: 7, `JointPRotY`: 8, `JointPRotZ`: 9, `JointPRotW`: 10, `JointCPosX`: 11, `JointCPosY`: 12, `JointCPosZ`: 13, `JointCRotX`: 14, `JointCRotY`: 15, `JointCRotZ`: 16, `JointCRotW`: 17, `JointAxisX`: 18, `JointAxisY`: 19, `JointAxisZ`: 20, `JointLimitLower`: 21, `JointLimitUpper`: 22, `JointStiffX`: 23, `JointStiffY`: 24, `JointStiffZ`: 25, `JointDampX`: 26, `JointDampY`: 27, `JointDampZ`: 28, `JointPForceX`: 29, `JointPForceY`: 30, `JointPForceZ`: 31, `JointPTorqueX`: 32, `JointPTorqueY`: 33, `JointPTorqueZ`: 34, `JointCForceX`: 35, `JointCForceY`: 36, `JointCForceZ`: 37, `JointCTorqueX`: 38, `JointCTorqueY`: 39, `JointCTorqueZ`: 40} +var _JointVarsValueMap = map[string]JointVars{`JointType`: 0, `JointEnabled`: 1, `JointParent`: 2, `JointChild`: 3, `JointPPosX`: 4, `JointPPosY`: 5, `JointPPosZ`: 6, `JointPRotX`: 7, `JointPRotY`: 8, `JointPRotZ`: 9, `JointPRotW`: 10, `JointCPosX`: 11, `JointCPosY`: 12, `JointCPosZ`: 13, `JointCRotX`: 14, `JointCRotY`: 15, `JointCRotZ`: 16, `JointCRotW`: 17, `JointAxisX`: 18, `JointAxisY`: 19, `JointAxisZ`: 20, `JointLimitLower`: 21, `JointLimitUpper`: 22, `JointStiffX`: 23, `JointStiffY`: 24, `JointStiffZ`: 25, `JointDampX`: 26, `JointDampY`: 27, `JointDampZ`: 28, `JointPForceX`: 29, `JointPForceY`: 30, `JointPForceZ`: 31, `JointPTorqueX`: 32, `JointPTorqueY`: 33, `JointPTorqueZ`: 34, `JointCForceX`: 35, `JointCForceY`: 36, `JointCForceZ`: 37, `JointCTorqueX`: 38, `JointCTorqueY`: 39, `JointCTorqueZ`: 40, `JointPDeltaX`: 41, `JointPDeltaY`: 42, `JointPDeltaZ`: 43, `JointPAngDeltaX`: 44, `JointPAngDeltaY`: 45, `JointPAngDeltaZ`: 46, `JointCDeltaX`: 47, `JointCDeltaY`: 48, `JointCDeltaZ`: 49, `JointCAngDeltaX`: 50, `JointCAngDeltaY`: 51, `JointCAngDeltaZ`: 52} -var _JointVarsDescMap = map[JointVars]string{0: `JointType (as an int32 from bits).`, 1: `JointEnabled allows joints to be dynamically enabled.`, 2: `JointParent is the dynamic body index for parent body. Can be -1 for a fixed parent for absolute anchor.`, 3: `JointChild is the dynamic body index for child body.`, 4: `position of joint, in parent frame.`, 5: ``, 6: ``, 7: `orientation of joint, in parent frame.`, 8: ``, 9: ``, 10: ``, 11: `position of joint, in child frame.`, 12: ``, 13: ``, 14: `orientation of joint, in child frame.`, 15: ``, 16: ``, 17: ``, 18: ``, 19: ``, 20: ``, 21: `joint limits`, 22: ``, 23: `joint stiffness target (ke)`, 24: ``, 25: ``, 26: `joint damping target (kd)`, 27: ``, 28: ``, 29: `Computed parent joint force value.`, 30: ``, 31: ``, 32: `Computed parent joint torque value.`, 33: ``, 34: ``, 35: `Computed child joint force value.`, 36: ``, 37: ``, 38: `Computed child joint torque value.`, 39: ``, 40: ``} +var _JointVarsDescMap = map[JointVars]string{0: `JointType (as an int32 from bits).`, 1: `JointEnabled allows joints to be dynamically enabled.`, 2: `JointParent is the dynamic body index for parent body. Can be -1 for a fixed parent for absolute anchor.`, 3: `JointChild is the dynamic body index for child body.`, 4: `position of joint, in parent frame.`, 5: ``, 6: ``, 7: `orientation of joint, in parent frame.`, 8: ``, 9: ``, 10: ``, 11: `position of joint, in child frame.`, 12: ``, 13: ``, 14: `orientation of joint, in child frame.`, 15: ``, 16: ``, 17: ``, 18: ``, 19: ``, 20: ``, 21: `joint limits`, 22: ``, 23: `joint stiffness target (ke)`, 24: ``, 25: ``, 26: `joint damping target (kd)`, 27: ``, 28: ``, 29: `Computed parent joint force value.`, 30: ``, 31: ``, 32: `Computed parent joint torque value.`, 33: ``, 34: ``, 35: `Computed child joint force value.`, 36: ``, 37: ``, 38: `Computed child joint torque value.`, 39: ``, 40: ``, 41: `Computed parent joint delta value.`, 42: ``, 43: ``, 44: `Computed parent joint angdelta value.`, 45: ``, 46: ``, 47: `Computed child joint delta value.`, 48: ``, 49: ``, 50: `Computed child joint angdelta value.`, 51: ``, 52: ``} -var _JointVarsMap = map[JointVars]string{0: `JointType`, 1: `JointEnabled`, 2: `JointParent`, 3: `JointChild`, 4: `JointPPosX`, 5: `JointPPosY`, 6: `JointPPosZ`, 7: `JointPRotX`, 8: `JointPRotY`, 9: `JointPRotZ`, 10: `JointPRotW`, 11: `JointCPosX`, 12: `JointCPosY`, 13: `JointCPosZ`, 14: `JointCRotX`, 15: `JointCRotY`, 16: `JointCRotZ`, 17: `JointCRotW`, 18: `JointAxisX`, 19: `JointAxisY`, 20: `JointAxisZ`, 21: `JointLimitLower`, 22: `JointLimitUpper`, 23: `JointStiffX`, 24: `JointStiffY`, 25: `JointStiffZ`, 26: `JointDampX`, 27: `JointDampY`, 28: `JointDampZ`, 29: `JointPForceX`, 30: `JointPForceY`, 31: `JointPForceZ`, 32: `JointPTorqueX`, 33: `JointPTorqueY`, 34: `JointPTorqueZ`, 35: `JointCForceX`, 36: `JointCForceY`, 37: `JointCForceZ`, 38: `JointCTorqueX`, 39: `JointCTorqueY`, 40: `JointCTorqueZ`} +var _JointVarsMap = map[JointVars]string{0: `JointType`, 1: `JointEnabled`, 2: `JointParent`, 3: `JointChild`, 4: `JointPPosX`, 5: `JointPPosY`, 6: `JointPPosZ`, 7: `JointPRotX`, 8: `JointPRotY`, 9: `JointPRotZ`, 10: `JointPRotW`, 11: `JointCPosX`, 12: `JointCPosY`, 13: `JointCPosZ`, 14: `JointCRotX`, 15: `JointCRotY`, 16: `JointCRotZ`, 17: `JointCRotW`, 18: `JointAxisX`, 19: `JointAxisY`, 20: `JointAxisZ`, 21: `JointLimitLower`, 22: `JointLimitUpper`, 23: `JointStiffX`, 24: `JointStiffY`, 25: `JointStiffZ`, 26: `JointDampX`, 27: `JointDampY`, 28: `JointDampZ`, 29: `JointPForceX`, 30: `JointPForceY`, 31: `JointPForceZ`, 32: `JointPTorqueX`, 33: `JointPTorqueY`, 34: `JointPTorqueZ`, 35: `JointCForceX`, 36: `JointCForceY`, 37: `JointCForceZ`, 38: `JointCTorqueX`, 39: `JointCTorqueY`, 40: `JointCTorqueZ`, 41: `JointPDeltaX`, 42: `JointPDeltaY`, 43: `JointPDeltaZ`, 44: `JointPAngDeltaX`, 45: `JointPAngDeltaY`, 46: `JointPAngDeltaZ`, 47: `JointCDeltaX`, 48: `JointCDeltaY`, 49: `JointCDeltaZ`, 50: `JointCAngDeltaX`, 51: `JointCAngDeltaY`, 52: `JointCAngDeltaZ`} // String returns the string representation of this JointVars value. func (i JointVars) String() string { return enums.String(i, _JointVarsMap) } diff --git a/physics/gosl.go b/physics/gosl.go index 4ff1ce2f..f0438e19 100644 --- a/physics/gosl.go +++ b/physics/gosl.go @@ -115,6 +115,11 @@ func GPUInit() { pl.AddVarUsed(1, "Bodies") pl.AddVarUsed(2, "Dynamics") pl.AddVarUsed(0, "Params") + pl = gpu.NewComputePipelineShaderFS(shaders, "shaders/StepBodyDeltas.wgsl", sy) + pl.AddVarUsed(0, "TensorStrides") + pl.AddVarUsed(1, "Bodies") + pl.AddVarUsed(2, "Dynamics") + pl.AddVarUsed(0, "Params") pl = gpu.NewComputePipelineShaderFS(shaders, "shaders/StepIntegrateBodies.wgsl", sy) pl.AddVarUsed(0, "TensorStrides") pl.AddVarUsed(1, "Bodies") @@ -237,6 +242,48 @@ func RunOneInitDynamics(n int, syncVars ...GPUVars) { RunInitDynamicsCPU(n) } } +// RunStepBodyDeltas runs the StepBodyDeltas kernel with given number of elements, +// on either the CPU or GPU depending on the UseGPU variable. +// Can call multiple Run* kernels in a row, which are then all launched +// in the same command submission on the GPU, which is by far the most efficient. +// MUST call RunDone (with optional vars to sync) after all Run calls. +// Alternatively, a single-shot RunOneStepBodyDeltas call does Run and Done for a +// single run-and-sync case. +func RunStepBodyDeltas(n int) { + if UseGPU { + RunStepBodyDeltasGPU(n) + } else { + RunStepBodyDeltasCPU(n) + } +} + +// RunStepBodyDeltasGPU runs the StepBodyDeltas kernel on the GPU. See [RunStepBodyDeltas] for more info. +func RunStepBodyDeltasGPU(n int) { + sy := GPUSystem + pl := sy.ComputePipelines["StepBodyDeltas"] + ce, _ := sy.BeginComputePass() + pl.Dispatch1D(ce, n, 64) +} + +// RunStepBodyDeltasCPU runs the StepBodyDeltas kernel on the CPU. +func RunStepBodyDeltasCPU(n int) { + gpu.VectorizeFunc(0, n, StepBodyDeltas) +} + +// RunOneStepBodyDeltas runs the StepBodyDeltas kernel with given number of elements, +// on either the CPU or GPU depending on the UseGPU variable. +// This version then calls RunDone with the given variables to sync +// after the Run, for a single-shot Run-and-Done call. If multiple kernels +// can be run in sequence, it is much more efficient to do multiple Run* +// calls followed by a RunDone call. +func RunOneStepBodyDeltas(n int, syncVars ...GPUVars) { + if UseGPU { + RunStepBodyDeltasGPU(n) + RunDone(syncVars...) + } else { + RunStepBodyDeltasCPU(n) + } +} // RunStepIntegrateBodies runs the StepIntegrateBodies kernel with given number of elements, // on either the CPU or GPU depending on the UseGPU variable. // Can call multiple Run* kernels in a row, which are then all launched diff --git a/physics/joint.go b/physics/joint.go index 1c6cc250..f565f296 100644 --- a/physics/joint.go +++ b/physics/joint.go @@ -94,6 +94,26 @@ const ( JointCTorqueX JointCTorqueY JointCTorqueZ + + // Computed parent joint delta value. + JointPDeltaX + JointPDeltaY + JointPDeltaZ + + // Computed parent joint angdelta value. + JointPAngDeltaX + JointPAngDeltaY + JointPAngDeltaZ + + // Computed child joint delta value. + JointCDeltaX + JointCDeltaY + JointCDeltaZ + + // Computed child joint angdelta value. + JointCAngDeltaX + JointCAngDeltaY + JointCAngDeltaZ ) func GetJointType(idx int32) JointTypes { @@ -232,6 +252,46 @@ func SetJointCTorque(idx int32, t math32.Vector3) { Joints.Set(t.Z, int(idx), int(JointCTorqueZ)) } +func JointPDelta(idx int32) math32.Vector3 { + return math32.Vec3(Joints.Value(int(idx), int(JointPDeltaX)), Joints.Value(int(idx), int(JointPDeltaY)), Joints.Value(int(idx), int(JointPDeltaZ))) +} + +func SetJointPDelta(idx int32, f math32.Vector3) { + Joints.Set(f.X, int(idx), int(JointPDeltaX)) + Joints.Set(f.Y, int(idx), int(JointPDeltaY)) + Joints.Set(f.Z, int(idx), int(JointPDeltaZ)) +} + +func JointPAngDelta(idx int32) math32.Vector3 { + return math32.Vec3(Joints.Value(int(idx), int(JointPAngDeltaX)), Joints.Value(int(idx), int(JointPAngDeltaY)), Joints.Value(int(idx), int(JointPAngDeltaZ))) +} + +func SetJointPAngDelta(idx int32, t math32.Vector3) { + Joints.Set(t.X, int(idx), int(JointPAngDeltaX)) + Joints.Set(t.Y, int(idx), int(JointPAngDeltaY)) + Joints.Set(t.Z, int(idx), int(JointPAngDeltaZ)) +} + +func JointCDelta(idx int32) math32.Vector3 { + return math32.Vec3(Joints.Value(int(idx), int(JointCDeltaX)), Joints.Value(int(idx), int(JointCDeltaY)), Joints.Value(int(idx), int(JointCDeltaZ))) +} + +func SetJointCDelta(idx int32, f math32.Vector3) { + Joints.Set(f.X, int(idx), int(JointCDeltaX)) + Joints.Set(f.Y, int(idx), int(JointCDeltaY)) + Joints.Set(f.Z, int(idx), int(JointCDeltaZ)) +} + +func JointCAngDelta(idx int32) math32.Vector3 { + return math32.Vec3(Joints.Value(int(idx), int(JointCAngDeltaX)), Joints.Value(int(idx), int(JointCAngDeltaY)), Joints.Value(int(idx), int(JointCAngDeltaZ))) +} + +func SetJointCAngDelta(idx int32, t math32.Vector3) { + Joints.Set(t.X, int(idx), int(JointCAngDeltaX)) + Joints.Set(t.Y, int(idx), int(JointCAngDeltaY)) + Joints.Set(t.Z, int(idx), int(JointCAngDeltaZ)) +} + // JointTypes are joint types that determine nature of interaction. type JointTypes int32 //enums:enum diff --git a/physics/joint.goal b/physics/joint.goal index d2f5f635..fe7165cd 100644 --- a/physics/joint.goal +++ b/physics/joint.goal @@ -92,6 +92,26 @@ const ( JointCTorqueX JointCTorqueY JointCTorqueZ + + // Computed parent joint delta value. + JointPDeltaX + JointPDeltaY + JointPDeltaZ + + // Computed parent joint angdelta value. + JointPAngDeltaX + JointPAngDeltaY + JointPAngDeltaZ + + // Computed child joint delta value. + JointCDeltaX + JointCDeltaY + JointCDeltaZ + + // Computed child joint angdelta value. + JointCAngDeltaX + JointCAngDeltaY + JointCAngDeltaZ ) func GetJointType(idx int32) JointTypes { @@ -230,6 +250,46 @@ func SetJointCTorque(idx int32, t math32.Vector3) { Joints[idx, JointCTorqueZ] = t.Z } +func JointPDelta(idx int32) math32.Vector3 { + return math32.Vec3(Joints[idx, JointPDeltaX], Joints[idx, JointPDeltaY], Joints[idx, JointPDeltaZ]) +} + +func SetJointPDelta(idx int32, f math32.Vector3) { + Joints[idx, JointPDeltaX] = f.X + Joints[idx, JointPDeltaY] = f.Y + Joints[idx, JointPDeltaZ] = f.Z +} + +func JointPAngDelta(idx int32) math32.Vector3 { + return math32.Vec3(Joints[idx, JointPAngDeltaX], Joints[idx, JointPAngDeltaY], Joints[idx, JointPAngDeltaZ]) +} + +func SetJointPAngDelta(idx int32, t math32.Vector3) { + Joints[idx, JointPAngDeltaX] = t.X + Joints[idx, JointPAngDeltaY] = t.Y + Joints[idx, JointPAngDeltaZ] = t.Z +} + +func JointCDelta(idx int32) math32.Vector3 { + return math32.Vec3(Joints[idx, JointCDeltaX], Joints[idx, JointCDeltaY], Joints[idx, JointCDeltaZ]) +} + +func SetJointCDelta(idx int32, f math32.Vector3) { + Joints[idx, JointCDeltaX] = f.X + Joints[idx, JointCDeltaY] = f.Y + Joints[idx, JointCDeltaZ] = f.Z +} + +func JointCAngDelta(idx int32) math32.Vector3 { + return math32.Vec3(Joints[idx, JointCAngDeltaX], Joints[idx, JointCAngDeltaY], Joints[idx, JointCAngDeltaZ]) +} + +func SetJointCAngDelta(idx int32, t math32.Vector3) { + Joints[idx, JointCAngDeltaX] = t.X + Joints[idx, JointCAngDeltaY] = t.Y + Joints[idx, JointCAngDeltaZ] = t.Z +} + // JointTypes are joint types that determine nature of interaction. type JointTypes int32 //enums:enum diff --git a/physics/shaders/DynamicsCurToNext.wgsl b/physics/shaders/DynamicsCurToNext.wgsl new file mode 100644 index 00000000..cafbabbd --- /dev/null +++ b/physics/shaders/DynamicsCurToNext.wgsl @@ -0,0 +1,261 @@ +// Code generated by "gosl"; DO NOT EDIT +// kernel: DynamicsCurToNext + +// // Params are global parameters. +@group(0) @binding(0) +var TensorStrides: array; +@group(0) @binding(1) +var Params: array; +// // Bodies are the rigid body elements (dynamic and static), // specifying the constant, non-dynamic properties, // which is initial state for dynamics. // [body][BodyVarsN] +// // Dynamics are the dynamic rigid body elements: these actually move. // Two alternating states are used: Params.Cur and Params.Next. // [dyn body][cur/next][DynamicVarsN] +@group(2) @binding(0) +var Dynamics: array; +// // JointControls are dynamic joint control inputs. // [joint][JointControlVarsN] + +alias GPUVars = i32; + +@compute @workgroup_size(64, 1, 1) +fn main(@builtin(workgroup_id) wgid: vec3, @builtin(num_workgroups) nwg: vec3, @builtin(local_invocation_index) loci: u32) { + let idx = loci + (wgid.x + wgid.y * nwg.x + wgid.z * nwg.x * nwg.y) * 64; + DynamicsCurToNext(idx); +} + +fn Index3D(s0: u32, s1: u32, s2: u32, i0: u32, i1: u32, i2: u32) -> u32 { + return s0 * i0 + s1 * i1 + s2 * i2; +} + + +//////// import: "vars.go" + +//////// import: "body.go" +alias BodyVars = i32; //enums:enum +const BodyShape: BodyVars = 0; +const BodyWorldIndex: BodyVars = 1; +const BodySizeX: BodyVars = 2; +const BodySizeY: BodyVars = 3; +const BodySizeZ: BodyVars = 4; +const BodyMass: BodyVars = 5; +const BodyInvMass: BodyVars = 6; +const BodyBounce: BodyVars = 7; +const BodyFriction: BodyVars = 8; +const BodyPosX: BodyVars = 9; +const BodyPosY: BodyVars = 10; +const BodyPosZ: BodyVars = 11; +const BodyRotX: BodyVars = 12; +const BodyRotY: BodyVars = 13; +const BodyRotZ: BodyVars = 14; +const BodyRotW: BodyVars = 15; +const BodyComX: BodyVars = 16; +const BodyComY: BodyVars = 17; +const BodyComZ: BodyVars = 18; +const BodyInertiaXX: BodyVars = 19; +const BodyInertiaYX: BodyVars = 20; +const BodyInertiaZX: BodyVars = 21; +const BodyInertiaXY: BodyVars = 22; +const BodyInertiaYY: BodyVars = 23; +const BodyInertiaZY: BodyVars = 24; +const BodyInertiaXZ: BodyVars = 25; +const BodyInertiaYZ: BodyVars = 26; +const BodyInertiaZZ: BodyVars = 27; +const BodyInvInertiaXX: BodyVars = 28; +const BodyInvInertiaYX: BodyVars = 29; +const BodyInvInertiaZX: BodyVars = 30; +const BodyInvInertiaXY: BodyVars = 31; +const BodyInvInertiaYY: BodyVars = 32; +const BodyInvInertiaZY: BodyVars = 33; +const BodyInvInertiaXZ: BodyVars = 34; +const BodyInvInertiaYZ: BodyVars = 35; +const BodyInvInertiaZZ: BodyVars = 36; + +//////// import: "contact.go" +alias ContactVars = i32; //enums:enum +const ContactA: ContactVars = 0; +const ContactB: ContactVars = 1; +const ContactNormX: ContactVars = 2; +const ContactNormY: ContactVars = 3; +const ContactNormZ: ContactVars = 4; +const ContactPointX: ContactVars = 5; +const ContactPointY: ContactVars = 6; +const ContactPointZ: ContactVars = 7; +const ContactDist: ContactVars = 8; + +//////// import: "control.go" +alias JointControlVars = i32; //enums:enum +const JointCtrlForceX: JointControlVars = 0; +const JointCtrlForceY: JointControlVars = 1; +const JointCtrlForceZ: JointControlVars = 2; +const JointCtrlTorqueX: JointControlVars = 3; +const JointCtrlTorqueY: JointControlVars = 4; +const JointCtrlTorqueZ: JointControlVars = 5; +const JointTargetPosX: JointControlVars = 6; +const JointTargetPosY: JointControlVars = 7; +const JointTargetPosZ: JointControlVars = 8; +const JointTargetRotX: JointControlVars = 9; +const JointTargetRotY: JointControlVars = 10; +const JointTargetRotZ: JointControlVars = 11; +const JointTargetRotW: JointControlVars = 12; +const JointTargetVelX: JointControlVars = 13; +const JointTargetVelY: JointControlVars = 14; +const JointTargetVelZ: JointControlVars = 15; +const JointTargetAngVelX: JointControlVars = 16; +const JointTargetAngVelY: JointControlVars = 17; +const JointTargetAngVelZ: JointControlVars = 18; + +//////// import: "dynamics.go" +alias DynamicVars = i32; //enums:enum +const DynIndex: DynamicVars = 0; +const DynPosX: DynamicVars = 1; +const DynPosY: DynamicVars = 2; +const DynPosZ: DynamicVars = 3; +const DynRotX: DynamicVars = 4; +const DynRotY: DynamicVars = 5; +const DynRotZ: DynamicVars = 6; +const DynRotW: DynamicVars = 7; +const DynVelX: DynamicVars = 8; +const DynVelY: DynamicVars = 9; +const DynVelZ: DynamicVars = 10; +const DynAngVelX: DynamicVars = 11; +const DynAngVelY: DynamicVars = 12; +const DynAngVelZ: DynamicVars = 13; +const DynAccX: DynamicVars = 14; +const DynAccY: DynamicVars = 15; +const DynAccZ: DynamicVars = 16; +const DynAngAccX: DynamicVars = 17; +const DynAngAccY: DynamicVars = 18; +const DynAngAccZ: DynamicVars = 19; +const DynForceX: DynamicVars = 20; +const DynForceY: DynamicVars = 21; +const DynForceZ: DynamicVars = 22; +const DynTorqueX: DynamicVars = 23; +const DynTorqueY: DynamicVars = 24; +const DynTorqueZ: DynamicVars = 25; +const DynDeltaX: DynamicVars = 26; +const DynDeltaY: DynamicVars = 27; +const DynDeltaZ: DynamicVars = 28; +const DynAngDeltaX: DynamicVars = 29; +const DynAngDeltaY: DynamicVars = 30; +const DynAngDeltaZ: DynamicVars = 31; + +//////// import: "enumgen.go" +const BodyVarsN: BodyVars = 37; +const ContactVarsN: ContactVars = 9; +const JointControlVarsN: JointControlVars = 19; +const DynamicVarsN: DynamicVars = 32; +const GPUVarsN: GPUVars = 7; +const JointVarsN: JointVars = 53; +const JointTypesN: JointTypes = 7; +const ShapesN: Shapes = 4; + +//////// import: "joint.go" +alias JointVars = i32; //enums:enum +const JointType: JointVars = 0; +const JointEnabled: JointVars = 1; +const JointParent: JointVars = 2; +const JointChild: JointVars = 3; +const JointPPosX: JointVars = 4; +const JointPPosY: JointVars = 5; +const JointPPosZ: JointVars = 6; +const JointPRotX: JointVars = 7; +const JointPRotY: JointVars = 8; +const JointPRotZ: JointVars = 9; +const JointPRotW: JointVars = 10; +const JointCPosX: JointVars = 11; +const JointCPosY: JointVars = 12; +const JointCPosZ: JointVars = 13; +const JointCRotX: JointVars = 14; +const JointCRotY: JointVars = 15; +const JointCRotZ: JointVars = 16; +const JointCRotW: JointVars = 17; +const JointAxisX: JointVars = 18; +const JointAxisY: JointVars = 19; +const JointAxisZ: JointVars = 20; +const JointLimitLower: JointVars = 21; +const JointLimitUpper: JointVars = 22; +const JointStiffX: JointVars = 23; +const JointStiffY: JointVars = 24; +const JointStiffZ: JointVars = 25; +const JointDampX: JointVars = 26; +const JointDampY: JointVars = 27; +const JointDampZ: JointVars = 28; +const JointPForceX: JointVars = 29; +const JointPForceY: JointVars = 30; +const JointPForceZ: JointVars = 31; +const JointPTorqueX: JointVars = 32; +const JointPTorqueY: JointVars = 33; +const JointPTorqueZ: JointVars = 34; +const JointCForceX: JointVars = 35; +const JointCForceY: JointVars = 36; +const JointCForceZ: JointVars = 37; +const JointCTorqueX: JointVars = 38; +const JointCTorqueY: JointVars = 39; +const JointCTorqueZ: JointVars = 40; +const JointPDeltaX: JointVars = 41; +const JointPDeltaY: JointVars = 42; +const JointPDeltaZ: JointVars = 43; +const JointPAngDeltaX: JointVars = 44; +const JointPAngDeltaY: JointVars = 45; +const JointPAngDeltaZ: JointVars = 46; +const JointCDeltaX: JointVars = 47; +const JointCDeltaY: JointVars = 48; +const JointCDeltaZ: JointVars = 49; +const JointCAngDeltaX: JointVars = 50; +const JointCAngDeltaY: JointVars = 51; +const JointCAngDeltaZ: JointVars = 52; +alias JointTypes = i32; //enums:enum +const Prismatic: JointTypes = 0; +const Revolute: JointTypes = 1; +const Ball: JointTypes = 2; +const Fixed: JointTypes = 3; +const Free: JointTypes = 4; +const Distance: JointTypes = 5; +const D6: JointTypes = 6; + +//////// import: "params.go" +struct PhysParams { + DynamicsN: i32, + JointsN: i32, + Cur: i32, + Next: i32, + Iters: i32, + Dt: f32, + SoftRelax: f32, + JointLinearRelax: f32, + JointAngularRelax: f32, + JointLinearComply: f32, + JointAngularComply: f32, + ContactRelax: f32, + AngularDamping: f32, + ContactWeighting: i32, + Restitution: i32, + pad: f32, + Gravity: vec4, +} + +//////// import: "shapes.go" +alias Shapes = i32; //enums:enum +const Box: Shapes = 0; +const Sphere: Shapes = 1; +const Cylinder: Shapes = 2; +const Capsule: Shapes = 3; + +//////// import: "slmath-matrix3.go" + +//////// import: "slmath-quaternion.go" + +//////// import: "slmath-vector3.go" + +//////// import: "step.go" + +//////// import: "step_body.go" +fn DynamicsCurToNext(i: u32) { //gosl:kernel + let pars = Params[0]; + var ii = i32(i); + if (ii >= pars.DynamicsN) { + return; + } + for (var di = DynIndex; di < DynamicVarsN; di++) { + Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(ii), u32(pars.Next), u32(di))] = Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(ii), u32(pars.Cur), u32(di))]; + } +} + +//////// import: "step_joint.go" \ No newline at end of file diff --git a/physics/shaders/InitDynamics.wgsl b/physics/shaders/InitDynamics.wgsl index 902aeae4..d419e946 100644 --- a/physics/shaders/InitDynamics.wgsl +++ b/physics/shaders/InitDynamics.wgsl @@ -151,7 +151,7 @@ const ContactVarsN: ContactVars = 9; const JointControlVarsN: JointControlVars = 19; const DynamicVarsN: DynamicVars = 32; const GPUVarsN: GPUVars = 7; -const JointVarsN: JointVars = 41; +const JointVarsN: JointVars = 53; const JointTypesN: JointTypes = 7; const ShapesN: Shapes = 4; @@ -198,6 +198,18 @@ const JointCForceZ: JointVars = 37; const JointCTorqueX: JointVars = 38; const JointCTorqueY: JointVars = 39; const JointCTorqueZ: JointVars = 40; +const JointPDeltaX: JointVars = 41; +const JointPDeltaY: JointVars = 42; +const JointPDeltaZ: JointVars = 43; +const JointPAngDeltaX: JointVars = 44; +const JointPAngDeltaY: JointVars = 45; +const JointPAngDeltaZ: JointVars = 46; +const JointCDeltaX: JointVars = 47; +const JointCDeltaY: JointVars = 48; +const JointCDeltaZ: JointVars = 49; +const JointCAngDeltaX: JointVars = 50; +const JointCAngDeltaY: JointVars = 51; +const JointCAngDeltaZ: JointVars = 52; alias JointTypes = i32; //enums:enum const Prismatic: JointTypes = 0; const Revolute: JointTypes = 1; diff --git a/physics/shaders/StepBodyDeltas.wgsl b/physics/shaders/StepBodyDeltas.wgsl new file mode 100644 index 00000000..e1e7eec1 --- /dev/null +++ b/physics/shaders/StepBodyDeltas.wgsl @@ -0,0 +1,392 @@ +// Code generated by "gosl"; DO NOT EDIT +// kernel: StepBodyDeltas + +// // Params are global parameters. +@group(0) @binding(0) +var TensorStrides: array; +@group(0) @binding(1) +var Params: array; +// // Bodies are the rigid body elements (dynamic and static), // specifying the constant, non-dynamic properties, // which is initial state for dynamics. // [body][BodyVarsN] +@group(1) @binding(0) +var Bodies: array; +// // Dynamics are the dynamic rigid body elements: these actually move. // Two alternating states are used: Params.Cur and Params.Next. // [dyn body][cur/next][DynamicVarsN] +@group(2) @binding(0) +var Dynamics: array; +// // JointControls are dynamic joint control inputs. // [joint][JointControlVarsN] + +alias GPUVars = i32; + +@compute @workgroup_size(64, 1, 1) +fn main(@builtin(workgroup_id) wgid: vec3, @builtin(num_workgroups) nwg: vec3, @builtin(local_invocation_index) loci: u32) { + let idx = loci + (wgid.x + wgid.y * nwg.x + wgid.z * nwg.x * nwg.y) * 64; + StepBodyDeltas(idx); +} + +fn Index2D(s0: u32, s1: u32, i0: u32, i1: u32) -> u32 { + return s0 * i0 + s1 * i1; +} + +fn Index3D(s0: u32, s1: u32, s2: u32, i0: u32, i1: u32, i2: u32) -> u32 { + return s0 * i0 + s1 * i1 + s2 * i2; +} + + +//////// import: "vars.go" + +//////// import: "body.go" +alias BodyVars = i32; //enums:enum +const BodyShape: BodyVars = 0; +const BodyWorldIndex: BodyVars = 1; +const BodySizeX: BodyVars = 2; +const BodySizeY: BodyVars = 3; +const BodySizeZ: BodyVars = 4; +const BodyMass: BodyVars = 5; +const BodyInvMass: BodyVars = 6; +const BodyBounce: BodyVars = 7; +const BodyFriction: BodyVars = 8; +const BodyPosX: BodyVars = 9; +const BodyPosY: BodyVars = 10; +const BodyPosZ: BodyVars = 11; +const BodyRotX: BodyVars = 12; +const BodyRotY: BodyVars = 13; +const BodyRotZ: BodyVars = 14; +const BodyRotW: BodyVars = 15; +const BodyComX: BodyVars = 16; +const BodyComY: BodyVars = 17; +const BodyComZ: BodyVars = 18; +const BodyInertiaXX: BodyVars = 19; +const BodyInertiaYX: BodyVars = 20; +const BodyInertiaZX: BodyVars = 21; +const BodyInertiaXY: BodyVars = 22; +const BodyInertiaYY: BodyVars = 23; +const BodyInertiaZY: BodyVars = 24; +const BodyInertiaXZ: BodyVars = 25; +const BodyInertiaYZ: BodyVars = 26; +const BodyInertiaZZ: BodyVars = 27; +const BodyInvInertiaXX: BodyVars = 28; +const BodyInvInertiaYX: BodyVars = 29; +const BodyInvInertiaZX: BodyVars = 30; +const BodyInvInertiaXY: BodyVars = 31; +const BodyInvInertiaYY: BodyVars = 32; +const BodyInvInertiaZY: BodyVars = 33; +const BodyInvInertiaXZ: BodyVars = 34; +const BodyInvInertiaYZ: BodyVars = 35; +const BodyInvInertiaZZ: BodyVars = 36; +fn BodyCom(idx: i32) -> vec3 { + return vec3(Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComX))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComY))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComZ))]); +} +fn BodyInertia(idx: i32) -> mat3x3f { + return mat3x3f(Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInertiaXX))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInertiaYX))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInertiaZX))], + Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInertiaXY))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInertiaYY))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInertiaZY))], + Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInertiaXZ))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInertiaYZ))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInertiaZZ))]); +} +fn BodyInvInertia(idx: i32) -> mat3x3f { + return mat3x3f(Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInvInertiaXX))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInvInertiaYX))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInvInertiaZX))], + Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInvInertiaXY))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInvInertiaYY))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInvInertiaZY))], + Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInvInertiaXZ))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInvInertiaYZ))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInvInertiaZZ))]); +} + +//////// import: "contact.go" +alias ContactVars = i32; //enums:enum +const ContactA: ContactVars = 0; +const ContactB: ContactVars = 1; +const ContactNormX: ContactVars = 2; +const ContactNormY: ContactVars = 3; +const ContactNormZ: ContactVars = 4; +const ContactPointX: ContactVars = 5; +const ContactPointY: ContactVars = 6; +const ContactPointZ: ContactVars = 7; +const ContactDist: ContactVars = 8; + +//////// import: "control.go" +alias JointControlVars = i32; //enums:enum +const JointCtrlForceX: JointControlVars = 0; +const JointCtrlForceY: JointControlVars = 1; +const JointCtrlForceZ: JointControlVars = 2; +const JointCtrlTorqueX: JointControlVars = 3; +const JointCtrlTorqueY: JointControlVars = 4; +const JointCtrlTorqueZ: JointControlVars = 5; +const JointTargetPosX: JointControlVars = 6; +const JointTargetPosY: JointControlVars = 7; +const JointTargetPosZ: JointControlVars = 8; +const JointTargetRotX: JointControlVars = 9; +const JointTargetRotY: JointControlVars = 10; +const JointTargetRotZ: JointControlVars = 11; +const JointTargetRotW: JointControlVars = 12; +const JointTargetVelX: JointControlVars = 13; +const JointTargetVelY: JointControlVars = 14; +const JointTargetVelZ: JointControlVars = 15; +const JointTargetAngVelX: JointControlVars = 16; +const JointTargetAngVelY: JointControlVars = 17; +const JointTargetAngVelZ: JointControlVars = 18; + +//////// import: "dynamics.go" +alias DynamicVars = i32; //enums:enum +const DynIndex: DynamicVars = 0; +const DynPosX: DynamicVars = 1; +const DynPosY: DynamicVars = 2; +const DynPosZ: DynamicVars = 3; +const DynRotX: DynamicVars = 4; +const DynRotY: DynamicVars = 5; +const DynRotZ: DynamicVars = 6; +const DynRotW: DynamicVars = 7; +const DynVelX: DynamicVars = 8; +const DynVelY: DynamicVars = 9; +const DynVelZ: DynamicVars = 10; +const DynAngVelX: DynamicVars = 11; +const DynAngVelY: DynamicVars = 12; +const DynAngVelZ: DynamicVars = 13; +const DynAccX: DynamicVars = 14; +const DynAccY: DynamicVars = 15; +const DynAccZ: DynamicVars = 16; +const DynAngAccX: DynamicVars = 17; +const DynAngAccY: DynamicVars = 18; +const DynAngAccZ: DynamicVars = 19; +const DynForceX: DynamicVars = 20; +const DynForceY: DynamicVars = 21; +const DynForceZ: DynamicVars = 22; +const DynTorqueX: DynamicVars = 23; +const DynTorqueY: DynamicVars = 24; +const DynTorqueZ: DynamicVars = 25; +const DynDeltaX: DynamicVars = 26; +const DynDeltaY: DynamicVars = 27; +const DynDeltaZ: DynamicVars = 28; +const DynAngDeltaX: DynamicVars = 29; +const DynAngDeltaY: DynamicVars = 30; +const DynAngDeltaZ: DynamicVars = 31; +fn DynamicIndex(idx: i32,cni: i32) -> i32 { + return i32(bitcast(Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynIndex))])); +} +fn DynamicPos(idx: i32,cni: i32) -> vec3 { + return vec3(Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynPosX))], Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynPosY))], Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynPosZ))]); +} +fn SetDynamicPos(idx: i32,cni: i32, pos: vec3) { + Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynPosX))] = pos.x; + Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynPosY))] = pos.y; + Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynPosZ))] = pos.z; +} +fn DynamicRot(idx: i32,cni: i32) -> vec4 { + return vec4(Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynRotX))], Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynRotY))], Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynRotZ))], Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynRotW))]); +} +fn SetDynamicRot(idx: i32,cni: i32, rot: vec4) { + Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynRotX))] = rot.x; + Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynRotY))] = rot.y; + Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynRotZ))] = rot.z; + Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynRotW))] = rot.w; +} +fn DynamicDelta(idx: i32,cni: i32) -> vec3 { + return vec3(Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynDeltaX))], Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynDeltaY))], Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynDeltaZ))]); +} +fn SetDynamicDelta(idx: i32,cni: i32, delta: vec3) { + Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynDeltaX))] = delta.x; + Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynDeltaY))] = delta.y; + Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynDeltaZ))] = delta.z; +} +fn DynamicAngDelta(idx: i32,cni: i32) -> vec3 { + return vec3(Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynAngDeltaX))], Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynAngDeltaY))], Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynAngDeltaZ))]); +} +fn SetDynamicAngDelta(idx: i32,cni: i32, angDelta: vec3) { + Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynAngDeltaX))] = angDelta.x; + Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynAngDeltaY))] = angDelta.y; + Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynAngDeltaZ))] = angDelta.z; +} + +//////// import: "enumgen.go" +const BodyVarsN: BodyVars = 37; +const ContactVarsN: ContactVars = 9; +const JointControlVarsN: JointControlVars = 19; +const DynamicVarsN: DynamicVars = 32; +const GPUVarsN: GPUVars = 7; +const JointVarsN: JointVars = 53; +const JointTypesN: JointTypes = 7; +const ShapesN: Shapes = 4; + +//////// import: "joint.go" +alias JointVars = i32; //enums:enum +const JointType: JointVars = 0; +const JointEnabled: JointVars = 1; +const JointParent: JointVars = 2; +const JointChild: JointVars = 3; +const JointPPosX: JointVars = 4; +const JointPPosY: JointVars = 5; +const JointPPosZ: JointVars = 6; +const JointPRotX: JointVars = 7; +const JointPRotY: JointVars = 8; +const JointPRotZ: JointVars = 9; +const JointPRotW: JointVars = 10; +const JointCPosX: JointVars = 11; +const JointCPosY: JointVars = 12; +const JointCPosZ: JointVars = 13; +const JointCRotX: JointVars = 14; +const JointCRotY: JointVars = 15; +const JointCRotZ: JointVars = 16; +const JointCRotW: JointVars = 17; +const JointAxisX: JointVars = 18; +const JointAxisY: JointVars = 19; +const JointAxisZ: JointVars = 20; +const JointLimitLower: JointVars = 21; +const JointLimitUpper: JointVars = 22; +const JointStiffX: JointVars = 23; +const JointStiffY: JointVars = 24; +const JointStiffZ: JointVars = 25; +const JointDampX: JointVars = 26; +const JointDampY: JointVars = 27; +const JointDampZ: JointVars = 28; +const JointPForceX: JointVars = 29; +const JointPForceY: JointVars = 30; +const JointPForceZ: JointVars = 31; +const JointPTorqueX: JointVars = 32; +const JointPTorqueY: JointVars = 33; +const JointPTorqueZ: JointVars = 34; +const JointCForceX: JointVars = 35; +const JointCForceY: JointVars = 36; +const JointCForceZ: JointVars = 37; +const JointCTorqueX: JointVars = 38; +const JointCTorqueY: JointVars = 39; +const JointCTorqueZ: JointVars = 40; +const JointPDeltaX: JointVars = 41; +const JointPDeltaY: JointVars = 42; +const JointPDeltaZ: JointVars = 43; +const JointPAngDeltaX: JointVars = 44; +const JointPAngDeltaY: JointVars = 45; +const JointPAngDeltaZ: JointVars = 46; +const JointCDeltaX: JointVars = 47; +const JointCDeltaY: JointVars = 48; +const JointCDeltaZ: JointVars = 49; +const JointCAngDeltaX: JointVars = 50; +const JointCAngDeltaY: JointVars = 51; +const JointCAngDeltaZ: JointVars = 52; +alias JointTypes = i32; //enums:enum +const Prismatic: JointTypes = 0; +const Revolute: JointTypes = 1; +const Ball: JointTypes = 2; +const Fixed: JointTypes = 3; +const Free: JointTypes = 4; +const Distance: JointTypes = 5; +const D6: JointTypes = 6; + +//////// import: "params.go" +struct PhysParams { + DynamicsN: i32, + JointsN: i32, + Cur: i32, + Next: i32, + Iters: i32, + Dt: f32, + SoftRelax: f32, + JointLinearRelax: f32, + JointAngularRelax: f32, + JointLinearComply: f32, + JointAngularComply: f32, + ContactRelax: f32, + AngularDamping: f32, + ContactWeighting: i32, + Restitution: i32, + pad: f32, + Gravity: vec4, +} + +//////// import: "shapes.go" +alias Shapes = i32; //enums:enum +const Box: Shapes = 0; +const Sphere: Shapes = 1; +const Cylinder: Shapes = 2; +const Capsule: Shapes = 3; + +//////// import: "slmath-matrix3.go" + +//////// import: "slmath-quaternion.go" +fn QuatLength(q: vec4) -> f32 { + return sqrt(q.x*q.x + q.y*q.y + q.z*q.z + q.w*q.w); +} +fn QuatNormalize(q: vec4) -> vec4 { + var nq: vec4; + var l = QuatLength(q); + if (l == 0) { + nq.x = f32(0); + nq.y = f32(0); + nq.z = f32(0); + nq.w = f32(1); + } else { + l = 1 / l; + nq.x *= l; + nq.y *= l; + nq.z *= l; + nq.w *= l; + }return nq; +} +fn MulQuatVector(q: vec4, v: vec3) -> vec3 { + var xyz = vec3(q.x, q.y, q.z); + var t = MulScalar3(Cross3(xyz, v), f32(f32(2)));return v+(MulScalar3(t, q.w))+(Cross3(xyz, t)); +} +fn MulQuatVectorInverse(q: vec4, v: vec3) -> vec3 { + var xyz = vec3(q.x, q.y, q.z); + var t = MulScalar3(Cross3(xyz, v), f32(f32(2)));return v-(MulScalar3(t, q.w))+(Cross3(xyz, t)); +} +fn MulQuats(a: vec4,b: vec4) -> vec4 { + var q: vec4; + q.x = a.x*b.w + a.w*b.x + a.y*b.z - a.z*b.y; + q.y = a.y*b.w + a.w*b.y + a.z*b.x - a.x*b.z; + q.z = a.z*b.w + a.w*b.z + a.x*b.y - a.y*b.x; + q.w = a.w*b.w - a.x*b.x - a.y*b.y - a.z*b.z;return q; +} + +//////// import: "slmath-vector3.go" +fn MulScalar3(v: vec3, s: f32) -> vec3 { + return vec3(v.x*s, v.y*s, v.z*s); +} +fn Length3(v: vec3) -> f32 { + return sqrt(v.x*v.x + v.y*v.y + v.z*v.z); +} +fn Cross3(v: vec3,o: vec3) -> vec3 { + return vec3(v.y*o.z-v.z*o.y, v.z*o.x-v.x*o.z, v.x*o.y-v.y*o.x); +} + +//////// import: "step.go" + +//////// import: "step_body.go" +fn StepBodyDeltas(i: u32) { //gosl:kernel + let pars = Params[0]; + var di = i32(i); + if (di >= pars.DynamicsN) { + return; + } + var bi = DynamicIndex(di, pars.Cur); + var invMass = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyInvMass))]; + if (invMass == 0) { + return; // no updates + } + var inertia = BodyInertia(bi); + var invInertia = BodyInvInertia(bi); + var p0 = DynamicPos(di, pars.Next); + var q0 = DynamicRot(di, pars.Next); + var v0 = DynamicDelta(di, pars.Next); + var w0 = DynamicAngDelta(di, pars.Next); + var weight = f32(1.0); + var dp = v0*(invMass * weight); + var dq = w0*(weight); + var wb = MulQuatVectorInverse(q0, w0); + var dwb = invInertia*(MulQuatVectorInverse(q0, dq)); + var tb = Cross3(dwb, inertia*(wb+(dwb)))+(Cross3(wb, inertia*(dwb))); + var dw1 = MulQuatVector(q0, dwb-(invInertia*(tb)*(pars.Dt))); + var q1 = q0+(MulQuats(vec4(dw1.x, dw1.y, dw1.z, 0), q0)*(0.5 * pars.Dt)); + q1 = QuatNormalize(q1); + var com = BodyCom(bi); + var pcom = MulQuatVector(q0, com)+(p0); + var p1 = pcom+(dp*(pars.Dt)); + p1 = p1-(MulQuatVector(q1, com)); + var v1 = v0+(dp); + var w1 = w0+(dw1); + if (Length3(v1) < 1e-4) { + v1 = vec3(0, 0, 0); + } + if (Length3(w1) < 1e-4) { + w1 = vec3(0, 0, 0); + } + SetDynamicPos(di, pars.Next, p1); + SetDynamicRot(di, pars.Next, q1); + SetDynamicDelta(di, pars.Next, v1); + SetDynamicAngDelta(di, pars.Next, w1); +} + +//////// import: "step_joint.go" \ No newline at end of file diff --git a/physics/shaders/StepIntegrateBodies.wgsl b/physics/shaders/StepIntegrateBodies.wgsl new file mode 100644 index 00000000..e061c866 --- /dev/null +++ b/physics/shaders/StepIntegrateBodies.wgsl @@ -0,0 +1,389 @@ +// Code generated by "gosl"; DO NOT EDIT +// kernel: StepIntegrateBodies + +// // Params are global parameters. +@group(0) @binding(0) +var TensorStrides: array; +@group(0) @binding(1) +var Params: array; +// // Bodies are the rigid body elements (dynamic and static), // specifying the constant, non-dynamic properties, // which is initial state for dynamics. // [body][BodyVarsN] +@group(1) @binding(0) +var Bodies: array; +// // Dynamics are the dynamic rigid body elements: these actually move. // Two alternating states are used: Params.Cur and Params.Next. // [dyn body][cur/next][DynamicVarsN] +@group(2) @binding(0) +var Dynamics: array; +// // JointControls are dynamic joint control inputs. // [joint][JointControlVarsN] + +alias GPUVars = i32; + +@compute @workgroup_size(64, 1, 1) +fn main(@builtin(workgroup_id) wgid: vec3, @builtin(num_workgroups) nwg: vec3, @builtin(local_invocation_index) loci: u32) { + let idx = loci + (wgid.x + wgid.y * nwg.x + wgid.z * nwg.x * nwg.y) * 64; + StepIntegrateBodies(idx); +} + +fn Index2D(s0: u32, s1: u32, i0: u32, i1: u32) -> u32 { + return s0 * i0 + s1 * i1; +} + +fn Index3D(s0: u32, s1: u32, s2: u32, i0: u32, i1: u32, i2: u32) -> u32 { + return s0 * i0 + s1 * i1 + s2 * i2; +} + + +//////// import: "vars.go" + +//////// import: "body.go" +alias BodyVars = i32; //enums:enum +const BodyShape: BodyVars = 0; +const BodyWorldIndex: BodyVars = 1; +const BodySizeX: BodyVars = 2; +const BodySizeY: BodyVars = 3; +const BodySizeZ: BodyVars = 4; +const BodyMass: BodyVars = 5; +const BodyInvMass: BodyVars = 6; +const BodyBounce: BodyVars = 7; +const BodyFriction: BodyVars = 8; +const BodyPosX: BodyVars = 9; +const BodyPosY: BodyVars = 10; +const BodyPosZ: BodyVars = 11; +const BodyRotX: BodyVars = 12; +const BodyRotY: BodyVars = 13; +const BodyRotZ: BodyVars = 14; +const BodyRotW: BodyVars = 15; +const BodyComX: BodyVars = 16; +const BodyComY: BodyVars = 17; +const BodyComZ: BodyVars = 18; +const BodyInertiaXX: BodyVars = 19; +const BodyInertiaYX: BodyVars = 20; +const BodyInertiaZX: BodyVars = 21; +const BodyInertiaXY: BodyVars = 22; +const BodyInertiaYY: BodyVars = 23; +const BodyInertiaZY: BodyVars = 24; +const BodyInertiaXZ: BodyVars = 25; +const BodyInertiaYZ: BodyVars = 26; +const BodyInertiaZZ: BodyVars = 27; +const BodyInvInertiaXX: BodyVars = 28; +const BodyInvInertiaYX: BodyVars = 29; +const BodyInvInertiaZX: BodyVars = 30; +const BodyInvInertiaXY: BodyVars = 31; +const BodyInvInertiaYY: BodyVars = 32; +const BodyInvInertiaZY: BodyVars = 33; +const BodyInvInertiaXZ: BodyVars = 34; +const BodyInvInertiaYZ: BodyVars = 35; +const BodyInvInertiaZZ: BodyVars = 36; +fn BodyCom(idx: i32) -> vec3 { + return vec3(Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComX))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComY))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComZ))]); +} +fn BodyInertia(idx: i32) -> mat3x3f { + return mat3x3f(Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInertiaXX))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInertiaYX))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInertiaZX))], + Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInertiaXY))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInertiaYY))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInertiaZY))], + Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInertiaXZ))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInertiaYZ))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInertiaZZ))]); +} +fn BodyInvInertia(idx: i32) -> mat3x3f { + return mat3x3f(Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInvInertiaXX))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInvInertiaYX))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInvInertiaZX))], + Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInvInertiaXY))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInvInertiaYY))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInvInertiaZY))], + Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInvInertiaXZ))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInvInertiaYZ))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInvInertiaZZ))]); +} + +//////// import: "contact.go" +alias ContactVars = i32; //enums:enum +const ContactA: ContactVars = 0; +const ContactB: ContactVars = 1; +const ContactNormX: ContactVars = 2; +const ContactNormY: ContactVars = 3; +const ContactNormZ: ContactVars = 4; +const ContactPointX: ContactVars = 5; +const ContactPointY: ContactVars = 6; +const ContactPointZ: ContactVars = 7; +const ContactDist: ContactVars = 8; + +//////// import: "control.go" +alias JointControlVars = i32; //enums:enum +const JointCtrlForceX: JointControlVars = 0; +const JointCtrlForceY: JointControlVars = 1; +const JointCtrlForceZ: JointControlVars = 2; +const JointCtrlTorqueX: JointControlVars = 3; +const JointCtrlTorqueY: JointControlVars = 4; +const JointCtrlTorqueZ: JointControlVars = 5; +const JointTargetPosX: JointControlVars = 6; +const JointTargetPosY: JointControlVars = 7; +const JointTargetPosZ: JointControlVars = 8; +const JointTargetRotX: JointControlVars = 9; +const JointTargetRotY: JointControlVars = 10; +const JointTargetRotZ: JointControlVars = 11; +const JointTargetRotW: JointControlVars = 12; +const JointTargetVelX: JointControlVars = 13; +const JointTargetVelY: JointControlVars = 14; +const JointTargetVelZ: JointControlVars = 15; +const JointTargetAngVelX: JointControlVars = 16; +const JointTargetAngVelY: JointControlVars = 17; +const JointTargetAngVelZ: JointControlVars = 18; + +//////// import: "dynamics.go" +alias DynamicVars = i32; //enums:enum +const DynIndex: DynamicVars = 0; +const DynPosX: DynamicVars = 1; +const DynPosY: DynamicVars = 2; +const DynPosZ: DynamicVars = 3; +const DynRotX: DynamicVars = 4; +const DynRotY: DynamicVars = 5; +const DynRotZ: DynamicVars = 6; +const DynRotW: DynamicVars = 7; +const DynVelX: DynamicVars = 8; +const DynVelY: DynamicVars = 9; +const DynVelZ: DynamicVars = 10; +const DynAngVelX: DynamicVars = 11; +const DynAngVelY: DynamicVars = 12; +const DynAngVelZ: DynamicVars = 13; +const DynAccX: DynamicVars = 14; +const DynAccY: DynamicVars = 15; +const DynAccZ: DynamicVars = 16; +const DynAngAccX: DynamicVars = 17; +const DynAngAccY: DynamicVars = 18; +const DynAngAccZ: DynamicVars = 19; +const DynForceX: DynamicVars = 20; +const DynForceY: DynamicVars = 21; +const DynForceZ: DynamicVars = 22; +const DynTorqueX: DynamicVars = 23; +const DynTorqueY: DynamicVars = 24; +const DynTorqueZ: DynamicVars = 25; +const DynDeltaX: DynamicVars = 26; +const DynDeltaY: DynamicVars = 27; +const DynDeltaZ: DynamicVars = 28; +const DynAngDeltaX: DynamicVars = 29; +const DynAngDeltaY: DynamicVars = 30; +const DynAngDeltaZ: DynamicVars = 31; +fn DynamicIndex(idx: i32,cni: i32) -> i32 { + return i32(bitcast(Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynIndex))])); +} +fn DynamicPos(idx: i32,cni: i32) -> vec3 { + return vec3(Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynPosX))], Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynPosY))], Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynPosZ))]); +} +fn SetDynamicPos(idx: i32,cni: i32, pos: vec3) { + Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynPosX))] = pos.x; + Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynPosY))] = pos.y; + Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynPosZ))] = pos.z; +} +fn DynamicRot(idx: i32,cni: i32) -> vec4 { + return vec4(Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynRotX))], Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynRotY))], Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynRotZ))], Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynRotW))]); +} +fn SetDynamicRot(idx: i32,cni: i32, rot: vec4) { + Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynRotX))] = rot.x; + Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynRotY))] = rot.y; + Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynRotZ))] = rot.z; + Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynRotW))] = rot.w; +} +fn DynamicForce(idx: i32,cni: i32) -> vec3 { + return vec3(Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynForceX))], Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynForceY))], Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynForceZ))]); +} +fn DynamicTorque(idx: i32,cni: i32) -> vec3 { + return vec3(Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynTorqueX))], Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynTorqueY))], Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynTorqueZ))]); +} +fn DynamicDelta(idx: i32,cni: i32) -> vec3 { + return vec3(Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynDeltaX))], Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynDeltaY))], Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynDeltaZ))]); +} +fn SetDynamicDelta(idx: i32,cni: i32, delta: vec3) { + Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynDeltaX))] = delta.x; + Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynDeltaY))] = delta.y; + Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynDeltaZ))] = delta.z; +} +fn DynamicAngDelta(idx: i32,cni: i32) -> vec3 { + return vec3(Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynAngDeltaX))], Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynAngDeltaY))], Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynAngDeltaZ))]); +} +fn SetDynamicAngDelta(idx: i32,cni: i32, angDelta: vec3) { + Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynAngDeltaX))] = angDelta.x; + Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynAngDeltaY))] = angDelta.y; + Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynAngDeltaZ))] = angDelta.z; +} + +//////// import: "enumgen.go" +const BodyVarsN: BodyVars = 37; +const ContactVarsN: ContactVars = 9; +const JointControlVarsN: JointControlVars = 19; +const DynamicVarsN: DynamicVars = 32; +const GPUVarsN: GPUVars = 7; +const JointVarsN: JointVars = 53; +const JointTypesN: JointTypes = 7; +const ShapesN: Shapes = 4; + +//////// import: "joint.go" +alias JointVars = i32; //enums:enum +const JointType: JointVars = 0; +const JointEnabled: JointVars = 1; +const JointParent: JointVars = 2; +const JointChild: JointVars = 3; +const JointPPosX: JointVars = 4; +const JointPPosY: JointVars = 5; +const JointPPosZ: JointVars = 6; +const JointPRotX: JointVars = 7; +const JointPRotY: JointVars = 8; +const JointPRotZ: JointVars = 9; +const JointPRotW: JointVars = 10; +const JointCPosX: JointVars = 11; +const JointCPosY: JointVars = 12; +const JointCPosZ: JointVars = 13; +const JointCRotX: JointVars = 14; +const JointCRotY: JointVars = 15; +const JointCRotZ: JointVars = 16; +const JointCRotW: JointVars = 17; +const JointAxisX: JointVars = 18; +const JointAxisY: JointVars = 19; +const JointAxisZ: JointVars = 20; +const JointLimitLower: JointVars = 21; +const JointLimitUpper: JointVars = 22; +const JointStiffX: JointVars = 23; +const JointStiffY: JointVars = 24; +const JointStiffZ: JointVars = 25; +const JointDampX: JointVars = 26; +const JointDampY: JointVars = 27; +const JointDampZ: JointVars = 28; +const JointPForceX: JointVars = 29; +const JointPForceY: JointVars = 30; +const JointPForceZ: JointVars = 31; +const JointPTorqueX: JointVars = 32; +const JointPTorqueY: JointVars = 33; +const JointPTorqueZ: JointVars = 34; +const JointCForceX: JointVars = 35; +const JointCForceY: JointVars = 36; +const JointCForceZ: JointVars = 37; +const JointCTorqueX: JointVars = 38; +const JointCTorqueY: JointVars = 39; +const JointCTorqueZ: JointVars = 40; +const JointPDeltaX: JointVars = 41; +const JointPDeltaY: JointVars = 42; +const JointPDeltaZ: JointVars = 43; +const JointPAngDeltaX: JointVars = 44; +const JointPAngDeltaY: JointVars = 45; +const JointPAngDeltaZ: JointVars = 46; +const JointCDeltaX: JointVars = 47; +const JointCDeltaY: JointVars = 48; +const JointCDeltaZ: JointVars = 49; +const JointCAngDeltaX: JointVars = 50; +const JointCAngDeltaY: JointVars = 51; +const JointCAngDeltaZ: JointVars = 52; +alias JointTypes = i32; //enums:enum +const Prismatic: JointTypes = 0; +const Revolute: JointTypes = 1; +const Ball: JointTypes = 2; +const Fixed: JointTypes = 3; +const Free: JointTypes = 4; +const Distance: JointTypes = 5; +const D6: JointTypes = 6; + +//////// import: "params.go" +struct PhysParams { + DynamicsN: i32, + JointsN: i32, + Cur: i32, + Next: i32, + Iters: i32, + Dt: f32, + SoftRelax: f32, + JointLinearRelax: f32, + JointAngularRelax: f32, + JointLinearComply: f32, + JointAngularComply: f32, + ContactRelax: f32, + AngularDamping: f32, + ContactWeighting: i32, + Restitution: i32, + pad: f32, + Gravity: vec4, +} + +//////// import: "shapes.go" +alias Shapes = i32; //enums:enum +const Box: Shapes = 0; +const Sphere: Shapes = 1; +const Cylinder: Shapes = 2; +const Capsule: Shapes = 3; + +//////// import: "slmath-matrix3.go" + +//////// import: "slmath-quaternion.go" +fn QuatLength(q: vec4) -> f32 { + return sqrt(q.x*q.x + q.y*q.y + q.z*q.z + q.w*q.w); +} +fn QuatNormalize(q: vec4) -> vec4 { + var nq: vec4; + var l = QuatLength(q); + if (l == 0) { + nq.x = f32(0); + nq.y = f32(0); + nq.z = f32(0); + nq.w = f32(1); + } else { + l = 1 / l; + nq.x *= l; + nq.y *= l; + nq.z *= l; + nq.w *= l; + }return nq; +} +fn MulQuatVector(q: vec4, v: vec3) -> vec3 { + var xyz = vec3(q.x, q.y, q.z); + var t = MulScalar3(Cross3(xyz, v), f32(f32(2)));return v+(MulScalar3(t, q.w))+(Cross3(xyz, t)); +} +fn MulQuatVectorInverse(q: vec4, v: vec3) -> vec3 { + var xyz = vec3(q.x, q.y, q.z); + var t = MulScalar3(Cross3(xyz, v), f32(f32(2)));return v-(MulScalar3(t, q.w))+(Cross3(xyz, t)); +} +fn MulQuats(a: vec4,b: vec4) -> vec4 { + var q: vec4; + q.x = a.x*b.w + a.w*b.x + a.y*b.z - a.z*b.y; + q.y = a.y*b.w + a.w*b.y + a.z*b.x - a.x*b.z; + q.z = a.z*b.w + a.w*b.z + a.x*b.y - a.y*b.x; + q.w = a.w*b.w - a.x*b.x - a.y*b.y - a.z*b.z;return q; +} + +//////// import: "slmath-vector3.go" +fn MulScalar3(v: vec3, s: f32) -> vec3 { + return vec3(v.x*s, v.y*s, v.z*s); +} +fn Cross3(v: vec3,o: vec3) -> vec3 { + return vec3(v.y*o.z-v.z*o.y, v.z*o.x-v.x*o.z, v.x*o.y-v.y*o.x); +} + +//////// import: "step.go" +fn OneIfNonzero(f: f32) -> f32 { + if (f != 0.0) { + return f32(1.0); + }return f32(0.0); +} + +//////// import: "step_body.go" +fn StepIntegrateBodies(i: u32) { //gosl:kernel + let pars = Params[0]; + var di = i32(i); + if (di >= pars.DynamicsN) { + return; + } + var bi = DynamicIndex(di, pars.Cur); + var invMass = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyInvMass))]; + var inertia = BodyInertia(bi); + var invInertia = BodyInvInertia(bi); + var com = BodyCom(bi); + var p0 = DynamicPos(di, pars.Cur); + var q0 = DynamicRot(di, pars.Cur); + var v0 = DynamicDelta(di, pars.Cur); + var w0 = DynamicAngDelta(di, pars.Cur); + var f0 = DynamicForce(di, pars.Next); + var t0 = DynamicTorque(di, pars.Next); + var pcom = MulQuatVector(q0, com)+(p0); + var v1 = v0+(f0*(invMass)+(vec3(pars.Gravity.x,pars.Gravity.y,pars.Gravity.z)*(OneIfNonzero(invMass)))*(pars.Dt)); + var p1 = pcom+(v1*(pars.Dt)); + var wb = MulQuatVectorInverse(q0, w0); + var tb = MulQuatVectorInverse(q0, t0)-(Cross3(wb, inertia*(wb))); // coriolis forces + var w1 = MulQuatVector(q0, wb+(invInertia*(tb)*(pars.Dt))); + var q1 = MulQuats(vec4(w1.x, w1.y, w1.z, 0), q0)*(0.5 * pars.Dt); + q1 = QuatNormalize(q1); + w1 = w1*(1.0 - pars.AngularDamping*pars.Dt); + var p1a = p1-(MulQuatVector(q1, com)); // pos corrected to nominal center. + SetDynamicPos(di, pars.Next, p1a); + SetDynamicRot(di, pars.Next, q1); + SetDynamicDelta(di, pars.Next, v1); + SetDynamicAngDelta(di, pars.Next, w1); +} + +//////// import: "step_joint.go" \ No newline at end of file diff --git a/physics/shaders/StepJointForces.wgsl b/physics/shaders/StepJointForces.wgsl new file mode 100644 index 00000000..f243c9da --- /dev/null +++ b/physics/shaders/StepJointForces.wgsl @@ -0,0 +1,396 @@ +// Code generated by "gosl"; DO NOT EDIT +// kernel: StepJointForces + +// // Params are global parameters. +@group(0) @binding(0) +var TensorStrides: array; +@group(0) @binding(1) +var Params: array; +// // Bodies are the rigid body elements (dynamic and static), // specifying the constant, non-dynamic properties, // which is initial state for dynamics. // [body][BodyVarsN] +@group(1) @binding(0) +var Bodies: array; +@group(1) @binding(1) +var Joints: array; +// // Dynamics are the dynamic rigid body elements: these actually move. // Two alternating states are used: Params.Cur and Params.Next. // [dyn body][cur/next][DynamicVarsN] +@group(2) @binding(0) +var Dynamics: array; +// // JointControls are dynamic joint control inputs. // [joint][JointControlVarsN] +@group(3) @binding(0) +var JointControls: array; + +alias GPUVars = i32; + +@compute @workgroup_size(64, 1, 1) +fn main(@builtin(workgroup_id) wgid: vec3, @builtin(num_workgroups) nwg: vec3, @builtin(local_invocation_index) loci: u32) { + let idx = loci + (wgid.x + wgid.y * nwg.x + wgid.z * nwg.x * nwg.y) * 64; + StepJointForces(idx); +} + +fn Index2D(s0: u32, s1: u32, i0: u32, i1: u32) -> u32 { + return s0 * i0 + s1 * i1; +} + +fn Index3D(s0: u32, s1: u32, s2: u32, i0: u32, i1: u32, i2: u32) -> u32 { + return s0 * i0 + s1 * i1 + s2 * i2; +} + + +//////// import: "vars.go" + +//////// import: "body.go" +alias BodyVars = i32; //enums:enum +const BodyShape: BodyVars = 0; +const BodyWorldIndex: BodyVars = 1; +const BodySizeX: BodyVars = 2; +const BodySizeY: BodyVars = 3; +const BodySizeZ: BodyVars = 4; +const BodyMass: BodyVars = 5; +const BodyInvMass: BodyVars = 6; +const BodyBounce: BodyVars = 7; +const BodyFriction: BodyVars = 8; +const BodyPosX: BodyVars = 9; +const BodyPosY: BodyVars = 10; +const BodyPosZ: BodyVars = 11; +const BodyRotX: BodyVars = 12; +const BodyRotY: BodyVars = 13; +const BodyRotZ: BodyVars = 14; +const BodyRotW: BodyVars = 15; +const BodyComX: BodyVars = 16; +const BodyComY: BodyVars = 17; +const BodyComZ: BodyVars = 18; +const BodyInertiaXX: BodyVars = 19; +const BodyInertiaYX: BodyVars = 20; +const BodyInertiaZX: BodyVars = 21; +const BodyInertiaXY: BodyVars = 22; +const BodyInertiaYY: BodyVars = 23; +const BodyInertiaZY: BodyVars = 24; +const BodyInertiaXZ: BodyVars = 25; +const BodyInertiaYZ: BodyVars = 26; +const BodyInertiaZZ: BodyVars = 27; +const BodyInvInertiaXX: BodyVars = 28; +const BodyInvInertiaYX: BodyVars = 29; +const BodyInvInertiaZX: BodyVars = 30; +const BodyInvInertiaXY: BodyVars = 31; +const BodyInvInertiaYY: BodyVars = 32; +const BodyInvInertiaZY: BodyVars = 33; +const BodyInvInertiaXZ: BodyVars = 34; +const BodyInvInertiaYZ: BodyVars = 35; +const BodyInvInertiaZZ: BodyVars = 36; +fn BodyCom(idx: i32) -> vec3 { + return vec3(Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComX))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComY))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComZ))]); +} + +//////// import: "contact.go" +alias ContactVars = i32; //enums:enum +const ContactA: ContactVars = 0; +const ContactB: ContactVars = 1; +const ContactNormX: ContactVars = 2; +const ContactNormY: ContactVars = 3; +const ContactNormZ: ContactVars = 4; +const ContactPointX: ContactVars = 5; +const ContactPointY: ContactVars = 6; +const ContactPointZ: ContactVars = 7; +const ContactDist: ContactVars = 8; + +//////// import: "control.go" +alias JointControlVars = i32; //enums:enum +const JointCtrlForceX: JointControlVars = 0; +const JointCtrlForceY: JointControlVars = 1; +const JointCtrlForceZ: JointControlVars = 2; +const JointCtrlTorqueX: JointControlVars = 3; +const JointCtrlTorqueY: JointControlVars = 4; +const JointCtrlTorqueZ: JointControlVars = 5; +const JointTargetPosX: JointControlVars = 6; +const JointTargetPosY: JointControlVars = 7; +const JointTargetPosZ: JointControlVars = 8; +const JointTargetRotX: JointControlVars = 9; +const JointTargetRotY: JointControlVars = 10; +const JointTargetRotZ: JointControlVars = 11; +const JointTargetRotW: JointControlVars = 12; +const JointTargetVelX: JointControlVars = 13; +const JointTargetVelY: JointControlVars = 14; +const JointTargetVelZ: JointControlVars = 15; +const JointTargetAngVelX: JointControlVars = 16; +const JointTargetAngVelY: JointControlVars = 17; +const JointTargetAngVelZ: JointControlVars = 18; +fn JointControlForce(idx: i32) -> vec3 { + return vec3(JointControls[Index2D(TensorStrides[50], TensorStrides[51], u32(idx), u32(JointCtrlForceX))], JointControls[Index2D(TensorStrides[50], TensorStrides[51], u32(idx), u32(JointCtrlForceY))], JointControls[Index2D(TensorStrides[50], TensorStrides[51], u32(idx), u32(JointCtrlForceZ))]); +} +fn JointControlTorque(idx: i32) -> vec3 { + return vec3(JointControls[Index2D(TensorStrides[50], TensorStrides[51], u32(idx), u32(JointCtrlTorqueX))], JointControls[Index2D(TensorStrides[50], TensorStrides[51], u32(idx), u32(JointCtrlTorqueY))], JointControls[Index2D(TensorStrides[50], TensorStrides[51], u32(idx), u32(JointCtrlTorqueZ))]); +} + +//////// import: "dynamics.go" +alias DynamicVars = i32; //enums:enum +const DynIndex: DynamicVars = 0; +const DynPosX: DynamicVars = 1; +const DynPosY: DynamicVars = 2; +const DynPosZ: DynamicVars = 3; +const DynRotX: DynamicVars = 4; +const DynRotY: DynamicVars = 5; +const DynRotZ: DynamicVars = 6; +const DynRotW: DynamicVars = 7; +const DynVelX: DynamicVars = 8; +const DynVelY: DynamicVars = 9; +const DynVelZ: DynamicVars = 10; +const DynAngVelX: DynamicVars = 11; +const DynAngVelY: DynamicVars = 12; +const DynAngVelZ: DynamicVars = 13; +const DynAccX: DynamicVars = 14; +const DynAccY: DynamicVars = 15; +const DynAccZ: DynamicVars = 16; +const DynAngAccX: DynamicVars = 17; +const DynAngAccY: DynamicVars = 18; +const DynAngAccZ: DynamicVars = 19; +const DynForceX: DynamicVars = 20; +const DynForceY: DynamicVars = 21; +const DynForceZ: DynamicVars = 22; +const DynTorqueX: DynamicVars = 23; +const DynTorqueY: DynamicVars = 24; +const DynTorqueZ: DynamicVars = 25; +const DynDeltaX: DynamicVars = 26; +const DynDeltaY: DynamicVars = 27; +const DynDeltaZ: DynamicVars = 28; +const DynAngDeltaX: DynamicVars = 29; +const DynAngDeltaY: DynamicVars = 30; +const DynAngDeltaZ: DynamicVars = 31; +fn DynamicIndex(idx: i32,cni: i32) -> i32 { + return i32(bitcast(Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynIndex))])); +} +fn DynamicPos(idx: i32,cni: i32) -> vec3 { + return vec3(Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynPosX))], Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynPosY))], Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynPosZ))]); +} +fn DynamicRot(idx: i32,cni: i32) -> vec4 { + return vec4(Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynRotX))], Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynRotY))], Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynRotZ))], Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynRotW))]); +} + +//////// import: "enumgen.go" +const BodyVarsN: BodyVars = 37; +const ContactVarsN: ContactVars = 9; +const JointControlVarsN: JointControlVars = 19; +const DynamicVarsN: DynamicVars = 32; +const GPUVarsN: GPUVars = 7; +const JointVarsN: JointVars = 53; +const JointTypesN: JointTypes = 7; +const ShapesN: Shapes = 4; + +//////// import: "joint.go" +alias JointVars = i32; //enums:enum +const JointType: JointVars = 0; +const JointEnabled: JointVars = 1; +const JointParent: JointVars = 2; +const JointChild: JointVars = 3; +const JointPPosX: JointVars = 4; +const JointPPosY: JointVars = 5; +const JointPPosZ: JointVars = 6; +const JointPRotX: JointVars = 7; +const JointPRotY: JointVars = 8; +const JointPRotZ: JointVars = 9; +const JointPRotW: JointVars = 10; +const JointCPosX: JointVars = 11; +const JointCPosY: JointVars = 12; +const JointCPosZ: JointVars = 13; +const JointCRotX: JointVars = 14; +const JointCRotY: JointVars = 15; +const JointCRotZ: JointVars = 16; +const JointCRotW: JointVars = 17; +const JointAxisX: JointVars = 18; +const JointAxisY: JointVars = 19; +const JointAxisZ: JointVars = 20; +const JointLimitLower: JointVars = 21; +const JointLimitUpper: JointVars = 22; +const JointStiffX: JointVars = 23; +const JointStiffY: JointVars = 24; +const JointStiffZ: JointVars = 25; +const JointDampX: JointVars = 26; +const JointDampY: JointVars = 27; +const JointDampZ: JointVars = 28; +const JointPForceX: JointVars = 29; +const JointPForceY: JointVars = 30; +const JointPForceZ: JointVars = 31; +const JointPTorqueX: JointVars = 32; +const JointPTorqueY: JointVars = 33; +const JointPTorqueZ: JointVars = 34; +const JointCForceX: JointVars = 35; +const JointCForceY: JointVars = 36; +const JointCForceZ: JointVars = 37; +const JointCTorqueX: JointVars = 38; +const JointCTorqueY: JointVars = 39; +const JointCTorqueZ: JointVars = 40; +const JointPDeltaX: JointVars = 41; +const JointPDeltaY: JointVars = 42; +const JointPDeltaZ: JointVars = 43; +const JointPAngDeltaX: JointVars = 44; +const JointPAngDeltaY: JointVars = 45; +const JointPAngDeltaZ: JointVars = 46; +const JointCDeltaX: JointVars = 47; +const JointCDeltaY: JointVars = 48; +const JointCDeltaZ: JointVars = 49; +const JointCAngDeltaX: JointVars = 50; +const JointCAngDeltaY: JointVars = 51; +const JointCAngDeltaZ: JointVars = 52; +fn GetJointType(idx: i32) -> JointTypes { + return JointTypes(bitcast(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointType))])); +} +fn JointParentIndex(idx: i32) -> i32 { + return i32(bitcast(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointParent))])); +} +fn JointChildIndex(idx: i32) -> i32 { + return i32(bitcast(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointChild))])); +} +fn JointPPos(idx: i32) -> vec3 { + return vec3(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPPosX))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPPosY))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPPosZ))]); +} +fn JointPRot(idx: i32) -> vec4 { + return vec4(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPRotX))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPRotY))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPRotZ))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPRotW))]); +} +fn JointAxis(idx: i32) -> vec3 { + return vec3(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointAxisX))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointAxisY))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointAxisZ))]); +} +fn SetJointPForce(idx: i32, f: vec3) { + Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPForceX))] = f.x; + Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPForceY))] = f.y; + Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPForceZ))] = f.z; +} +fn SetJointPTorque(idx: i32, t: vec3) { + Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPTorqueX))] = t.x; + Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPTorqueY))] = t.y; + Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPTorqueZ))] = t.z; +} +fn SetJointCForce(idx: i32, f: vec3) { + Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCForceX))] = f.x; + Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCForceY))] = f.y; + Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCForceZ))] = f.z; +} +fn SetJointCTorque(idx: i32, t: vec3) { + Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCTorqueX))] = t.x; + Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCTorqueY))] = t.y; + Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCTorqueZ))] = t.z; +} +alias JointTypes = i32; //enums:enum +const Prismatic: JointTypes = 0; +const Revolute: JointTypes = 1; +const Ball: JointTypes = 2; +const Fixed: JointTypes = 3; +const Free: JointTypes = 4; +const Distance: JointTypes = 5; +const D6: JointTypes = 6; + +//////// import: "params.go" +struct PhysParams { + DynamicsN: i32, + JointsN: i32, + Cur: i32, + Next: i32, + Iters: i32, + Dt: f32, + SoftRelax: f32, + JointLinearRelax: f32, + JointAngularRelax: f32, + JointLinearComply: f32, + JointAngularComply: f32, + ContactRelax: f32, + AngularDamping: f32, + ContactWeighting: i32, + Restitution: i32, + pad: f32, + Gravity: vec4, +} + +//////// import: "shapes.go" +alias Shapes = i32; //enums:enum +const Box: Shapes = 0; +const Sphere: Shapes = 1; +const Cylinder: Shapes = 2; +const Capsule: Shapes = 3; + +//////// import: "slmath-matrix3.go" + +//////// import: "slmath-quaternion.go" +fn MulQuatVector(q: vec4, v: vec3) -> vec3 { + var xyz = vec3(q.x, q.y, q.z); + var t = MulScalar3(Cross3(xyz, v), f32(f32(2)));return v+(MulScalar3(t, q.w))+(Cross3(xyz, t)); +} +fn MulQuats(a: vec4,b: vec4) -> vec4 { + var q: vec4; + q.x = a.x*b.w + a.w*b.x + a.y*b.z - a.z*b.y; + q.y = a.y*b.w + a.w*b.y + a.z*b.x - a.x*b.z; + q.z = a.z*b.w + a.w*b.z + a.x*b.y - a.y*b.x; + q.w = a.w*b.w - a.x*b.x - a.y*b.y - a.z*b.z;return q; +} +fn MulQPTransforms(aP: vec3, aQ: vec4, bP: vec3, bQ: vec4, oP: ptr>, oQ: ptr>) { + *oP = MulQuatVector(aQ, bP)+(aP); + *oQ = MulQuats(aQ, bQ); +} +fn MulQPPoint(xP: vec3, xQ: vec4, p: vec3) -> vec3 { + var dp = MulQuatVector(xQ, p);return dp+(xP); +} + +//////// import: "slmath-vector3.go" +fn MulScalar3(v: vec3, s: f32) -> vec3 { + return vec3(v.x*s, v.y*s, v.z*s); +} +fn Cross3(v: vec3,o: vec3) -> vec3 { + return vec3(v.y*o.z-v.z*o.y, v.z*o.x-v.x*o.z, v.x*o.y-v.y*o.x); +} + +//////// import: "step.go" + +//////// import: "step_body.go" + +//////// import: "step_joint.go" +fn StepJointForces(i: u32) { //gosl:kernel + let pars = Params[0]; + var ji = i32(i); + if (ji >= pars.JointsN) { + return; + } + var jpi = JointParentIndex(ji); + var jpbi = DynamicIndex(jpi, pars.Cur); + var jci = JointChildIndex(ji); + var jcbi = DynamicIndex(jci, pars.Cur); + var jt = GetJointType(ji); + var jpP = JointPPos(ji); + var jpQ = JointPRot(ji); + var xwpP = jpP; + var xwpQ = jpQ; + var posepP = jpP; + var posepQ = jpQ; + var comp: vec3; + if (jpi >= 0) { // can be fixed + posepP = DynamicPos(jpi, pars.Cur); + posepQ = DynamicRot(jpi, pars.Cur); + MulQPTransforms(posepP, posepQ, jpP, jpQ, &xwpP, &xwpQ); + comp = BodyCom(jpbi); + } + var rp = xwpP-(MulQPPoint(posepP, posepQ, comp)); // parent moment arm + var posecP = DynamicPos(jci, pars.Cur); + var posecQ = DynamicRot(jci, pars.Cur); + var xwcP = posecP; + var comc = BodyCom(jcbi); + var rc = xwcP-(MulQPPoint(posecP, posecQ, comc)); // child moment arm + var jf = JointControlForce(ji); + var jtq = JointControlTorque(ji); + var f: vec3; + var t: vec3; + switch (jt) { + case Free, Distance: { + f = jf; + t = jtq; + } + case Ball: { + t = jtq; + } + case Revolute, Prismatic: { + var axis = JointAxis(ji); + var ap = MulQuatVector(xwpQ, axis); + f = f+(MulScalar3(ap, jf.x)); + } + default: { + } + } + SetJointPForce(ji, f); + SetJointCForce(ji, f); + SetJointPTorque(ji, t+(Cross3(rp, f))); + SetJointCTorque(ji, t+(Cross3(rc, f))); +} \ No newline at end of file diff --git a/physics/shaders/StepSolveJoints.wgsl b/physics/shaders/StepSolveJoints.wgsl new file mode 100644 index 00000000..b21bdbe4 --- /dev/null +++ b/physics/shaders/StepSolveJoints.wgsl @@ -0,0 +1,582 @@ +// Code generated by "gosl"; DO NOT EDIT +// kernel: StepSolveJoints + +// // Params are global parameters. +@group(0) @binding(0) +var TensorStrides: array; +@group(0) @binding(1) +var Params: array; +// // Bodies are the rigid body elements (dynamic and static), // specifying the constant, non-dynamic properties, // which is initial state for dynamics. // [body][BodyVarsN] +@group(1) @binding(0) +var Bodies: array; +@group(1) @binding(1) +var Joints: array; +// // Dynamics are the dynamic rigid body elements: these actually move. // Two alternating states are used: Params.Cur and Params.Next. // [dyn body][cur/next][DynamicVarsN] +@group(2) @binding(0) +var Dynamics: array; +// // JointControls are dynamic joint control inputs. // [joint][JointControlVarsN] +@group(3) @binding(0) +var JointControls: array; + +alias GPUVars = i32; + +@compute @workgroup_size(64, 1, 1) +fn main(@builtin(workgroup_id) wgid: vec3, @builtin(num_workgroups) nwg: vec3, @builtin(local_invocation_index) loci: u32) { + let idx = loci + (wgid.x + wgid.y * nwg.x + wgid.z * nwg.x * nwg.y) * 64; + StepSolveJoints(idx); +} + +fn Index2D(s0: u32, s1: u32, i0: u32, i1: u32) -> u32 { + return s0 * i0 + s1 * i1; +} + +fn Index3D(s0: u32, s1: u32, s2: u32, i0: u32, i1: u32, i2: u32) -> u32 { + return s0 * i0 + s1 * i1 + s2 * i2; +} + + +//////// import: "vars.go" + +//////// import: "body.go" +alias BodyVars = i32; //enums:enum +const BodyShape: BodyVars = 0; +const BodyWorldIndex: BodyVars = 1; +const BodySizeX: BodyVars = 2; +const BodySizeY: BodyVars = 3; +const BodySizeZ: BodyVars = 4; +const BodyMass: BodyVars = 5; +const BodyInvMass: BodyVars = 6; +const BodyBounce: BodyVars = 7; +const BodyFriction: BodyVars = 8; +const BodyPosX: BodyVars = 9; +const BodyPosY: BodyVars = 10; +const BodyPosZ: BodyVars = 11; +const BodyRotX: BodyVars = 12; +const BodyRotY: BodyVars = 13; +const BodyRotZ: BodyVars = 14; +const BodyRotW: BodyVars = 15; +const BodyComX: BodyVars = 16; +const BodyComY: BodyVars = 17; +const BodyComZ: BodyVars = 18; +const BodyInertiaXX: BodyVars = 19; +const BodyInertiaYX: BodyVars = 20; +const BodyInertiaZX: BodyVars = 21; +const BodyInertiaXY: BodyVars = 22; +const BodyInertiaYY: BodyVars = 23; +const BodyInertiaZY: BodyVars = 24; +const BodyInertiaXZ: BodyVars = 25; +const BodyInertiaYZ: BodyVars = 26; +const BodyInertiaZZ: BodyVars = 27; +const BodyInvInertiaXX: BodyVars = 28; +const BodyInvInertiaYX: BodyVars = 29; +const BodyInvInertiaZX: BodyVars = 30; +const BodyInvInertiaXY: BodyVars = 31; +const BodyInvInertiaYY: BodyVars = 32; +const BodyInvInertiaZY: BodyVars = 33; +const BodyInvInertiaXZ: BodyVars = 34; +const BodyInvInertiaYZ: BodyVars = 35; +const BodyInvInertiaZZ: BodyVars = 36; +fn BodyCom(idx: i32) -> vec3 { + return vec3(Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComX))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComY))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComZ))]); +} +fn BodyInvInertia(idx: i32) -> mat3x3f { + return mat3x3f(Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInvInertiaXX))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInvInertiaYX))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInvInertiaZX))], + Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInvInertiaXY))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInvInertiaYY))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInvInertiaZY))], + Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInvInertiaXZ))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInvInertiaYZ))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInvInertiaZZ))]); +} + +//////// import: "contact.go" +alias ContactVars = i32; //enums:enum +const ContactA: ContactVars = 0; +const ContactB: ContactVars = 1; +const ContactNormX: ContactVars = 2; +const ContactNormY: ContactVars = 3; +const ContactNormZ: ContactVars = 4; +const ContactPointX: ContactVars = 5; +const ContactPointY: ContactVars = 6; +const ContactPointZ: ContactVars = 7; +const ContactDist: ContactVars = 8; + +//////// import: "control.go" +alias JointControlVars = i32; //enums:enum +const JointCtrlForceX: JointControlVars = 0; +const JointCtrlForceY: JointControlVars = 1; +const JointCtrlForceZ: JointControlVars = 2; +const JointCtrlTorqueX: JointControlVars = 3; +const JointCtrlTorqueY: JointControlVars = 4; +const JointCtrlTorqueZ: JointControlVars = 5; +const JointTargetPosX: JointControlVars = 6; +const JointTargetPosY: JointControlVars = 7; +const JointTargetPosZ: JointControlVars = 8; +const JointTargetRotX: JointControlVars = 9; +const JointTargetRotY: JointControlVars = 10; +const JointTargetRotZ: JointControlVars = 11; +const JointTargetRotW: JointControlVars = 12; +const JointTargetVelX: JointControlVars = 13; +const JointTargetVelY: JointControlVars = 14; +const JointTargetVelZ: JointControlVars = 15; +const JointTargetAngVelX: JointControlVars = 16; +const JointTargetAngVelY: JointControlVars = 17; +const JointTargetAngVelZ: JointControlVars = 18; +fn JointTargetPos(idx: i32) -> vec3 { + return vec3(JointControls[Index2D(TensorStrides[50], TensorStrides[51], u32(idx), u32(JointTargetPosX))], JointControls[Index2D(TensorStrides[50], TensorStrides[51], u32(idx), u32(JointTargetPosY))], JointControls[Index2D(TensorStrides[50], TensorStrides[51], u32(idx), u32(JointTargetPosZ))]); +} +fn JointTargetVel(idx: i32) -> vec3 { + return vec3(JointControls[Index2D(TensorStrides[50], TensorStrides[51], u32(idx), u32(JointTargetVelX))], JointControls[Index2D(TensorStrides[50], TensorStrides[51], u32(idx), u32(JointTargetVelY))], JointControls[Index2D(TensorStrides[50], TensorStrides[51], u32(idx), u32(JointTargetVelZ))]); +} + +//////// import: "dynamics.go" +alias DynamicVars = i32; //enums:enum +const DynIndex: DynamicVars = 0; +const DynPosX: DynamicVars = 1; +const DynPosY: DynamicVars = 2; +const DynPosZ: DynamicVars = 3; +const DynRotX: DynamicVars = 4; +const DynRotY: DynamicVars = 5; +const DynRotZ: DynamicVars = 6; +const DynRotW: DynamicVars = 7; +const DynVelX: DynamicVars = 8; +const DynVelY: DynamicVars = 9; +const DynVelZ: DynamicVars = 10; +const DynAngVelX: DynamicVars = 11; +const DynAngVelY: DynamicVars = 12; +const DynAngVelZ: DynamicVars = 13; +const DynAccX: DynamicVars = 14; +const DynAccY: DynamicVars = 15; +const DynAccZ: DynamicVars = 16; +const DynAngAccX: DynamicVars = 17; +const DynAngAccY: DynamicVars = 18; +const DynAngAccZ: DynamicVars = 19; +const DynForceX: DynamicVars = 20; +const DynForceY: DynamicVars = 21; +const DynForceZ: DynamicVars = 22; +const DynTorqueX: DynamicVars = 23; +const DynTorqueY: DynamicVars = 24; +const DynTorqueZ: DynamicVars = 25; +const DynDeltaX: DynamicVars = 26; +const DynDeltaY: DynamicVars = 27; +const DynDeltaZ: DynamicVars = 28; +const DynAngDeltaX: DynamicVars = 29; +const DynAngDeltaY: DynamicVars = 30; +const DynAngDeltaZ: DynamicVars = 31; +fn DynamicIndex(idx: i32,cni: i32) -> i32 { + return i32(bitcast(Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynIndex))])); +} +fn DynamicPos(idx: i32,cni: i32) -> vec3 { + return vec3(Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynPosX))], Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynPosY))], Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynPosZ))]); +} +fn DynamicRot(idx: i32,cni: i32) -> vec4 { + return vec4(Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynRotX))], Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynRotY))], Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynRotZ))], Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynRotW))]); +} +fn DynamicDelta(idx: i32,cni: i32) -> vec3 { + return vec3(Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynDeltaX))], Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynDeltaY))], Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynDeltaZ))]); +} +fn DynamicAngDelta(idx: i32,cni: i32) -> vec3 { + return vec3(Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynAngDeltaX))], Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynAngDeltaY))], Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynAngDeltaZ))]); +} + +//////// import: "enumgen.go" +const BodyVarsN: BodyVars = 37; +const ContactVarsN: ContactVars = 9; +const JointControlVarsN: JointControlVars = 19; +const DynamicVarsN: DynamicVars = 32; +const GPUVarsN: GPUVars = 7; +const JointVarsN: JointVars = 53; +const JointTypesN: JointTypes = 7; +const ShapesN: Shapes = 4; + +//////// import: "joint.go" +alias JointVars = i32; //enums:enum +const JointType: JointVars = 0; +const JointEnabled: JointVars = 1; +const JointParent: JointVars = 2; +const JointChild: JointVars = 3; +const JointPPosX: JointVars = 4; +const JointPPosY: JointVars = 5; +const JointPPosZ: JointVars = 6; +const JointPRotX: JointVars = 7; +const JointPRotY: JointVars = 8; +const JointPRotZ: JointVars = 9; +const JointPRotW: JointVars = 10; +const JointCPosX: JointVars = 11; +const JointCPosY: JointVars = 12; +const JointCPosZ: JointVars = 13; +const JointCRotX: JointVars = 14; +const JointCRotY: JointVars = 15; +const JointCRotZ: JointVars = 16; +const JointCRotW: JointVars = 17; +const JointAxisX: JointVars = 18; +const JointAxisY: JointVars = 19; +const JointAxisZ: JointVars = 20; +const JointLimitLower: JointVars = 21; +const JointLimitUpper: JointVars = 22; +const JointStiffX: JointVars = 23; +const JointStiffY: JointVars = 24; +const JointStiffZ: JointVars = 25; +const JointDampX: JointVars = 26; +const JointDampY: JointVars = 27; +const JointDampZ: JointVars = 28; +const JointPForceX: JointVars = 29; +const JointPForceY: JointVars = 30; +const JointPForceZ: JointVars = 31; +const JointPTorqueX: JointVars = 32; +const JointPTorqueY: JointVars = 33; +const JointPTorqueZ: JointVars = 34; +const JointCForceX: JointVars = 35; +const JointCForceY: JointVars = 36; +const JointCForceZ: JointVars = 37; +const JointCTorqueX: JointVars = 38; +const JointCTorqueY: JointVars = 39; +const JointCTorqueZ: JointVars = 40; +const JointPDeltaX: JointVars = 41; +const JointPDeltaY: JointVars = 42; +const JointPDeltaZ: JointVars = 43; +const JointPAngDeltaX: JointVars = 44; +const JointPAngDeltaY: JointVars = 45; +const JointPAngDeltaZ: JointVars = 46; +const JointCDeltaX: JointVars = 47; +const JointCDeltaY: JointVars = 48; +const JointCDeltaZ: JointVars = 49; +const JointCAngDeltaX: JointVars = 50; +const JointCAngDeltaY: JointVars = 51; +const JointCAngDeltaZ: JointVars = 52; +fn GetJointType(idx: i32) -> JointTypes { + return JointTypes(bitcast(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointType))])); +} +fn JointParentIndex(idx: i32) -> i32 { + return i32(bitcast(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointParent))])); +} +fn JointChildIndex(idx: i32) -> i32 { + return i32(bitcast(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointChild))])); +} +fn JointPPos(idx: i32) -> vec3 { + return vec3(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPPosX))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPPosY))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPPosZ))]); +} +fn JointPRot(idx: i32) -> vec4 { + return vec4(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPRotX))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPRotY))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPRotZ))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPRotW))]); +} +fn JointCPos(idx: i32) -> vec3 { + return vec3(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCPosX))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCPosY))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCPosZ))]); +} +fn JointCRot(idx: i32) -> vec4 { + return vec4(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCRotX))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCRotY))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCRotZ))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCRotW))]); +} +fn JointAxis(idx: i32) -> vec3 { + return vec3(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointAxisX))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointAxisY))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointAxisZ))]); +} +fn JointStiff(idx: i32) -> vec3 { + return vec3(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointStiffX))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointStiffY))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointStiffZ))]); +} +fn JointDamp(idx: i32) -> vec3 { + return vec3(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointDampX))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointDampY))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointDampZ))]); +} +fn SetJointPDelta(idx: i32, f: vec3) { + Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPDeltaX))] = f.x; + Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPDeltaY))] = f.y; + Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPDeltaZ))] = f.z; +} +fn SetJointPAngDelta(idx: i32, t: vec3) { + Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPAngDeltaX))] = t.x; + Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPAngDeltaY))] = t.y; + Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPAngDeltaZ))] = t.z; +} +fn SetJointCDelta(idx: i32, f: vec3) { + Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCDeltaX))] = f.x; + Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCDeltaY))] = f.y; + Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCDeltaZ))] = f.z; +} +fn SetJointCAngDelta(idx: i32, t: vec3) { + Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCAngDeltaX))] = t.x; + Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCAngDeltaY))] = t.y; + Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCAngDeltaZ))] = t.z; +} +alias JointTypes = i32; //enums:enum +const Prismatic: JointTypes = 0; +const Revolute: JointTypes = 1; +const Ball: JointTypes = 2; +const Fixed: JointTypes = 3; +const Free: JointTypes = 4; +const Distance: JointTypes = 5; +const D6: JointTypes = 6; + +//////// import: "params.go" +struct PhysParams { + DynamicsN: i32, + JointsN: i32, + Cur: i32, + Next: i32, + Iters: i32, + Dt: f32, + SoftRelax: f32, + JointLinearRelax: f32, + JointAngularRelax: f32, + JointLinearComply: f32, + JointAngularComply: f32, + ContactRelax: f32, + AngularDamping: f32, + ContactWeighting: i32, + Restitution: i32, + pad: f32, + Gravity: vec4, +} + +//////// import: "shapes.go" +alias Shapes = i32; //enums:enum +const Box: Shapes = 0; +const Sphere: Shapes = 1; +const Cylinder: Shapes = 2; +const Capsule: Shapes = 3; + +//////// import: "slmath-matrix3.go" + +//////// import: "slmath-quaternion.go" +fn QuatLength(q: vec4) -> f32 { + return sqrt(q.x*q.x + q.y*q.y + q.z*q.z + q.w*q.w); +} +fn QuatNormalize(q: vec4) -> vec4 { + var nq: vec4; + var l = QuatLength(q); + if (l == 0) { + nq.x = f32(0); + nq.y = f32(0); + nq.z = f32(0); + nq.w = f32(1); + } else { + l = 1 / l; + nq.x *= l; + nq.y *= l; + nq.z *= l; + nq.w *= l; + }return nq; +} +fn MulQuatVector(q: vec4, v: vec3) -> vec3 { + var xyz = vec3(q.x, q.y, q.z); + var t = MulScalar3(Cross3(xyz, v), f32(f32(2)));return v+(MulScalar3(t, q.w))+(Cross3(xyz, t)); +} +fn MulQuatVectorInverse(q: vec4, v: vec3) -> vec3 { + var xyz = vec3(q.x, q.y, q.z); + var t = MulScalar3(Cross3(xyz, v), f32(f32(2)));return v-(MulScalar3(t, q.w))+(Cross3(xyz, t)); +} +fn MulQuats(a: vec4,b: vec4) -> vec4 { + var q: vec4; + q.x = a.x*b.w + a.w*b.x + a.y*b.z - a.z*b.y; + q.y = a.y*b.w + a.w*b.y + a.z*b.x - a.x*b.z; + q.z = a.z*b.w + a.w*b.z + a.x*b.y - a.y*b.x; + q.w = a.w*b.w - a.x*b.x - a.y*b.y - a.z*b.z;return q; +} +fn MulQPTransforms(aP: vec3, aQ: vec4, bP: vec3, bQ: vec4, oP: ptr>, oQ: ptr>) { + *oP = MulQuatVector(aQ, bP)+(aP); + *oQ = MulQuats(aQ, bQ); +} +fn MulQPPoint(xP: vec3, xQ: vec4, p: vec3) -> vec3 { + var dp = MulQuatVector(xQ, p);return dp+(xP); +} +fn QPTransformInverse(p: vec3, q: vec4, oP: ptr>, oQ: ptr>) { + var qi = QuatInverse(q); + *oP = Negate3(MulQuatVector(qi, p)); + *oQ = qi; +} +fn QuatInverse(q: vec4) -> vec4 { + var nq = q; + nq.x *= f32(-1); + nq.y *= f32(-1); + nq.z *= f32(-1);return QuatNormalize(nq); +} + +//////// import: "slmath-vector3.go" +fn MulScalar3(v: vec3, s: f32) -> vec3 { + return vec3(v.x*s, v.y*s, v.z*s); +} +fn Negate3(v: vec3) -> vec3 { + return vec3(-v.x, -v.y, -v.z); +} +fn LengthSquared3(v: vec3) -> f32 { + return v.x*v.x + v.y*v.y + v.z*v.z; +} +fn Dot3(v: vec3,o: vec3) -> f32 { + return v.x*o.x + v.y*o.y + v.z*o.z; +} +fn Max3(v: vec3,o: vec3) -> vec3 { + return vec3(max(v.x, o.x), max(v.y, o.y), max(v.z, o.z)); +} +fn Min3(v: vec3,o: vec3) -> vec3 { + return vec3(min(v.x, o.x), min(v.y, o.y), min(v.z, o.z)); +} +fn Abs3(v: vec3) -> vec3 { + return vec3(abs(v.x), abs(v.y), abs(v.z)); +} +fn Cross3(v: vec3,o: vec3) -> vec3 { + return vec3(v.y*o.z-v.z*o.y, v.z*o.x-v.x*o.z, v.x*o.y-v.y*o.x); +} + +//////// import: "step.go" + +//////// import: "step_body.go" + +//////// import: "step_joint.go" +fn StepSolveJoints(i: u32) { //gosl:kernel + let pars = Params[0]; + var ji = i32(i); + if (ji >= pars.JointsN) { + return; + } + var jpi = JointParentIndex(ji); + var jpbi = DynamicIndex(jpi, pars.Cur); + var jci = JointChildIndex(ji); + var jcbi = DynamicIndex(jci, pars.Cur); + var jt = GetJointType(ji); + if (jt == Free) { + return; + } + var jpP = JointPPos(ji); + var jpQ = JointPRot(ji); + var xwpP = jpP; // world xform, parent, pos + var xwpQ = jpQ; // quat + var mInvp = f32(0.0); + var iInvp = mat3x3f(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0); + var posepP = jpP; + var posepQ = jpQ; + var comp: vec3; + var + velp: vec3; + var omegap: vec3; + if (jpi >= 0) { + posepP = DynamicPos(jpi, pars.Next); // now using next + posepQ = DynamicRot(jpi, pars.Next); + MulQPTransforms(posepP, posepQ, jpP, jpQ, &xwpP, &xwpQ); + comp = BodyCom(jpbi); + mInvp = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(jpbi), u32(BodyInvMass))]; + iInvp = BodyInvInertia(jpbi); + velp = DynamicDelta(jpi, pars.Next); + omegap = DynamicAngDelta(jpi, pars.Next); + } + var posecP = DynamicPos(jci, pars.Next); + var posecQ = DynamicRot(jci, pars.Next); + var jcP = JointCPos(ji); + var jcQ = JointCRot(ji); + var xwcP = jcP; + var xwcQ = jcQ; + MulQPTransforms(posecP, posecQ, jcP, jcQ, &xwcP, &xwcQ); + var comc = BodyCom(jcbi); + var mInvc = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(jcbi), u32(BodyInvMass))]; + var iInvc = BodyInvInertia(jcbi); + var velc = DynamicDelta(jci, pars.Next); + var omegac = DynamicAngDelta(jci, pars.Next); + if (mInvp == 0.0 && mInvc == 0.0) { // connection between two immovable bodies + return; + } + var linDeltaP: vec3; + var angDeltaP: vec3; + var linDeltaC: vec3; + var angDeltaC: vec3; + var relPoseP = xwpP; + var relPoseQ = xwpQ; + QPTransformInverse(xwpP, xwpQ, &relPoseP, &relPoseQ); + MulQPTransforms(relPoseP, relPoseQ, xwcP, xwcQ, &relPoseP, &relPoseQ); + var xc = xwcP; + var worldComp = MulQPPoint(posepP, posepQ, comp); + var worldComc = MulQPPoint(posecP, posecQ, comc); + _ = worldComc; + if (jt == Distance) { + } else { // compute joint target, stiffness, damping + var axisLimitsD: vec3; + var axisLimitsA: vec3; + var axisTargetPosKeD: vec3; + var axisTargetPosKeA: vec3; + var axisTargetVelKdD: vec3; + var axisTargetVelKdA: vec3; + var axis = JointAxis(ji); + var loTemp = axis*(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(ji), u32(JointLimitLower))]); + var upTemp = axis*(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(ji), u32(JointLimitUpper))]); + axisLimitsD = Min3(loTemp, upTemp); + axisLimitsA = Max3(loTemp, upTemp); + var ke = JointStiff(ji); + var kd = JointDamp(ji); + var targetPos = JointTargetPos(ji); + var targetVel = JointTargetVel(ji); + if (ke.x > 0.0) { // has position control + UpdateJointAxisWeightedTarget(axis, targetPos.x, ke.x, &axisTargetPosKeD, &axisTargetPosKeA); + } + if (kd.x > 0.0) { // has velocity control + UpdateJointAxisWeightedTarget(axis, targetVel.x, kd.x, &axisTargetVelKdD, &axisTargetVelKdA); + } + var axisStiffness = axisTargetPosKeA; + var axisDamping = axisTargetVelKdA; + if (axisStiffness.x > 0.0) { // todo: Dim(i) access + axisTargetPosKeD.x /= axisStiffness.x; + } + if (axisDamping.x > 0.0) { // todo Dim + axisTargetVelKdD.x /= axisDamping.x; + } + var axisLimitsLower = axisLimitsD; + var axisLimitsUpper = axisLimitsA; + var rp = xc-(worldComp); + var rc = xc-(MulQPPoint(posecP, posecQ, comc)); + { + var e = relPoseP.x; // rel_p[dim] + var linearc = vec3(0, 0, 0); + var linearp = Negate3(linearc); + var angularp = Cross3(rp, linearc); + var angularc = Cross3(rc, linearc); + var derr = Dot3(linearp, velp) + Dot3(linearc, velc) + Dot3(angularp, omegap) + Dot3(angularc, omegac); + var err = f32(0.0); + var compliance = pars.JointLinearComply; + var damping = f32(0.0); + var targetVel = axisTargetVelKdD.x; // [dim] + var derrRel = derr - targetVel; + var lower = axisLimitsLower.x; // [dim] + var upper = axisLimitsUpper.x; // [dim] + if (e < lower) { + err = e - lower; + } else if (e > upper) { + err = e - upper; + } else { + var targetPos = axisTargetPosKeD.x; // [dim] + targetPos = clamp(targetPos, lower, upper); + if (axisStiffness.x > 0.0) { + err = e - targetPos; + compliance = 1.0 / axisStiffness.x; // [dim] + damping = axisDamping.x; // [dim] + } else if (axisDamping.x > 0.0) { + compliance = 1.0 / axisDamping.x; // [dim] + damping = axisDamping.x; // [dim] + } + } + if (abs(err) > 1e-9 || abs(derrRel) > 1e-9) { + var lambdaIn = f32(0.0); + var dLambda = PositionalCorrection(err, derrRel, posepQ, posecQ, mInvp, mInvc, + iInvp, iInvc, linearp, linearc, angularp, angularc, lambdaIn, compliance, damping, pars.Dt); + linDeltaP = linDeltaP+(linearp*(dLambda * pars.JointLinearRelax)); + angDeltaP = angDeltaP+(angularp*(dLambda * pars.JointAngularRelax)); + linDeltaC = linDeltaC+(linearc*(dLambda * pars.JointLinearRelax)); + angDeltaC = angDeltaC+(angularc*(dLambda * pars.JointAngularRelax)); + } + } + } + SetJointPDelta(ji, linDeltaP); + SetJointPAngDelta(ji, angDeltaP); + SetJointCDelta(ji, linDeltaC); + SetJointCAngDelta(ji, angDeltaC); +} +fn UpdateJointAxisWeightedTarget(axis: vec3, targ: f32,weight: f32, axisTargets: ptr>,axisWeights: ptr>) { + var weightedAxis = axis*(weight); + *axisTargets = (*axisTargets)+(weightedAxis*(targ)); // weighted target (to be normalized later by sum of weights) + *axisWeights = (*axisWeights)+(Abs3(weightedAxis)); +} +fn PositionalCorrection(err: f32,derr: f32, tfaQ: vec4,tfbQ: vec4, mInva: f32,mInvb: f32, Iinva: mat3x3f,Iinvb: mat3x3f, lineara: vec3,linearb: vec3,angulara: vec3,angularb: vec3, lambdaIn: f32,compliance: f32,damping: f32,dt: f32) -> f32 { + var denom = f32(0.0); + denom += LengthSquared3(lineara) * mInva; + denom += LengthSquared3(linearb) * mInvb; + var q1 = tfaQ; + var q2 = tfbQ; + var rotAngulara = MulQuatVectorInverse(q1, angulara); + var rotAngularb = MulQuatVectorInverse(q2, angularb); + denom += Dot3(rotAngulara, Iinva*(rotAngulara)); + denom += Dot3(rotAngularb, Iinvb*(rotAngularb)); + var alpha = compliance; + var gamma = compliance * damping; + var deltaLambda = -(err + alpha*lambdaIn + gamma*derr); + if (denom+alpha > 0.0) { + deltaLambda /= (dt+gamma)*denom + alpha/dt; + }return deltaLambda; +} \ No newline at end of file diff --git a/physics/step_body.go b/physics/step_body.go index 7b7740dc..a4a01c86 100644 --- a/physics/step_body.go +++ b/physics/step_body.go @@ -55,7 +55,7 @@ func DynamicsCurToNext(i uint32) { //gosl:kernel // todo: aggregate forces -// StepIntegrateBodies +// StepIntegrateBodies applies forces to update pos and deltas func StepIntegrateBodies(i uint32) { //gosl:kernel pars := GetParams(0) di := int32(i) @@ -70,40 +70,109 @@ func StepIntegrateBodies(i uint32) { //gosl:kernel com := BodyCom(bi) - // unpack transform - x0 := DynamicPos(di, pars.Cur) - r0 := DynamicRot(di, pars.Cur) + // current pos + p0 := DynamicPos(di, pars.Cur) + q0 := DynamicRot(di, pars.Cur) - // unpack spatial twist + // current deltas v0 := DynamicDelta(di, pars.Cur) w0 := DynamicAngDelta(di, pars.Cur) - // unpack spatial wrench + // new forces integrated from joints f0 := DynamicForce(di, pars.Next) t0 := DynamicTorque(di, pars.Next) - xcom := slmath.MulQuatVector(r0, com).Add(x0) + pcom := slmath.MulQuatVector(q0, com).Add(p0) // linear part v1 := v0.Add(f0.MulScalar(invMass).Add(pars.Gravity.V().MulScalar(OneIfNonzero(invMass))).MulScalar(pars.Dt)) - x1 := xcom.Add(v1.MulScalar(pars.Dt)) + p1 := pcom.Add(v1.MulScalar(pars.Dt)) // angular part (compute in body frame) - wb := slmath.MulQuatVectorInverse(r0, w0) - tb := slmath.MulQuatVectorInverse(r0, t0).Sub(slmath.Cross3(wb, inertia.MulVector3(wb))) // coriolis forces + wb := slmath.MulQuatVectorInverse(q0, w0) + tb := slmath.MulQuatVectorInverse(q0, t0).Sub(slmath.Cross3(wb, inertia.MulVector3(wb))) // coriolis forces - w1 := slmath.MulQuatVector(r0, wb.Add(invInertia.MulVector3(tb).MulScalar(pars.Dt))) - r1 := slmath.MulQuats(math32.NewQuat(w1.X, w1.Y, w1.Z, 0), r0).MulScalar(0.5 * pars.Dt) - r1 = slmath.QuatNormalize(r1) + w1 := slmath.MulQuatVector(q0, wb.Add(invInertia.MulVector3(tb).MulScalar(pars.Dt))) + q1 := slmath.MulQuats(math32.NewQuat(w1.X, w1.Y, w1.Z, 0), q0).MulScalar(0.5 * pars.Dt) + q1 = slmath.QuatNormalize(q1) // angular damping w1 = w1.MulScalar(1.0 - pars.AngularDamping*pars.Dt) - x1a := x1.Sub(slmath.MulQuatVector(r1, com)) // pos corrected to nominal center. + p1a := p1.Sub(slmath.MulQuatVector(q1, com)) // pos corrected to nominal center. - SetDynamicPos(di, pars.Next, x1a) - SetDynamicRot(di, pars.Next, r1) + SetDynamicPos(di, pars.Next, p1a) + SetDynamicRot(di, pars.Next, q1) + SetDynamicDelta(di, pars.Next, v1) + SetDynamicAngDelta(di, pars.Next, w1) +} + +// StepBodyDeltas updates Next position with deltas. +func StepBodyDeltas(i uint32) { //gosl:kernel + pars := GetParams(0) + di := int32(i) + if di >= pars.DynamicsN { + return + } + bi := DynamicIndex(di, pars.Cur) + + invMass := Bodies.Value(int(bi), int(BodyInvMass)) + if invMass == 0 { + return // no updates + } + inertia := BodyInertia(bi) + invInertia := BodyInvInertia(bi) + + // starting pos (from force integration) + p0 := DynamicPos(di, pars.Next) + q0 := DynamicRot(di, pars.Next) + + // starting deltas + v0 := DynamicDelta(di, pars.Next) + w0 := DynamicAngDelta(di, pars.Next) + + weight := float32(1.0) + // todo: this is rigid_contact_inv_weight in solver_xpbd.py: from contacts, with restitution + // if constraint_inv_weights: + // inv_weight = constraint_inv_weights[tid] + // if inv_weight > 0.0: + // weight = 1.0 / inv_weight + + dp := v0.MulScalar(invMass * weight) + dq := w0.MulScalar(weight) + + wb := slmath.MulQuatVectorInverse(q0, w0) + dwb := invInertia.MulVector3(slmath.MulQuatVectorInverse(q0, dq)) + // coriolis forces delta from dwb = (wb + dwb) I (wb + dwb) - wb I wb + tb := slmath.Cross3(dwb, inertia.MulVector3(wb.Add(dwb))).Add(slmath.Cross3(wb, inertia.MulVector3(dwb))) + dw1 := slmath.MulQuatVector(q0, dwb.Sub(invInertia.MulVector3(tb).MulScalar(pars.Dt))) + + // update orientation + q1 := q0.Add(slmath.MulQuats(math32.NewQuat(dw1.X, dw1.Y, dw1.Z, 0), q0).MulScalar(0.5 * pars.Dt)) + // q1 := q0 + 0.5 * wp.quat(dw1 * dt, 0.0) * q0 + q1 = slmath.QuatNormalize(q1) + + // update position + com := BodyCom(bi) + pcom := slmath.MulQuatVector(q0, com).Add(p0) + + p1 := pcom.Add(dp.MulScalar(pars.Dt)) + p1 = p1.Sub(slmath.MulQuatVector(q1, com)) + + // update linear and angular velocity + v1 := v0.Add(dp) + w1 := w0.Add(dw1) + + // this improves gradient stability + if slmath.Length3(v1) < 1e-4 { + v1 = math32.Vec3(0, 0, 0) + } + if slmath.Length3(w1) < 1e-4 { + w1 = math32.Vec3(0, 0, 0) + } + SetDynamicPos(di, pars.Next, p1) + SetDynamicRot(di, pars.Next, q1) SetDynamicDelta(di, pars.Next, v1) SetDynamicAngDelta(di, pars.Next, w1) } diff --git a/physics/step_body.goal b/physics/step_body.goal index 54297b73..c0106fd0 100644 --- a/physics/step_body.goal +++ b/physics/step_body.goal @@ -53,7 +53,7 @@ func DynamicsCurToNext(i uint32) { //gosl:kernel // todo: aggregate forces -// StepIntegrateBodies +// StepIntegrateBodies applies forces to update pos and deltas func StepIntegrateBodies(i uint32) { //gosl:kernel pars := GetParams(0) di := int32(i) @@ -68,40 +68,109 @@ func StepIntegrateBodies(i uint32) { //gosl:kernel com := BodyCom(bi) - // unpack transform - x0 := DynamicPos(di, pars.Cur) - r0 := DynamicRot(di, pars.Cur) + // current pos + p0 := DynamicPos(di, pars.Cur) + q0 := DynamicRot(di, pars.Cur) - // unpack spatial twist + // current deltas v0 := DynamicDelta(di, pars.Cur) w0 := DynamicAngDelta(di, pars.Cur) - // unpack spatial wrench + // new forces integrated from joints f0 := DynamicForce(di, pars.Next) t0 := DynamicTorque(di, pars.Next) - xcom := slmath.MulQuatVector(r0, com).Add(x0) + pcom := slmath.MulQuatVector(q0, com).Add(p0) // linear part v1 := v0.Add(f0.MulScalar(invMass).Add(pars.Gravity.V().MulScalar(OneIfNonzero(invMass))).MulScalar(pars.Dt)) - x1 := xcom.Add(v1.MulScalar(pars.Dt)) + p1 := pcom.Add(v1.MulScalar(pars.Dt)) // angular part (compute in body frame) - wb := slmath.MulQuatVectorInverse(r0, w0) - tb := slmath.MulQuatVectorInverse(r0, t0).Sub(slmath.Cross3(wb, inertia.MulVector3(wb))) // coriolis forces + wb := slmath.MulQuatVectorInverse(q0, w0) + tb := slmath.MulQuatVectorInverse(q0, t0).Sub(slmath.Cross3(wb, inertia.MulVector3(wb))) // coriolis forces - w1 := slmath.MulQuatVector(r0, wb.Add(invInertia.MulVector3(tb).MulScalar(pars.Dt))) - r1 := slmath.MulQuats(math32.NewQuat(w1.X, w1.Y, w1.Z, 0), r0).MulScalar(0.5 * pars.Dt) - r1 = slmath.QuatNormalize(r1) + w1 := slmath.MulQuatVector(q0, wb.Add(invInertia.MulVector3(tb).MulScalar(pars.Dt))) + q1 := slmath.MulQuats(math32.NewQuat(w1.X, w1.Y, w1.Z, 0), q0).MulScalar(0.5 * pars.Dt) + q1 = slmath.QuatNormalize(q1) // angular damping w1 = w1.MulScalar(1.0 - pars.AngularDamping*pars.Dt) - x1a := x1.Sub(slmath.MulQuatVector(r1, com)) // pos corrected to nominal center. + p1a := p1.Sub(slmath.MulQuatVector(q1, com)) // pos corrected to nominal center. - SetDynamicPos(di, pars.Next, x1a) - SetDynamicRot(di, pars.Next, r1) + SetDynamicPos(di, pars.Next, p1a) + SetDynamicRot(di, pars.Next, q1) + SetDynamicDelta(di, pars.Next, v1) + SetDynamicAngDelta(di, pars.Next, w1) +} + +// StepBodyDeltas updates Next position with deltas. +func StepBodyDeltas(i uint32) { //gosl:kernel + pars := GetParams(0) + di := int32(i) + if di >= pars.DynamicsN { + return + } + bi := DynamicIndex(di, pars.Cur) + + invMass := Bodies[bi, BodyInvMass] + if invMass == 0 { + return // no updates + } + inertia := BodyInertia(bi) + invInertia := BodyInvInertia(bi) + + // starting pos (from force integration) + p0 := DynamicPos(di, pars.Next) + q0 := DynamicRot(di, pars.Next) + + // starting deltas + v0 := DynamicDelta(di, pars.Next) + w0 := DynamicAngDelta(di, pars.Next) + + weight := float32(1.0) + // todo: this is rigid_contact_inv_weight in solver_xpbd.py: from contacts, with restitution + // if constraint_inv_weights: + // inv_weight = constraint_inv_weights[tid] + // if inv_weight > 0.0: + // weight = 1.0 / inv_weight + + dp := v0.MulScalar(invMass * weight) + dq := w0.MulScalar(weight) + + wb := slmath.MulQuatVectorInverse(q0, w0) + dwb := invInertia.MulVector3(slmath.MulQuatVectorInverse(q0, dq)) + // coriolis forces delta from dwb = (wb + dwb) I (wb + dwb) - wb I wb + tb := slmath.Cross3(dwb, inertia.MulVector3(wb.Add(dwb))).Add(slmath.Cross3(wb, inertia.MulVector3(dwb))) + dw1 := slmath.MulQuatVector(q0, dwb.Sub(invInertia.MulVector3(tb).MulScalar(pars.Dt))) + + // update orientation + q1 := q0.Add(slmath.MulQuats(math32.NewQuat(dw1.X, dw1.Y, dw1.Z, 0), q0).MulScalar(0.5 * pars.Dt)) + // q1 := q0 + 0.5 * wp.quat(dw1 * dt, 0.0) * q0 + q1 = slmath.QuatNormalize(q1) + + // update position + com := BodyCom(bi) + pcom := slmath.MulQuatVector(q0, com).Add(p0) + + p1 := pcom.Add(dp.MulScalar(pars.Dt)) + p1 = p1.Sub(slmath.MulQuatVector(q1, com)) + + // update linear and angular velocity + v1 := v0.Add(dp) + w1 := w0.Add(dw1) + + // this improves gradient stability + if slmath.Length3(v1) < 1e-4 { + v1 = math32.Vec3(0, 0, 0) + } + if slmath.Length3(w1) < 1e-4 { + w1 = math32.Vec3(0, 0, 0) + } + SetDynamicPos(di, pars.Next, p1) + SetDynamicRot(di, pars.Next, q1) SetDynamicDelta(di, pars.Next, v1) SetDynamicAngDelta(di, pars.Next, w1) } diff --git a/physics/step_joint.go b/physics/step_joint.go index b6913248..051a200a 100644 --- a/physics/step_joint.go +++ b/physics/step_joint.go @@ -76,6 +76,7 @@ func StepJointForces(i uint32) { //gosl:kernel default: // todo: D6 requires more iteration! } + // These are unique to joint: aggregate into dynamics Next in separate step. SetJointPForce(ji, f) SetJointCForce(ji, f) SetJointPTorque(ji, t.Add(slmath.Cross3(rp, f))) @@ -285,16 +286,16 @@ func StepSolveJoints(i uint32) { //gosl:kernel axisStiffness := axisTargetPosKeA axisDamping := axisTargetVelKdA - for i := range 3 { - if axisStiffness.X > 0.0 { // todo: Dim(i) access - axisTargetPosKeD.X /= axisStiffness.X - } + // for i := range 3 { + if axisStiffness.X > 0.0 { // todo: Dim(i) access + axisTargetPosKeD.X /= axisStiffness.X } - for i := range 3 { - if axisDamping.X > 0.0 { // todo Dim - axisTargetVelKdD.X /= axisDamping.X - } + // } + // for i := range 3 { + if axisDamping.X > 0.0 { // todo Dim + axisTargetVelKdD.X /= axisDamping.X } + // } axisLimitsLower := axisLimitsD axisLimitsUpper := axisLimitsA // todo: @@ -305,12 +306,15 @@ func StepSolveJoints(i uint32) { //gosl:kernel rp := xc.Sub(worldComp) rc := xc.Sub(slmath.MulQPPoint(posecP, posecQ, comc)) - // for loop will be unrolled, so we can modify local variables - for dim := range 3 { + // for loop will be unrolled, so we can modify local variables + // + // for dim := range 3 { + { e := relPoseP.X // rel_p[dim] // compute gradients - linearc := math32.Vec3(frame_p.Value(int(0), int(dim)), frame_p.Value(int(1), int(dim)), frame_p.Value(int(2), int(dim))) + // linearc := math32.Vec3(frame_p[0, dim], frame_p[1, dim], frame_p[2, dim]) // todo + linearc := math32.Vec3(0, 0, 0) linearp := slmath.Negate3(linearc) angularp := slmath.Cross3(rp, linearc) angularc := slmath.Cross3(rc, linearc) @@ -544,6 +548,12 @@ func StepSolveJoints(i uint32) { //gosl:kernel // angDelta_p += angular_p * d_lambda // angDelta_c += angular_c * d_lambda + // These are unique to joint: aggregate into dynamics Next in separate step. + SetJointPDelta(ji, linDeltaP) + SetJointPAngDelta(ji, angDeltaP) + SetJointCDelta(ji, linDeltaC) + SetJointCAngDelta(ji, angDeltaC) + // if id_p >= 0: // // wp.atomic_add(deltas, id_p, wp.spatial_vector(linDelta_p, angDelta_p)) @@ -553,10 +563,10 @@ func StepSolveJoints(i uint32) { //gosl:kernel // wp.atomic_add(deltas, id_c, wp.spatial_vector(linDelta_c, angDelta_c)) } -func UpdateJointAxisWeightedTarget(axis math32.Vector3, target, weight float32, axisTargets, axisWeights *math32.Vector3) { +func UpdateJointAxisWeightedTarget(axis math32.Vector3, targ, weight float32, axisTargets, axisWeights *math32.Vector3) { weightedAxis := axis.MulScalar(weight) - *axisTargets = axisTargets.Add(weightedAxis.MulScalar(target)) // weighted target (to be normalized later by sum of weights) - *axisWeights = axisWeights.Add(slmath.Abs3(weightedAxis)) + *axisTargets = (*axisTargets).Add(weightedAxis.MulScalar(targ)) // weighted target (to be normalized later by sum of weights) + *axisWeights = (*axisWeights).Add(slmath.Abs3(weightedAxis)) } func PositionalCorrection(err, derr float32, tfaQ, tfbQ math32.Quat, mInva, mInvb float32, Iinva, Iinvb math32.Matrix3, lineara, linearb, angulara, angularb math32.Vector3, lambdaIn, compliance, damping, dt float32) float32 { diff --git a/physics/step_joint.goal b/physics/step_joint.goal index 7edf0aa2..591953d7 100644 --- a/physics/step_joint.goal +++ b/physics/step_joint.goal @@ -74,6 +74,7 @@ func StepJointForces(i uint32) { //gosl:kernel default: // todo: D6 requires more iteration! } + // These are unique to joint: aggregate into dynamics Next in separate step. SetJointPForce(ji, f) SetJointCForce(ji, f) SetJointPTorque(ji, t.Add(slmath.Cross3(rp, f))) @@ -277,16 +278,16 @@ func StepSolveJoints(i uint32) { //gosl:kernel axisStiffness := axisTargetPosKeA axisDamping := axisTargetVelKdA - for i := range 3 { - if axisStiffness.X > 0.0 { // todo: Dim(i) access - axisTargetPosKeD.X /= axisStiffness.X - } + // for i := range 3 { + if axisStiffness.X > 0.0 { // todo: Dim(i) access + axisTargetPosKeD.X /= axisStiffness.X } - for i := range 3 { - if axisDamping.X > 0.0 { // todo Dim - axisTargetVelKdD.X /= axisDamping.X - } + // } + // for i := range 3 { + if axisDamping.X > 0.0 { // todo Dim + axisTargetVelKdD.X /= axisDamping.X } + // } axisLimitsLower := axisLimitsD axisLimitsUpper := axisLimitsA // todo: @@ -296,11 +297,13 @@ func StepSolveJoints(i uint32) { //gosl:kernel rc := xc.Sub(slmath.MulQPPoint(posecP, posecQ, comc)) // for loop will be unrolled, so we can modify local variables - for dim := range 3 { + // for dim := range 3 { + { e := relPoseP.X // rel_p[dim] // compute gradients - linearc := math32.Vec3(frame_p[0, dim], frame_p[1, dim], frame_p[2, dim]) + // linearc := math32.Vec3(frame_p[0, dim], frame_p[1, dim], frame_p[2, dim]) // todo + linearc := math32.Vec3(0, 0, 0) linearp := slmath.Negate3(linearc) angularp := slmath.Cross3(rp, linearc) angularc := slmath.Cross3(rc, linearc) @@ -534,16 +537,22 @@ func StepSolveJoints(i uint32) { //gosl:kernel // angDelta_p += angular_p * d_lambda // angDelta_c += angular_c * d_lambda + // These are unique to joint: aggregate into dynamics Next in separate step. + SetJointPDelta(ji, linDeltaP) + SetJointPAngDelta(ji, angDeltaP) + SetJointCDelta(ji, linDeltaC) + SetJointCAngDelta(ji, angDeltaC) + // if id_p >= 0: // wp.atomic_add(deltas, id_p, wp.spatial_vector(linDelta_p, angDelta_p)) // if id_c >= 0: // wp.atomic_add(deltas, id_c, wp.spatial_vector(linDelta_c, angDelta_c)) } -func UpdateJointAxisWeightedTarget(axis math32.Vector3, target, weight float32, axisTargets, axisWeights *math32.Vector3) { +func UpdateJointAxisWeightedTarget(axis math32.Vector3, targ, weight float32, axisTargets, axisWeights *math32.Vector3) { weightedAxis := axis.MulScalar(weight) - *axisTargets = axisTargets.Add(weightedAxis.MulScalar(target)) // weighted target (to be normalized later by sum of weights) - *axisWeights = axisWeights.Add(slmath.Abs3(weightedAxis)) + *axisTargets = (*axisTargets).Add(weightedAxis.MulScalar(targ)) // weighted target (to be normalized later by sum of weights) + *axisWeights = (*axisWeights).Add(slmath.Abs3(weightedAxis)) } func PositionalCorrection(err, derr float32, tfaQ, tfbQ math32.Quat, mInva, mInvb float32, Iinva, Iinvb math32.Matrix3, lineara, linearb, angulara, angularb math32.Vector3, lambdaIn, compliance, damping, dt float32) float32 { From 758d4a6f41235146260849b4dc9e5ab9835e46a9 Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Tue, 16 Dec 2025 09:09:39 +0100 Subject: [PATCH 18/97] physics: jointbodies, agg forces, deltas --- physics/config.go | 48 ++++ physics/config.goal | 46 ++++ physics/gosl.go | 96 +++++++ physics/params.go | 27 +- physics/shaders/DeltasFromJoints.wgsl | 308 +++++++++++++++++++++++ physics/shaders/DynamicsCurToNext.wgsl | 16 +- physics/shaders/ForcesFromJoints.wgsl | 308 +++++++++++++++++++++++ physics/shaders/InitDynamics.wgsl | 14 +- physics/shaders/StepBodyDeltas.wgsl | 38 +-- physics/shaders/StepIntegrateBodies.wgsl | 46 ++-- physics/shaders/StepJointForces.wgsl | 26 +- physics/shaders/StepSolveJoints.wgsl | 46 ++-- physics/step.go | 4 +- physics/step.goal | 4 +- physics/step_body.go | 134 +++++++--- physics/step_body.goal | 134 +++++++--- physics/step_joint.go | 56 ++--- physics/step_joint.goal | 56 ++--- physics/typegen.go | 4 +- physics/world.go | 2 +- physics/world.goal | 2 +- 21 files changed, 1169 insertions(+), 246 deletions(-) create mode 100644 physics/config.go create mode 100644 physics/config.goal create mode 100644 physics/shaders/DeltasFromJoints.wgsl create mode 100644 physics/shaders/ForcesFromJoints.wgsl diff --git a/physics/config.go b/physics/config.go new file mode 100644 index 00000000..6dcd8406 --- /dev/null +++ b/physics/config.go @@ -0,0 +1,48 @@ +// Code generated by "goal build"; DO NOT EDIT. +//line config.goal:1 +// Copyright (c) 2025, Cogent Core. All rights reserved. +// Use of this source code is governed by a BSD-style +// license that can be found in the LICENSE file. + +package physics + +// Config does final configuration prior to running +// after everything has been added. +func (wl *World) Config() { + wl.ConfigJoints() +} + +// ConfigJoints does all of the initialization associated with joints. +func (wl *World) ConfigJoints() { + // accumulate parent and child joints per dynamic + nj := wl.Params[0].JointsN + nd := wl.Params[0].DynamicsN + bjp := make([][]int32, nd) + bjc := make([][]int32, nd) + + maxi := 0 + + for ji := range nj { + jpi := JointParentIndex(ji) + jci := JointChildIndex(ji) + bjp[jpi] = append(bjp[jpi], ji) + bjc[jci] = append(bjc[jci], ji) + maxi = max(maxi, len(bjp[jpi])) + maxi = max(maxi, len(bjc[jci])) + } + maxi = maxi + 1 // extra for n + wl.Params[0].BodyJointsMax = int32(maxi) + wl.BodyJoints.SetShapeSizes(int(nd), 2, maxi) + for di := range nd { + np := int32(len(bjp[di])) + wl.BodyJoints.Set(np, int(di), int(0), int(0)) + for i, ji := range bjp[di] { + wl.BodyJoints.Set(ji, int(di), int(0), int(1+i)) + } + nc := int32(len(bjc[di])) + wl.BodyJoints.Set(nc, int(di), int(1), int(0)) + for i, ji := range bjc[di] { + wl.BodyJoints.Set(ji, int(di), int(1), int(1+i)) + } + } +} diff --git a/physics/config.goal b/physics/config.goal new file mode 100644 index 00000000..b5c60d56 --- /dev/null +++ b/physics/config.goal @@ -0,0 +1,46 @@ +// Copyright (c) 2025, Cogent Core. All rights reserved. +// Use of this source code is governed by a BSD-style +// license that can be found in the LICENSE file. + +package physics + +// Config does final configuration prior to running +// after everything has been added. +func (wl *World) Config() { + wl.ConfigJoints() +} + +// ConfigJoints does all of the initialization associated with joints. +func (wl *World) ConfigJoints() { + // accumulate parent and child joints per dynamic + nj := wl.Params[0].JointsN + nd := wl.Params[0].DynamicsN + bjp := make([][]int32, nd) + bjc := make([][]int32, nd) + + maxi := 0 + + for ji := range nj { + jpi := JointParentIndex(ji) + jci := JointChildIndex(ji) + bjp[jpi] = append(bjp[jpi], ji) + bjc[jci] = append(bjc[jci], ji) + maxi = max(maxi, len(bjp[jpi])) + maxi = max(maxi, len(bjc[jci])) + } + maxi = maxi+1 // extra for n + wl.Params[0].BodyJointsMax = int32(maxi) + wl.BodyJoints.SetShapeSizes(int(nd), 2, maxi) + for di := range nd { + np := int32(len(bjp[di])) + wl.BodyJoints[di, 0, 0] = np + for i, ji := range bjp[di] { + wl.BodyJoints[di, 0, 1+i] = ji + } + nc := int32(len(bjc[di])) + wl.BodyJoints[di, 1, 0] = nc + for i, ji := range bjc[di] { + wl.BodyJoints[di, 1, 1+i] = ji + } + } +} diff --git a/physics/gosl.go b/physics/gosl.go index f0438e19..290c75d2 100644 --- a/physics/gosl.go +++ b/physics/gosl.go @@ -106,10 +106,22 @@ func GPUInit() { sgp.SetNValues(1) } var pl *gpu.ComputePipeline + pl = gpu.NewComputePipelineShaderFS(shaders, "shaders/DeltasFromJoints.wgsl", sy) + pl.AddVarUsed(0, "TensorStrides") + pl.AddVarUsed(1, "BodyJoints") + pl.AddVarUsed(2, "Dynamics") + pl.AddVarUsed(1, "Joints") + pl.AddVarUsed(0, "Params") pl = gpu.NewComputePipelineShaderFS(shaders, "shaders/DynamicsCurToNext.wgsl", sy) pl.AddVarUsed(0, "TensorStrides") pl.AddVarUsed(2, "Dynamics") pl.AddVarUsed(0, "Params") + pl = gpu.NewComputePipelineShaderFS(shaders, "shaders/ForcesFromJoints.wgsl", sy) + pl.AddVarUsed(0, "TensorStrides") + pl.AddVarUsed(1, "BodyJoints") + pl.AddVarUsed(2, "Dynamics") + pl.AddVarUsed(1, "Joints") + pl.AddVarUsed(0, "Params") pl = gpu.NewComputePipelineShaderFS(shaders, "shaders/InitDynamics.wgsl", sy) pl.AddVarUsed(0, "TensorStrides") pl.AddVarUsed(1, "Bodies") @@ -158,6 +170,48 @@ func GPURelease() { ComputeGPU = nil } +// RunDeltasFromJoints runs the DeltasFromJoints kernel with given number of elements, +// on either the CPU or GPU depending on the UseGPU variable. +// Can call multiple Run* kernels in a row, which are then all launched +// in the same command submission on the GPU, which is by far the most efficient. +// MUST call RunDone (with optional vars to sync) after all Run calls. +// Alternatively, a single-shot RunOneDeltasFromJoints call does Run and Done for a +// single run-and-sync case. +func RunDeltasFromJoints(n int) { + if UseGPU { + RunDeltasFromJointsGPU(n) + } else { + RunDeltasFromJointsCPU(n) + } +} + +// RunDeltasFromJointsGPU runs the DeltasFromJoints kernel on the GPU. See [RunDeltasFromJoints] for more info. +func RunDeltasFromJointsGPU(n int) { + sy := GPUSystem + pl := sy.ComputePipelines["DeltasFromJoints"] + ce, _ := sy.BeginComputePass() + pl.Dispatch1D(ce, n, 64) +} + +// RunDeltasFromJointsCPU runs the DeltasFromJoints kernel on the CPU. +func RunDeltasFromJointsCPU(n int) { + gpu.VectorizeFunc(0, n, DeltasFromJoints) +} + +// RunOneDeltasFromJoints runs the DeltasFromJoints kernel with given number of elements, +// on either the CPU or GPU depending on the UseGPU variable. +// This version then calls RunDone with the given variables to sync +// after the Run, for a single-shot Run-and-Done call. If multiple kernels +// can be run in sequence, it is much more efficient to do multiple Run* +// calls followed by a RunDone call. +func RunOneDeltasFromJoints(n int, syncVars ...GPUVars) { + if UseGPU { + RunDeltasFromJointsGPU(n) + RunDone(syncVars...) + } else { + RunDeltasFromJointsCPU(n) + } +} // RunDynamicsCurToNext runs the DynamicsCurToNext kernel with given number of elements, // on either the CPU or GPU depending on the UseGPU variable. // Can call multiple Run* kernels in a row, which are then all launched @@ -200,6 +254,48 @@ func RunOneDynamicsCurToNext(n int, syncVars ...GPUVars) { RunDynamicsCurToNextCPU(n) } } +// RunForcesFromJoints runs the ForcesFromJoints kernel with given number of elements, +// on either the CPU or GPU depending on the UseGPU variable. +// Can call multiple Run* kernels in a row, which are then all launched +// in the same command submission on the GPU, which is by far the most efficient. +// MUST call RunDone (with optional vars to sync) after all Run calls. +// Alternatively, a single-shot RunOneForcesFromJoints call does Run and Done for a +// single run-and-sync case. +func RunForcesFromJoints(n int) { + if UseGPU { + RunForcesFromJointsGPU(n) + } else { + RunForcesFromJointsCPU(n) + } +} + +// RunForcesFromJointsGPU runs the ForcesFromJoints kernel on the GPU. See [RunForcesFromJoints] for more info. +func RunForcesFromJointsGPU(n int) { + sy := GPUSystem + pl := sy.ComputePipelines["ForcesFromJoints"] + ce, _ := sy.BeginComputePass() + pl.Dispatch1D(ce, n, 64) +} + +// RunForcesFromJointsCPU runs the ForcesFromJoints kernel on the CPU. +func RunForcesFromJointsCPU(n int) { + gpu.VectorizeFunc(0, n, ForcesFromJoints) +} + +// RunOneForcesFromJoints runs the ForcesFromJoints kernel with given number of elements, +// on either the CPU or GPU depending on the UseGPU variable. +// This version then calls RunDone with the given variables to sync +// after the Run, for a single-shot Run-and-Done call. If multiple kernels +// can be run in sequence, it is much more efficient to do multiple Run* +// calls followed by a RunDone call. +func RunOneForcesFromJoints(n int, syncVars ...GPUVars) { + if UseGPU { + RunForcesFromJointsGPU(n) + RunDone(syncVars...) + } else { + RunForcesFromJointsCPU(n) + } +} // RunInitDynamics runs the InitDynamics kernel with given number of elements, // on either the CPU or GPU depending on the UseGPU variable. // Can call multiple Run* kernels in a row, which are then all launched diff --git a/physics/params.go b/physics/params.go index b30bff76..65169b3d 100644 --- a/physics/params.go +++ b/physics/params.go @@ -13,18 +13,6 @@ import ( // PhysParams are the physics parameters type PhysParams struct { - // DynamicsN is number of dynamics bodies. - DynamicsN int32 - - // JointsN is number of joints. - JointsN int32 - - // Index for the current state (0 or 1, alternates with Next). - Cur int32 - - // Index for the next state (1 or 0, alternates with Cur). - Next int32 - // Iters is the number of iterations to perform. Iters int32 `default:"2"` @@ -58,7 +46,20 @@ type PhysParams struct { // Restitution Restitution slbool.Bool `default:"false"` - pad float32 + // DynamicsN is number of dynamics bodies. + DynamicsN int32 `edit:"-"` + + // JointsN is number of joints. + JointsN int32 `edit:"-"` + + // BodyJointsMax is max number of joints per body + 1 for actual n. + BodyJointsMax int32 `edit:"-"` + + // Index for the current state (0 or 1, alternates with Next). + Cur int32 `edit:"-"` + + // Index for the next state (1 or 0, alternates with Cur). + Next int32 `edit:"-"` // Gravity is the gravity acceleration function Gravity slvec.Vector3 diff --git a/physics/shaders/DeltasFromJoints.wgsl b/physics/shaders/DeltasFromJoints.wgsl new file mode 100644 index 00000000..517b6ea1 --- /dev/null +++ b/physics/shaders/DeltasFromJoints.wgsl @@ -0,0 +1,308 @@ +// Code generated by "gosl"; DO NOT EDIT +// kernel: DeltasFromJoints + +// // Params are global parameters. +@group(0) @binding(0) +var TensorStrides: array; +@group(0) @binding(1) +var Params: array; +// // Bodies are the rigid body elements (dynamic and static), // specifying the constant, non-dynamic properties, // which is initial state for dynamics. // [body][BodyVarsN] +@group(1) @binding(1) +var Joints: array; +@group(1) @binding(2) +var BodyJoints: array; +// // Dynamics are the dynamic rigid body elements: these actually move. // Two alternating states are used: Params.Cur and Params.Next. // [dyn body][cur/next][DynamicVarsN] +@group(2) @binding(0) +var Dynamics: array; +// // JointControls are dynamic joint control inputs. // [joint][JointControlVarsN] + +alias GPUVars = i32; + +@compute @workgroup_size(64, 1, 1) +fn main(@builtin(workgroup_id) wgid: vec3, @builtin(num_workgroups) nwg: vec3, @builtin(local_invocation_index) loci: u32) { + let idx = loci + (wgid.x + wgid.y * nwg.x + wgid.z * nwg.x * nwg.y) * 64; + DeltasFromJoints(idx); +} + +fn Index2D(s0: u32, s1: u32, i0: u32, i1: u32) -> u32 { + return s0 * i0 + s1 * i1; +} + +fn Index3D(s0: u32, s1: u32, s2: u32, i0: u32, i1: u32, i2: u32) -> u32 { + return s0 * i0 + s1 * i1 + s2 * i2; +} + + +//////// import: "vars.go" + +//////// import: "body.go" +alias BodyVars = i32; //enums:enum +const BodyShape: BodyVars = 0; +const BodyWorldIndex: BodyVars = 1; +const BodySizeX: BodyVars = 2; +const BodySizeY: BodyVars = 3; +const BodySizeZ: BodyVars = 4; +const BodyMass: BodyVars = 5; +const BodyInvMass: BodyVars = 6; +const BodyBounce: BodyVars = 7; +const BodyFriction: BodyVars = 8; +const BodyPosX: BodyVars = 9; +const BodyPosY: BodyVars = 10; +const BodyPosZ: BodyVars = 11; +const BodyRotX: BodyVars = 12; +const BodyRotY: BodyVars = 13; +const BodyRotZ: BodyVars = 14; +const BodyRotW: BodyVars = 15; +const BodyComX: BodyVars = 16; +const BodyComY: BodyVars = 17; +const BodyComZ: BodyVars = 18; +const BodyInertiaXX: BodyVars = 19; +const BodyInertiaYX: BodyVars = 20; +const BodyInertiaZX: BodyVars = 21; +const BodyInertiaXY: BodyVars = 22; +const BodyInertiaYY: BodyVars = 23; +const BodyInertiaZY: BodyVars = 24; +const BodyInertiaXZ: BodyVars = 25; +const BodyInertiaYZ: BodyVars = 26; +const BodyInertiaZZ: BodyVars = 27; +const BodyInvInertiaXX: BodyVars = 28; +const BodyInvInertiaYX: BodyVars = 29; +const BodyInvInertiaZX: BodyVars = 30; +const BodyInvInertiaXY: BodyVars = 31; +const BodyInvInertiaYY: BodyVars = 32; +const BodyInvInertiaZY: BodyVars = 33; +const BodyInvInertiaXZ: BodyVars = 34; +const BodyInvInertiaYZ: BodyVars = 35; +const BodyInvInertiaZZ: BodyVars = 36; + +//////// import: "contact.go" +alias ContactVars = i32; //enums:enum +const ContactA: ContactVars = 0; +const ContactB: ContactVars = 1; +const ContactNormX: ContactVars = 2; +const ContactNormY: ContactVars = 3; +const ContactNormZ: ContactVars = 4; +const ContactPointX: ContactVars = 5; +const ContactPointY: ContactVars = 6; +const ContactPointZ: ContactVars = 7; +const ContactDist: ContactVars = 8; + +//////// import: "control.go" +alias JointControlVars = i32; //enums:enum +const JointCtrlForceX: JointControlVars = 0; +const JointCtrlForceY: JointControlVars = 1; +const JointCtrlForceZ: JointControlVars = 2; +const JointCtrlTorqueX: JointControlVars = 3; +const JointCtrlTorqueY: JointControlVars = 4; +const JointCtrlTorqueZ: JointControlVars = 5; +const JointTargetPosX: JointControlVars = 6; +const JointTargetPosY: JointControlVars = 7; +const JointTargetPosZ: JointControlVars = 8; +const JointTargetRotX: JointControlVars = 9; +const JointTargetRotY: JointControlVars = 10; +const JointTargetRotZ: JointControlVars = 11; +const JointTargetRotW: JointControlVars = 12; +const JointTargetVelX: JointControlVars = 13; +const JointTargetVelY: JointControlVars = 14; +const JointTargetVelZ: JointControlVars = 15; +const JointTargetAngVelX: JointControlVars = 16; +const JointTargetAngVelY: JointControlVars = 17; +const JointTargetAngVelZ: JointControlVars = 18; + +//////// import: "dynamics.go" +alias DynamicVars = i32; //enums:enum +const DynIndex: DynamicVars = 0; +const DynPosX: DynamicVars = 1; +const DynPosY: DynamicVars = 2; +const DynPosZ: DynamicVars = 3; +const DynRotX: DynamicVars = 4; +const DynRotY: DynamicVars = 5; +const DynRotZ: DynamicVars = 6; +const DynRotW: DynamicVars = 7; +const DynVelX: DynamicVars = 8; +const DynVelY: DynamicVars = 9; +const DynVelZ: DynamicVars = 10; +const DynAngVelX: DynamicVars = 11; +const DynAngVelY: DynamicVars = 12; +const DynAngVelZ: DynamicVars = 13; +const DynAccX: DynamicVars = 14; +const DynAccY: DynamicVars = 15; +const DynAccZ: DynamicVars = 16; +const DynAngAccX: DynamicVars = 17; +const DynAngAccY: DynamicVars = 18; +const DynAngAccZ: DynamicVars = 19; +const DynForceX: DynamicVars = 20; +const DynForceY: DynamicVars = 21; +const DynForceZ: DynamicVars = 22; +const DynTorqueX: DynamicVars = 23; +const DynTorqueY: DynamicVars = 24; +const DynTorqueZ: DynamicVars = 25; +const DynDeltaX: DynamicVars = 26; +const DynDeltaY: DynamicVars = 27; +const DynDeltaZ: DynamicVars = 28; +const DynAngDeltaX: DynamicVars = 29; +const DynAngDeltaY: DynamicVars = 30; +const DynAngDeltaZ: DynamicVars = 31; +fn SetDynamicDelta(idx: i32,cni: i32, delta: vec3) { + Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynDeltaX))] = delta.x; + Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynDeltaY))] = delta.y; + Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynDeltaZ))] = delta.z; +} +fn SetDynamicAngDelta(idx: i32,cni: i32, angDelta: vec3) { + Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynAngDeltaX))] = angDelta.x; + Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynAngDeltaY))] = angDelta.y; + Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynAngDeltaZ))] = angDelta.z; +} + +//////// import: "enumgen.go" +const BodyVarsN: BodyVars = 37; +const ContactVarsN: ContactVars = 9; +const JointControlVarsN: JointControlVars = 19; +const DynamicVarsN: DynamicVars = 32; +const GPUVarsN: GPUVars = 7; +const JointVarsN: JointVars = 53; +const JointTypesN: JointTypes = 7; +const ShapesN: Shapes = 4; + +//////// import: "joint.go" +alias JointVars = i32; //enums:enum +const JointType: JointVars = 0; +const JointEnabled: JointVars = 1; +const JointParent: JointVars = 2; +const JointChild: JointVars = 3; +const JointPPosX: JointVars = 4; +const JointPPosY: JointVars = 5; +const JointPPosZ: JointVars = 6; +const JointPRotX: JointVars = 7; +const JointPRotY: JointVars = 8; +const JointPRotZ: JointVars = 9; +const JointPRotW: JointVars = 10; +const JointCPosX: JointVars = 11; +const JointCPosY: JointVars = 12; +const JointCPosZ: JointVars = 13; +const JointCRotX: JointVars = 14; +const JointCRotY: JointVars = 15; +const JointCRotZ: JointVars = 16; +const JointCRotW: JointVars = 17; +const JointAxisX: JointVars = 18; +const JointAxisY: JointVars = 19; +const JointAxisZ: JointVars = 20; +const JointLimitLower: JointVars = 21; +const JointLimitUpper: JointVars = 22; +const JointStiffX: JointVars = 23; +const JointStiffY: JointVars = 24; +const JointStiffZ: JointVars = 25; +const JointDampX: JointVars = 26; +const JointDampY: JointVars = 27; +const JointDampZ: JointVars = 28; +const JointPForceX: JointVars = 29; +const JointPForceY: JointVars = 30; +const JointPForceZ: JointVars = 31; +const JointPTorqueX: JointVars = 32; +const JointPTorqueY: JointVars = 33; +const JointPTorqueZ: JointVars = 34; +const JointCForceX: JointVars = 35; +const JointCForceY: JointVars = 36; +const JointCForceZ: JointVars = 37; +const JointCTorqueX: JointVars = 38; +const JointCTorqueY: JointVars = 39; +const JointCTorqueZ: JointVars = 40; +const JointPDeltaX: JointVars = 41; +const JointPDeltaY: JointVars = 42; +const JointPDeltaZ: JointVars = 43; +const JointPAngDeltaX: JointVars = 44; +const JointPAngDeltaY: JointVars = 45; +const JointPAngDeltaZ: JointVars = 46; +const JointCDeltaX: JointVars = 47; +const JointCDeltaY: JointVars = 48; +const JointCDeltaZ: JointVars = 49; +const JointCAngDeltaX: JointVars = 50; +const JointCAngDeltaY: JointVars = 51; +const JointCAngDeltaZ: JointVars = 52; +fn JointPDelta(idx: i32) -> vec3 { + return vec3(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPDeltaX))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPDeltaY))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPDeltaZ))]); +} +fn JointPAngDelta(idx: i32) -> vec3 { + return vec3(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPAngDeltaX))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPAngDeltaY))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPAngDeltaZ))]); +} +fn JointCDelta(idx: i32) -> vec3 { + return vec3(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCDeltaX))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCDeltaY))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCDeltaZ))]); +} +fn JointCAngDelta(idx: i32) -> vec3 { + return vec3(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCAngDeltaX))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCAngDeltaY))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCAngDeltaZ))]); +} +alias JointTypes = i32; //enums:enum +const Prismatic: JointTypes = 0; +const Revolute: JointTypes = 1; +const Ball: JointTypes = 2; +const Fixed: JointTypes = 3; +const Free: JointTypes = 4; +const Distance: JointTypes = 5; +const D6: JointTypes = 6; + +//////// import: "params.go" +struct PhysParams { + Iters: i32, + Dt: f32, + SoftRelax: f32, + JointLinearRelax: f32, + JointAngularRelax: f32, + JointLinearComply: f32, + JointAngularComply: f32, + ContactRelax: f32, + AngularDamping: f32, + ContactWeighting: i32, + Restitution: i32, + DynamicsN: i32, + JointsN: i32, + BodyJointsMax: i32, + Cur: i32, + Next: i32, + Gravity: vec4, +} + +//////// import: "shapes.go" +alias Shapes = i32; //enums:enum +const Box: Shapes = 0; +const Sphere: Shapes = 1; +const Cylinder: Shapes = 2; +const Capsule: Shapes = 3; + +//////// import: "slmath-matrix3.go" + +//////// import: "slmath-quaternion.go" + +//////// import: "slmath-vector3.go" + +//////// import: "step.go" + +//////// import: "step_body.go" +fn DeltasFromJoints(i: u32) { //gosl:kernel + let params = Params[0]; + var di = i32(i); + if (di >= params.DynamicsN) { + return; + } + var np = BodyJoints[Index3D(TensorStrides[20], TensorStrides[21], TensorStrides[22], u32(di), u32(0), u32(0))]; + var nc = BodyJoints[Index3D(TensorStrides[20], TensorStrides[21], TensorStrides[22], u32(di), u32(1), u32(0))]; + var td = vec3(0, 0, 0); + var ta = vec3(0, 0, 0); + for (var i = i32(1); i <= np; i++) { + var ji = BodyJoints[Index3D(TensorStrides[20], TensorStrides[21], TensorStrides[22], u32(di), u32(0), u32(i))]; + var d = JointPDelta(ji); + td = td+(d); + var a = JointPAngDelta(ji); + ta = ta+(a); + } + for (var i = i32(1); i <= nc; i++) { + var ji = BodyJoints[Index3D(TensorStrides[20], TensorStrides[21], TensorStrides[22], u32(di), u32(1), u32(i))]; + var d = JointCDelta(ji); + td = td+(d); + var a = JointCAngDelta(ji); + ta = ta+(a); + } + SetDynamicDelta(di, params.Next, td); + SetDynamicAngDelta(di, params.Next, ta); +} + +//////// import: "step_joint.go" \ No newline at end of file diff --git a/physics/shaders/DynamicsCurToNext.wgsl b/physics/shaders/DynamicsCurToNext.wgsl index cafbabbd..81f131a5 100644 --- a/physics/shaders/DynamicsCurToNext.wgsl +++ b/physics/shaders/DynamicsCurToNext.wgsl @@ -212,10 +212,6 @@ const D6: JointTypes = 6; //////// import: "params.go" struct PhysParams { - DynamicsN: i32, - JointsN: i32, - Cur: i32, - Next: i32, Iters: i32, Dt: f32, SoftRelax: f32, @@ -227,7 +223,11 @@ struct PhysParams { AngularDamping: f32, ContactWeighting: i32, Restitution: i32, - pad: f32, + DynamicsN: i32, + JointsN: i32, + BodyJointsMax: i32, + Cur: i32, + Next: i32, Gravity: vec4, } @@ -248,13 +248,13 @@ const Capsule: Shapes = 3; //////// import: "step_body.go" fn DynamicsCurToNext(i: u32) { //gosl:kernel - let pars = Params[0]; + let params = Params[0]; var ii = i32(i); - if (ii >= pars.DynamicsN) { + if (ii >= params.DynamicsN) { return; } for (var di = DynIndex; di < DynamicVarsN; di++) { - Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(ii), u32(pars.Next), u32(di))] = Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(ii), u32(pars.Cur), u32(di))]; + Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(ii), u32(params.Next), u32(di))] = Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(ii), u32(params.Cur), u32(di))]; } } diff --git a/physics/shaders/ForcesFromJoints.wgsl b/physics/shaders/ForcesFromJoints.wgsl new file mode 100644 index 00000000..0bb58bca --- /dev/null +++ b/physics/shaders/ForcesFromJoints.wgsl @@ -0,0 +1,308 @@ +// Code generated by "gosl"; DO NOT EDIT +// kernel: ForcesFromJoints + +// // Params are global parameters. +@group(0) @binding(0) +var TensorStrides: array; +@group(0) @binding(1) +var Params: array; +// // Bodies are the rigid body elements (dynamic and static), // specifying the constant, non-dynamic properties, // which is initial state for dynamics. // [body][BodyVarsN] +@group(1) @binding(1) +var Joints: array; +@group(1) @binding(2) +var BodyJoints: array; +// // Dynamics are the dynamic rigid body elements: these actually move. // Two alternating states are used: Params.Cur and Params.Next. // [dyn body][cur/next][DynamicVarsN] +@group(2) @binding(0) +var Dynamics: array; +// // JointControls are dynamic joint control inputs. // [joint][JointControlVarsN] + +alias GPUVars = i32; + +@compute @workgroup_size(64, 1, 1) +fn main(@builtin(workgroup_id) wgid: vec3, @builtin(num_workgroups) nwg: vec3, @builtin(local_invocation_index) loci: u32) { + let idx = loci + (wgid.x + wgid.y * nwg.x + wgid.z * nwg.x * nwg.y) * 64; + ForcesFromJoints(idx); +} + +fn Index2D(s0: u32, s1: u32, i0: u32, i1: u32) -> u32 { + return s0 * i0 + s1 * i1; +} + +fn Index3D(s0: u32, s1: u32, s2: u32, i0: u32, i1: u32, i2: u32) -> u32 { + return s0 * i0 + s1 * i1 + s2 * i2; +} + + +//////// import: "vars.go" + +//////// import: "body.go" +alias BodyVars = i32; //enums:enum +const BodyShape: BodyVars = 0; +const BodyWorldIndex: BodyVars = 1; +const BodySizeX: BodyVars = 2; +const BodySizeY: BodyVars = 3; +const BodySizeZ: BodyVars = 4; +const BodyMass: BodyVars = 5; +const BodyInvMass: BodyVars = 6; +const BodyBounce: BodyVars = 7; +const BodyFriction: BodyVars = 8; +const BodyPosX: BodyVars = 9; +const BodyPosY: BodyVars = 10; +const BodyPosZ: BodyVars = 11; +const BodyRotX: BodyVars = 12; +const BodyRotY: BodyVars = 13; +const BodyRotZ: BodyVars = 14; +const BodyRotW: BodyVars = 15; +const BodyComX: BodyVars = 16; +const BodyComY: BodyVars = 17; +const BodyComZ: BodyVars = 18; +const BodyInertiaXX: BodyVars = 19; +const BodyInertiaYX: BodyVars = 20; +const BodyInertiaZX: BodyVars = 21; +const BodyInertiaXY: BodyVars = 22; +const BodyInertiaYY: BodyVars = 23; +const BodyInertiaZY: BodyVars = 24; +const BodyInertiaXZ: BodyVars = 25; +const BodyInertiaYZ: BodyVars = 26; +const BodyInertiaZZ: BodyVars = 27; +const BodyInvInertiaXX: BodyVars = 28; +const BodyInvInertiaYX: BodyVars = 29; +const BodyInvInertiaZX: BodyVars = 30; +const BodyInvInertiaXY: BodyVars = 31; +const BodyInvInertiaYY: BodyVars = 32; +const BodyInvInertiaZY: BodyVars = 33; +const BodyInvInertiaXZ: BodyVars = 34; +const BodyInvInertiaYZ: BodyVars = 35; +const BodyInvInertiaZZ: BodyVars = 36; + +//////// import: "contact.go" +alias ContactVars = i32; //enums:enum +const ContactA: ContactVars = 0; +const ContactB: ContactVars = 1; +const ContactNormX: ContactVars = 2; +const ContactNormY: ContactVars = 3; +const ContactNormZ: ContactVars = 4; +const ContactPointX: ContactVars = 5; +const ContactPointY: ContactVars = 6; +const ContactPointZ: ContactVars = 7; +const ContactDist: ContactVars = 8; + +//////// import: "control.go" +alias JointControlVars = i32; //enums:enum +const JointCtrlForceX: JointControlVars = 0; +const JointCtrlForceY: JointControlVars = 1; +const JointCtrlForceZ: JointControlVars = 2; +const JointCtrlTorqueX: JointControlVars = 3; +const JointCtrlTorqueY: JointControlVars = 4; +const JointCtrlTorqueZ: JointControlVars = 5; +const JointTargetPosX: JointControlVars = 6; +const JointTargetPosY: JointControlVars = 7; +const JointTargetPosZ: JointControlVars = 8; +const JointTargetRotX: JointControlVars = 9; +const JointTargetRotY: JointControlVars = 10; +const JointTargetRotZ: JointControlVars = 11; +const JointTargetRotW: JointControlVars = 12; +const JointTargetVelX: JointControlVars = 13; +const JointTargetVelY: JointControlVars = 14; +const JointTargetVelZ: JointControlVars = 15; +const JointTargetAngVelX: JointControlVars = 16; +const JointTargetAngVelY: JointControlVars = 17; +const JointTargetAngVelZ: JointControlVars = 18; + +//////// import: "dynamics.go" +alias DynamicVars = i32; //enums:enum +const DynIndex: DynamicVars = 0; +const DynPosX: DynamicVars = 1; +const DynPosY: DynamicVars = 2; +const DynPosZ: DynamicVars = 3; +const DynRotX: DynamicVars = 4; +const DynRotY: DynamicVars = 5; +const DynRotZ: DynamicVars = 6; +const DynRotW: DynamicVars = 7; +const DynVelX: DynamicVars = 8; +const DynVelY: DynamicVars = 9; +const DynVelZ: DynamicVars = 10; +const DynAngVelX: DynamicVars = 11; +const DynAngVelY: DynamicVars = 12; +const DynAngVelZ: DynamicVars = 13; +const DynAccX: DynamicVars = 14; +const DynAccY: DynamicVars = 15; +const DynAccZ: DynamicVars = 16; +const DynAngAccX: DynamicVars = 17; +const DynAngAccY: DynamicVars = 18; +const DynAngAccZ: DynamicVars = 19; +const DynForceX: DynamicVars = 20; +const DynForceY: DynamicVars = 21; +const DynForceZ: DynamicVars = 22; +const DynTorqueX: DynamicVars = 23; +const DynTorqueY: DynamicVars = 24; +const DynTorqueZ: DynamicVars = 25; +const DynDeltaX: DynamicVars = 26; +const DynDeltaY: DynamicVars = 27; +const DynDeltaZ: DynamicVars = 28; +const DynAngDeltaX: DynamicVars = 29; +const DynAngDeltaY: DynamicVars = 30; +const DynAngDeltaZ: DynamicVars = 31; +fn SetDynamicForce(idx: i32,cni: i32, force: vec3) { + Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynForceX))] = force.x; + Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynForceY))] = force.y; + Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynForceZ))] = force.z; +} +fn SetDynamicTorque(idx: i32,cni: i32, torque: vec3) { + Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynTorqueX))] = torque.x; + Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynTorqueY))] = torque.y; + Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynTorqueZ))] = torque.z; +} + +//////// import: "enumgen.go" +const BodyVarsN: BodyVars = 37; +const ContactVarsN: ContactVars = 9; +const JointControlVarsN: JointControlVars = 19; +const DynamicVarsN: DynamicVars = 32; +const GPUVarsN: GPUVars = 7; +const JointVarsN: JointVars = 53; +const JointTypesN: JointTypes = 7; +const ShapesN: Shapes = 4; + +//////// import: "joint.go" +alias JointVars = i32; //enums:enum +const JointType: JointVars = 0; +const JointEnabled: JointVars = 1; +const JointParent: JointVars = 2; +const JointChild: JointVars = 3; +const JointPPosX: JointVars = 4; +const JointPPosY: JointVars = 5; +const JointPPosZ: JointVars = 6; +const JointPRotX: JointVars = 7; +const JointPRotY: JointVars = 8; +const JointPRotZ: JointVars = 9; +const JointPRotW: JointVars = 10; +const JointCPosX: JointVars = 11; +const JointCPosY: JointVars = 12; +const JointCPosZ: JointVars = 13; +const JointCRotX: JointVars = 14; +const JointCRotY: JointVars = 15; +const JointCRotZ: JointVars = 16; +const JointCRotW: JointVars = 17; +const JointAxisX: JointVars = 18; +const JointAxisY: JointVars = 19; +const JointAxisZ: JointVars = 20; +const JointLimitLower: JointVars = 21; +const JointLimitUpper: JointVars = 22; +const JointStiffX: JointVars = 23; +const JointStiffY: JointVars = 24; +const JointStiffZ: JointVars = 25; +const JointDampX: JointVars = 26; +const JointDampY: JointVars = 27; +const JointDampZ: JointVars = 28; +const JointPForceX: JointVars = 29; +const JointPForceY: JointVars = 30; +const JointPForceZ: JointVars = 31; +const JointPTorqueX: JointVars = 32; +const JointPTorqueY: JointVars = 33; +const JointPTorqueZ: JointVars = 34; +const JointCForceX: JointVars = 35; +const JointCForceY: JointVars = 36; +const JointCForceZ: JointVars = 37; +const JointCTorqueX: JointVars = 38; +const JointCTorqueY: JointVars = 39; +const JointCTorqueZ: JointVars = 40; +const JointPDeltaX: JointVars = 41; +const JointPDeltaY: JointVars = 42; +const JointPDeltaZ: JointVars = 43; +const JointPAngDeltaX: JointVars = 44; +const JointPAngDeltaY: JointVars = 45; +const JointPAngDeltaZ: JointVars = 46; +const JointCDeltaX: JointVars = 47; +const JointCDeltaY: JointVars = 48; +const JointCDeltaZ: JointVars = 49; +const JointCAngDeltaX: JointVars = 50; +const JointCAngDeltaY: JointVars = 51; +const JointCAngDeltaZ: JointVars = 52; +fn JointPForce(idx: i32) -> vec3 { + return vec3(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPForceX))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPForceY))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPForceZ))]); +} +fn JointPTorque(idx: i32) -> vec3 { + return vec3(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPTorqueX))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPTorqueY))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPTorqueZ))]); +} +fn JointCForce(idx: i32) -> vec3 { + return vec3(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCForceX))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCForceY))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCForceZ))]); +} +fn JointCTorque(idx: i32) -> vec3 { + return vec3(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCTorqueX))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCTorqueY))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCTorqueZ))]); +} +alias JointTypes = i32; //enums:enum +const Prismatic: JointTypes = 0; +const Revolute: JointTypes = 1; +const Ball: JointTypes = 2; +const Fixed: JointTypes = 3; +const Free: JointTypes = 4; +const Distance: JointTypes = 5; +const D6: JointTypes = 6; + +//////// import: "params.go" +struct PhysParams { + Iters: i32, + Dt: f32, + SoftRelax: f32, + JointLinearRelax: f32, + JointAngularRelax: f32, + JointLinearComply: f32, + JointAngularComply: f32, + ContactRelax: f32, + AngularDamping: f32, + ContactWeighting: i32, + Restitution: i32, + DynamicsN: i32, + JointsN: i32, + BodyJointsMax: i32, + Cur: i32, + Next: i32, + Gravity: vec4, +} + +//////// import: "shapes.go" +alias Shapes = i32; //enums:enum +const Box: Shapes = 0; +const Sphere: Shapes = 1; +const Cylinder: Shapes = 2; +const Capsule: Shapes = 3; + +//////// import: "slmath-matrix3.go" + +//////// import: "slmath-quaternion.go" + +//////// import: "slmath-vector3.go" + +//////// import: "step.go" + +//////// import: "step_body.go" +fn ForcesFromJoints(i: u32) { //gosl:kernel + let params = Params[0]; + var di = i32(i); + if (di >= params.DynamicsN) { + return; + } + var np = BodyJoints[Index3D(TensorStrides[20], TensorStrides[21], TensorStrides[22], u32(di), u32(0), u32(0))]; + var nc = BodyJoints[Index3D(TensorStrides[20], TensorStrides[21], TensorStrides[22], u32(di), u32(1), u32(0))]; + var tf = vec3(0, 0, 0); + var tt = vec3(0, 0, 0); + for (var i = i32(1); i <= np; i++) { + var ji = BodyJoints[Index3D(TensorStrides[20], TensorStrides[21], TensorStrides[22], u32(di), u32(0), u32(i))]; + var f = JointPForce(ji); + tf = tf+(f); + var t = JointPTorque(ji); + tt = tt+(t); + } + for (var i = i32(1); i <= nc; i++) { + var ji = BodyJoints[Index3D(TensorStrides[20], TensorStrides[21], TensorStrides[22], u32(di), u32(1), u32(i))]; + var f = JointCForce(ji); + tf = tf+(f); + var t = JointCTorque(ji); + tt = tt+(t); + } + SetDynamicForce(di, params.Next, tf); + SetDynamicTorque(di, params.Next, tt); +} + +//////// import: "step_joint.go" \ No newline at end of file diff --git a/physics/shaders/InitDynamics.wgsl b/physics/shaders/InitDynamics.wgsl index d419e946..31834a3f 100644 --- a/physics/shaders/InitDynamics.wgsl +++ b/physics/shaders/InitDynamics.wgsl @@ -221,10 +221,6 @@ const D6: JointTypes = 6; //////// import: "params.go" struct PhysParams { - DynamicsN: i32, - JointsN: i32, - Cur: i32, - Next: i32, Iters: i32, Dt: f32, SoftRelax: f32, @@ -236,7 +232,11 @@ struct PhysParams { AngularDamping: f32, ContactWeighting: i32, Restitution: i32, - pad: f32, + DynamicsN: i32, + JointsN: i32, + BodyJointsMax: i32, + Cur: i32, + Next: i32, Gravity: vec4, } @@ -257,9 +257,9 @@ const Capsule: Shapes = 3; //////// import: "step_body.go" fn InitDynamics(i: u32) { //gosl:kernel - let pars = Params[0]; + let params = Params[0]; var ii = i32(i); - if (ii >= pars.DynamicsN) { + if (ii >= params.DynamicsN) { return; } for (var cni=0; cni<2; cni++) { diff --git a/physics/shaders/StepBodyDeltas.wgsl b/physics/shaders/StepBodyDeltas.wgsl index e1e7eec1..ea04a75b 100644 --- a/physics/shaders/StepBodyDeltas.wgsl +++ b/physics/shaders/StepBodyDeltas.wgsl @@ -267,10 +267,6 @@ const D6: JointTypes = 6; //////// import: "params.go" struct PhysParams { - DynamicsN: i32, - JointsN: i32, - Cur: i32, - Next: i32, Iters: i32, Dt: f32, SoftRelax: f32, @@ -282,7 +278,11 @@ struct PhysParams { AngularDamping: f32, ContactWeighting: i32, Restitution: i32, - pad: f32, + DynamicsN: i32, + JointsN: i32, + BodyJointsMax: i32, + Cur: i32, + Next: i32, Gravity: vec4, } @@ -346,34 +346,34 @@ fn Cross3(v: vec3,o: vec3) -> vec3 { //////// import: "step_body.go" fn StepBodyDeltas(i: u32) { //gosl:kernel - let pars = Params[0]; + let params = Params[0]; var di = i32(i); - if (di >= pars.DynamicsN) { + if (di >= params.DynamicsN) { return; } - var bi = DynamicIndex(di, pars.Cur); + var bi = DynamicIndex(di, params.Cur); var invMass = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyInvMass))]; if (invMass == 0) { return; // no updates } var inertia = BodyInertia(bi); var invInertia = BodyInvInertia(bi); - var p0 = DynamicPos(di, pars.Next); - var q0 = DynamicRot(di, pars.Next); - var v0 = DynamicDelta(di, pars.Next); - var w0 = DynamicAngDelta(di, pars.Next); + var p0 = DynamicPos(di, params.Next); + var q0 = DynamicRot(di, params.Next); + var v0 = DynamicDelta(di, params.Next); + var w0 = DynamicAngDelta(di, params.Next); var weight = f32(1.0); var dp = v0*(invMass * weight); var dq = w0*(weight); var wb = MulQuatVectorInverse(q0, w0); var dwb = invInertia*(MulQuatVectorInverse(q0, dq)); var tb = Cross3(dwb, inertia*(wb+(dwb)))+(Cross3(wb, inertia*(dwb))); - var dw1 = MulQuatVector(q0, dwb-(invInertia*(tb)*(pars.Dt))); - var q1 = q0+(MulQuats(vec4(dw1.x, dw1.y, dw1.z, 0), q0)*(0.5 * pars.Dt)); + var dw1 = MulQuatVector(q0, dwb-(invInertia*(tb)*(params.Dt))); + var q1 = q0+(MulQuats(vec4(dw1.x, dw1.y, dw1.z, 0), q0)*(0.5 * params.Dt)); q1 = QuatNormalize(q1); var com = BodyCom(bi); var pcom = MulQuatVector(q0, com)+(p0); - var p1 = pcom+(dp*(pars.Dt)); + var p1 = pcom+(dp*(params.Dt)); p1 = p1-(MulQuatVector(q1, com)); var v1 = v0+(dp); var w1 = w0+(dw1); @@ -383,10 +383,10 @@ fn StepBodyDeltas(i: u32) { //gosl:kernel if (Length3(w1) < 1e-4) { w1 = vec3(0, 0, 0); } - SetDynamicPos(di, pars.Next, p1); - SetDynamicRot(di, pars.Next, q1); - SetDynamicDelta(di, pars.Next, v1); - SetDynamicAngDelta(di, pars.Next, w1); + SetDynamicPos(di, params.Next, p1); + SetDynamicRot(di, params.Next, q1); + SetDynamicDelta(di, params.Next, v1); + SetDynamicAngDelta(di, params.Next, w1); } //////// import: "step_joint.go" \ No newline at end of file diff --git a/physics/shaders/StepIntegrateBodies.wgsl b/physics/shaders/StepIntegrateBodies.wgsl index e061c866..2b229a11 100644 --- a/physics/shaders/StepIntegrateBodies.wgsl +++ b/physics/shaders/StepIntegrateBodies.wgsl @@ -273,10 +273,6 @@ const D6: JointTypes = 6; //////// import: "params.go" struct PhysParams { - DynamicsN: i32, - JointsN: i32, - Cur: i32, - Next: i32, Iters: i32, Dt: f32, SoftRelax: f32, @@ -288,7 +284,11 @@ struct PhysParams { AngularDamping: f32, ContactWeighting: i32, Restitution: i32, - pad: f32, + DynamicsN: i32, + JointsN: i32, + BodyJointsMax: i32, + Cur: i32, + Next: i32, Gravity: vec4, } @@ -354,36 +354,36 @@ fn OneIfNonzero(f: f32) -> f32 { //////// import: "step_body.go" fn StepIntegrateBodies(i: u32) { //gosl:kernel - let pars = Params[0]; + let params = Params[0]; var di = i32(i); - if (di >= pars.DynamicsN) { + if (di >= params.DynamicsN) { return; } - var bi = DynamicIndex(di, pars.Cur); + var bi = DynamicIndex(di, params.Cur); var invMass = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyInvMass))]; var inertia = BodyInertia(bi); var invInertia = BodyInvInertia(bi); var com = BodyCom(bi); - var p0 = DynamicPos(di, pars.Cur); - var q0 = DynamicRot(di, pars.Cur); - var v0 = DynamicDelta(di, pars.Cur); - var w0 = DynamicAngDelta(di, pars.Cur); - var f0 = DynamicForce(di, pars.Next); - var t0 = DynamicTorque(di, pars.Next); + var p0 = DynamicPos(di, params.Cur); + var q0 = DynamicRot(di, params.Cur); + var v0 = DynamicDelta(di, params.Cur); + var w0 = DynamicAngDelta(di, params.Cur); + var f0 = DynamicForce(di, params.Next); + var t0 = DynamicTorque(di, params.Next); var pcom = MulQuatVector(q0, com)+(p0); - var v1 = v0+(f0*(invMass)+(vec3(pars.Gravity.x,pars.Gravity.y,pars.Gravity.z)*(OneIfNonzero(invMass)))*(pars.Dt)); - var p1 = pcom+(v1*(pars.Dt)); + var v1 = v0+(f0*(invMass)+(vec3(params.Gravity.x,params.Gravity.y,params.Gravity.z)*(OneIfNonzero(invMass)))*(params.Dt)); + var p1 = pcom+(v1*(params.Dt)); var wb = MulQuatVectorInverse(q0, w0); var tb = MulQuatVectorInverse(q0, t0)-(Cross3(wb, inertia*(wb))); // coriolis forces - var w1 = MulQuatVector(q0, wb+(invInertia*(tb)*(pars.Dt))); - var q1 = MulQuats(vec4(w1.x, w1.y, w1.z, 0), q0)*(0.5 * pars.Dt); + var w1 = MulQuatVector(q0, wb+(invInertia*(tb)*(params.Dt))); + var q1 = MulQuats(vec4(w1.x, w1.y, w1.z, 0), q0)*(0.5 * params.Dt); q1 = QuatNormalize(q1); - w1 = w1*(1.0 - pars.AngularDamping*pars.Dt); + w1 = w1*(1.0 - params.AngularDamping*params.Dt); var p1a = p1-(MulQuatVector(q1, com)); // pos corrected to nominal center. - SetDynamicPos(di, pars.Next, p1a); - SetDynamicRot(di, pars.Next, q1); - SetDynamicDelta(di, pars.Next, v1); - SetDynamicAngDelta(di, pars.Next, w1); + SetDynamicPos(di, params.Next, p1a); + SetDynamicRot(di, params.Next, q1); + SetDynamicDelta(di, params.Next, v1); + SetDynamicAngDelta(di, params.Next, w1); } //////// import: "step_joint.go" \ No newline at end of file diff --git a/physics/shaders/StepJointForces.wgsl b/physics/shaders/StepJointForces.wgsl index f243c9da..1eafb8a8 100644 --- a/physics/shaders/StepJointForces.wgsl +++ b/physics/shaders/StepJointForces.wgsl @@ -278,10 +278,6 @@ const D6: JointTypes = 6; //////// import: "params.go" struct PhysParams { - DynamicsN: i32, - JointsN: i32, - Cur: i32, - Next: i32, Iters: i32, Dt: f32, SoftRelax: f32, @@ -293,7 +289,11 @@ struct PhysParams { AngularDamping: f32, ContactWeighting: i32, Restitution: i32, - pad: f32, + DynamicsN: i32, + JointsN: i32, + BodyJointsMax: i32, + Cur: i32, + Next: i32, Gravity: vec4, } @@ -340,15 +340,15 @@ fn Cross3(v: vec3,o: vec3) -> vec3 { //////// import: "step_joint.go" fn StepJointForces(i: u32) { //gosl:kernel - let pars = Params[0]; + let params = Params[0]; var ji = i32(i); - if (ji >= pars.JointsN) { + if (ji >= params.JointsN) { return; } var jpi = JointParentIndex(ji); - var jpbi = DynamicIndex(jpi, pars.Cur); + var jpbi = DynamicIndex(jpi, params.Cur); var jci = JointChildIndex(ji); - var jcbi = DynamicIndex(jci, pars.Cur); + var jcbi = DynamicIndex(jci, params.Cur); var jt = GetJointType(ji); var jpP = JointPPos(ji); var jpQ = JointPRot(ji); @@ -358,14 +358,14 @@ fn StepJointForces(i: u32) { //gosl:kernel var posepQ = jpQ; var comp: vec3; if (jpi >= 0) { // can be fixed - posepP = DynamicPos(jpi, pars.Cur); - posepQ = DynamicRot(jpi, pars.Cur); + posepP = DynamicPos(jpi, params.Cur); + posepQ = DynamicRot(jpi, params.Cur); MulQPTransforms(posepP, posepQ, jpP, jpQ, &xwpP, &xwpQ); comp = BodyCom(jpbi); } var rp = xwpP-(MulQPPoint(posepP, posepQ, comp)); // parent moment arm - var posecP = DynamicPos(jci, pars.Cur); - var posecQ = DynamicRot(jci, pars.Cur); + var posecP = DynamicPos(jci, params.Cur); + var posecQ = DynamicRot(jci, params.Cur); var xwcP = posecP; var comc = BodyCom(jcbi); var rc = xwcP-(MulQPPoint(posecP, posecQ, comc)); // child moment arm diff --git a/physics/shaders/StepSolveJoints.wgsl b/physics/shaders/StepSolveJoints.wgsl index b21bdbe4..6c91e937 100644 --- a/physics/shaders/StepSolveJoints.wgsl +++ b/physics/shaders/StepSolveJoints.wgsl @@ -301,10 +301,6 @@ const D6: JointTypes = 6; //////// import: "params.go" struct PhysParams { - DynamicsN: i32, - JointsN: i32, - Cur: i32, - Next: i32, Iters: i32, Dt: f32, SoftRelax: f32, @@ -316,7 +312,11 @@ struct PhysParams { AngularDamping: f32, ContactWeighting: i32, Restitution: i32, - pad: f32, + DynamicsN: i32, + JointsN: i32, + BodyJointsMax: i32, + Cur: i32, + Next: i32, Gravity: vec4, } @@ -415,15 +415,15 @@ fn Cross3(v: vec3,o: vec3) -> vec3 { //////// import: "step_joint.go" fn StepSolveJoints(i: u32) { //gosl:kernel - let pars = Params[0]; + let params = Params[0]; var ji = i32(i); - if (ji >= pars.JointsN) { + if (ji >= params.JointsN) { return; } var jpi = JointParentIndex(ji); - var jpbi = DynamicIndex(jpi, pars.Cur); + var jpbi = DynamicIndex(jpi, params.Cur); var jci = JointChildIndex(ji); - var jcbi = DynamicIndex(jci, pars.Cur); + var jcbi = DynamicIndex(jci, params.Cur); var jt = GetJointType(ji); if (jt == Free) { return; @@ -441,17 +441,17 @@ fn StepSolveJoints(i: u32) { //gosl:kernel velp: vec3; var omegap: vec3; if (jpi >= 0) { - posepP = DynamicPos(jpi, pars.Next); // now using next - posepQ = DynamicRot(jpi, pars.Next); + posepP = DynamicPos(jpi, params.Next); // now using next + posepQ = DynamicRot(jpi, params.Next); MulQPTransforms(posepP, posepQ, jpP, jpQ, &xwpP, &xwpQ); comp = BodyCom(jpbi); mInvp = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(jpbi), u32(BodyInvMass))]; iInvp = BodyInvInertia(jpbi); - velp = DynamicDelta(jpi, pars.Next); - omegap = DynamicAngDelta(jpi, pars.Next); + velp = DynamicDelta(jpi, params.Next); + omegap = DynamicAngDelta(jpi, params.Next); } - var posecP = DynamicPos(jci, pars.Next); - var posecQ = DynamicRot(jci, pars.Next); + var posecP = DynamicPos(jci, params.Next); + var posecQ = DynamicRot(jci, params.Next); var jcP = JointCPos(ji); var jcQ = JointCRot(ji); var xwcP = jcP; @@ -460,8 +460,8 @@ fn StepSolveJoints(i: u32) { //gosl:kernel var comc = BodyCom(jcbi); var mInvc = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(jcbi), u32(BodyInvMass))]; var iInvc = BodyInvInertia(jcbi); - var velc = DynamicDelta(jci, pars.Next); - var omegac = DynamicAngDelta(jci, pars.Next); + var velc = DynamicDelta(jci, params.Next); + var omegac = DynamicAngDelta(jci, params.Next); if (mInvp == 0.0 && mInvc == 0.0) { // connection between two immovable bodies return; } @@ -520,7 +520,7 @@ fn StepSolveJoints(i: u32) { //gosl:kernel var angularc = Cross3(rc, linearc); var derr = Dot3(linearp, velp) + Dot3(linearc, velc) + Dot3(angularp, omegap) + Dot3(angularc, omegac); var err = f32(0.0); - var compliance = pars.JointLinearComply; + var compliance = params.JointLinearComply; var damping = f32(0.0); var targetVel = axisTargetVelKdD.x; // [dim] var derrRel = derr - targetVel; @@ -545,11 +545,11 @@ fn StepSolveJoints(i: u32) { //gosl:kernel if (abs(err) > 1e-9 || abs(derrRel) > 1e-9) { var lambdaIn = f32(0.0); var dLambda = PositionalCorrection(err, derrRel, posepQ, posecQ, mInvp, mInvc, - iInvp, iInvc, linearp, linearc, angularp, angularc, lambdaIn, compliance, damping, pars.Dt); - linDeltaP = linDeltaP+(linearp*(dLambda * pars.JointLinearRelax)); - angDeltaP = angDeltaP+(angularp*(dLambda * pars.JointAngularRelax)); - linDeltaC = linDeltaC+(linearc*(dLambda * pars.JointLinearRelax)); - angDeltaC = angDeltaC+(angularc*(dLambda * pars.JointAngularRelax)); + iInvp, iInvc, linearp, linearc, angularp, angularc, lambdaIn, compliance, damping, params.Dt); + linDeltaP = linDeltaP+(linearp*(dLambda * params.JointLinearRelax)); + angDeltaP = angDeltaP+(angularp*(dLambda * params.JointAngularRelax)); + linDeltaC = linDeltaC+(linearc*(dLambda * params.JointLinearRelax)); + angDeltaC = angDeltaC+(angularc*(dLambda * params.JointAngularRelax)); } } } diff --git a/physics/step.go b/physics/step.go index 351d28fe..869c99d2 100644 --- a/physics/step.go +++ b/physics/step.go @@ -48,6 +48,6 @@ func OneIfNonzero(f float32) float32 { //gosl:end func (wl *World) StepJointForces() { - pars := GetParams(0) - RunStepJointForces(int(pars.JointsN)) + params := GetParams(0) + RunStepJointForces(int(params.JointsN)) } diff --git a/physics/step.goal b/physics/step.goal index c4b501c9..a52c600c 100644 --- a/physics/step.goal +++ b/physics/step.goal @@ -46,6 +46,6 @@ func OneIfNonzero(f float32) float32 { //gosl:end func (wl *World) StepJointForces() { - pars := GetParams(0) - RunStepJointForces(int(pars.JointsN)) + params := GetParams(0) + RunStepJointForces(int(params.JointsN)) } diff --git a/physics/step_body.go b/physics/step_body.go index a4a01c86..04813d3d 100644 --- a/physics/step_body.go +++ b/physics/step_body.go @@ -19,9 +19,9 @@ import ( // InitDynamics copies Body initial state to dynamic state (cur and next). func InitDynamics(i uint32) { //gosl:kernel - pars := GetParams(0) + params := GetParams(0) ii := int32(i) - if ii >= pars.DynamicsN { + if ii >= params.DynamicsN { return } for cni := range 2 { @@ -43,26 +43,84 @@ func InitDynamics(i uint32) { //gosl:kernel // DynamicsCurToNext copies [Dynamics] state from Cur to Next. func DynamicsCurToNext(i uint32) { //gosl:kernel - pars := GetParams(0) + params := GetParams(0) ii := int32(i) - if ii >= pars.DynamicsN { + if ii >= params.DynamicsN { return } for di := DynIndex; di < DynamicVarsN; di++ { - Dynamics.Set(Dynamics.Value(int(ii), int(pars.Cur), int(di)), int(ii), int(pars.Next), int(di)) + Dynamics.Set(Dynamics.Value(int(ii), int(params.Cur), int(di)), int(ii), int(params.Next), int(di)) } } -// todo: aggregate forces +// ForcesFromJoints gathers forces and torques from joints per dynamic +func ForcesFromJoints(i uint32) { //gosl:kernel + params := GetParams(0) + di := int32(i) + if di >= params.DynamicsN { + return + } + np := BodyJoints.Value(int(di), int(0), int(0)) + nc := BodyJoints.Value(int(di), int(1), int(0)) + + tf := math32.Vec3(0, 0, 0) + tt := math32.Vec3(0, 0, 0) + for i := int32(1); i <= np; i++ { + ji := BodyJoints.Value(int(di), int(0), int(i)) + f := JointPForce(ji) + tf = tf.Add(f) + t := JointPTorque(ji) + tt = tt.Add(t) + } + for i := int32(1); i <= nc; i++ { + ji := BodyJoints.Value(int(di), int(1), int(i)) + f := JointCForce(ji) + tf = tf.Add(f) + t := JointCTorque(ji) + tt = tt.Add(t) + } + SetDynamicForce(di, params.Next, tf) + SetDynamicTorque(di, params.Next, tt) +} + +// DeltasFromJoints gathers deltas, angDeltas from joints per dynamic +func DeltasFromJoints(i uint32) { //gosl:kernel + params := GetParams(0) + di := int32(i) + if di >= params.DynamicsN { + return + } + np := BodyJoints.Value(int(di), int(0), int(0)) + nc := BodyJoints.Value(int(di), int(1), int(0)) + + td := math32.Vec3(0, 0, 0) + ta := math32.Vec3(0, 0, 0) + for i := int32(1); i <= np; i++ { + ji := BodyJoints.Value(int(di), int(0), int(i)) + d := JointPDelta(ji) + td = td.Add(d) + a := JointPAngDelta(ji) + ta = ta.Add(a) + } + for i := int32(1); i <= nc; i++ { + ji := BodyJoints.Value(int(di), int(1), int(i)) + d := JointCDelta(ji) + td = td.Add(d) + a := JointCAngDelta(ji) + ta = ta.Add(a) + } + SetDynamicDelta(di, params.Next, td) + SetDynamicAngDelta(di, params.Next, ta) +} // StepIntegrateBodies applies forces to update pos and deltas func StepIntegrateBodies(i uint32) { //gosl:kernel - pars := GetParams(0) + params := GetParams(0) di := int32(i) - if di >= pars.DynamicsN { + if di >= params.DynamicsN { return } - bi := DynamicIndex(di, pars.Cur) + bi := DynamicIndex(di, params.Cur) invMass := Bodies.Value(int(bi), int(BodyInvMass)) inertia := BodyInertia(bi) @@ -71,50 +129,50 @@ func StepIntegrateBodies(i uint32) { //gosl:kernel com := BodyCom(bi) // current pos - p0 := DynamicPos(di, pars.Cur) - q0 := DynamicRot(di, pars.Cur) + p0 := DynamicPos(di, params.Cur) + q0 := DynamicRot(di, params.Cur) // current deltas - v0 := DynamicDelta(di, pars.Cur) - w0 := DynamicAngDelta(di, pars.Cur) + v0 := DynamicDelta(di, params.Cur) + w0 := DynamicAngDelta(di, params.Cur) // new forces integrated from joints - f0 := DynamicForce(di, pars.Next) - t0 := DynamicTorque(di, pars.Next) + f0 := DynamicForce(di, params.Next) + t0 := DynamicTorque(di, params.Next) pcom := slmath.MulQuatVector(q0, com).Add(p0) // linear part - v1 := v0.Add(f0.MulScalar(invMass).Add(pars.Gravity.V().MulScalar(OneIfNonzero(invMass))).MulScalar(pars.Dt)) - p1 := pcom.Add(v1.MulScalar(pars.Dt)) + v1 := v0.Add(f0.MulScalar(invMass).Add(params.Gravity.V().MulScalar(OneIfNonzero(invMass))).MulScalar(params.Dt)) + p1 := pcom.Add(v1.MulScalar(params.Dt)) // angular part (compute in body frame) wb := slmath.MulQuatVectorInverse(q0, w0) tb := slmath.MulQuatVectorInverse(q0, t0).Sub(slmath.Cross3(wb, inertia.MulVector3(wb))) // coriolis forces - w1 := slmath.MulQuatVector(q0, wb.Add(invInertia.MulVector3(tb).MulScalar(pars.Dt))) - q1 := slmath.MulQuats(math32.NewQuat(w1.X, w1.Y, w1.Z, 0), q0).MulScalar(0.5 * pars.Dt) + w1 := slmath.MulQuatVector(q0, wb.Add(invInertia.MulVector3(tb).MulScalar(params.Dt))) + q1 := slmath.MulQuats(math32.NewQuat(w1.X, w1.Y, w1.Z, 0), q0).MulScalar(0.5 * params.Dt) q1 = slmath.QuatNormalize(q1) // angular damping - w1 = w1.MulScalar(1.0 - pars.AngularDamping*pars.Dt) + w1 = w1.MulScalar(1.0 - params.AngularDamping*params.Dt) p1a := p1.Sub(slmath.MulQuatVector(q1, com)) // pos corrected to nominal center. - SetDynamicPos(di, pars.Next, p1a) - SetDynamicRot(di, pars.Next, q1) - SetDynamicDelta(di, pars.Next, v1) - SetDynamicAngDelta(di, pars.Next, w1) + SetDynamicPos(di, params.Next, p1a) + SetDynamicRot(di, params.Next, q1) + SetDynamicDelta(di, params.Next, v1) + SetDynamicAngDelta(di, params.Next, w1) } // StepBodyDeltas updates Next position with deltas. func StepBodyDeltas(i uint32) { //gosl:kernel - pars := GetParams(0) + params := GetParams(0) di := int32(i) - if di >= pars.DynamicsN { + if di >= params.DynamicsN { return } - bi := DynamicIndex(di, pars.Cur) + bi := DynamicIndex(di, params.Cur) invMass := Bodies.Value(int(bi), int(BodyInvMass)) if invMass == 0 { @@ -124,12 +182,12 @@ func StepBodyDeltas(i uint32) { //gosl:kernel invInertia := BodyInvInertia(bi) // starting pos (from force integration) - p0 := DynamicPos(di, pars.Next) - q0 := DynamicRot(di, pars.Next) + p0 := DynamicPos(di, params.Next) + q0 := DynamicRot(di, params.Next) // starting deltas - v0 := DynamicDelta(di, pars.Next) - w0 := DynamicAngDelta(di, pars.Next) + v0 := DynamicDelta(di, params.Next) + w0 := DynamicAngDelta(di, params.Next) weight := float32(1.0) // todo: this is rigid_contact_inv_weight in solver_xpbd.py: from contacts, with restitution @@ -145,10 +203,10 @@ func StepBodyDeltas(i uint32) { //gosl:kernel dwb := invInertia.MulVector3(slmath.MulQuatVectorInverse(q0, dq)) // coriolis forces delta from dwb = (wb + dwb) I (wb + dwb) - wb I wb tb := slmath.Cross3(dwb, inertia.MulVector3(wb.Add(dwb))).Add(slmath.Cross3(wb, inertia.MulVector3(dwb))) - dw1 := slmath.MulQuatVector(q0, dwb.Sub(invInertia.MulVector3(tb).MulScalar(pars.Dt))) + dw1 := slmath.MulQuatVector(q0, dwb.Sub(invInertia.MulVector3(tb).MulScalar(params.Dt))) // update orientation - q1 := q0.Add(slmath.MulQuats(math32.NewQuat(dw1.X, dw1.Y, dw1.Z, 0), q0).MulScalar(0.5 * pars.Dt)) + q1 := q0.Add(slmath.MulQuats(math32.NewQuat(dw1.X, dw1.Y, dw1.Z, 0), q0).MulScalar(0.5 * params.Dt)) // q1 := q0 + 0.5 * wp.quat(dw1 * dt, 0.0) * q0 q1 = slmath.QuatNormalize(q1) @@ -156,7 +214,7 @@ func StepBodyDeltas(i uint32) { //gosl:kernel com := BodyCom(bi) pcom := slmath.MulQuatVector(q0, com).Add(p0) - p1 := pcom.Add(dp.MulScalar(pars.Dt)) + p1 := pcom.Add(dp.MulScalar(params.Dt)) p1 = p1.Sub(slmath.MulQuatVector(q1, com)) // update linear and angular velocity @@ -171,10 +229,10 @@ func StepBodyDeltas(i uint32) { //gosl:kernel w1 = math32.Vec3(0, 0, 0) } - SetDynamicPos(di, pars.Next, p1) - SetDynamicRot(di, pars.Next, q1) - SetDynamicDelta(di, pars.Next, v1) - SetDynamicAngDelta(di, pars.Next, w1) + SetDynamicPos(di, params.Next, p1) + SetDynamicRot(di, params.Next, q1) + SetDynamicDelta(di, params.Next, v1) + SetDynamicAngDelta(di, params.Next, w1) } //gosl:end diff --git a/physics/step_body.goal b/physics/step_body.goal index c0106fd0..9a2646eb 100644 --- a/physics/step_body.goal +++ b/physics/step_body.goal @@ -17,9 +17,9 @@ import ( // InitDynamics copies Body initial state to dynamic state (cur and next). func InitDynamics(i uint32) { //gosl:kernel - pars := GetParams(0) + params := GetParams(0) ii := int32(i) - if ii >= pars.DynamicsN { + if ii >= params.DynamicsN { return } for cni := range 2 { @@ -41,26 +41,84 @@ func InitDynamics(i uint32) { //gosl:kernel // DynamicsCurToNext copies [Dynamics] state from Cur to Next. func DynamicsCurToNext(i uint32) { //gosl:kernel - pars := GetParams(0) + params := GetParams(0) ii := int32(i) - if ii >= pars.DynamicsN { + if ii >= params.DynamicsN { return } for di := DynIndex; di < DynamicVarsN; di++ { - Dynamics[ii, pars.Next, di] = Dynamics[ii, pars.Cur, di] + Dynamics[ii, params.Next, di] = Dynamics[ii, params.Cur, di] } } -// todo: aggregate forces +// ForcesFromJoints gathers forces and torques from joints per dynamic +func ForcesFromJoints(i uint32) { //gosl:kernel + params := GetParams(0) + di := int32(i) + if di >= params.DynamicsN { + return + } + np := BodyJoints[di, 0, 0] + nc := BodyJoints[di, 1, 0] + + tf := math32.Vec3(0,0,0) + tt := math32.Vec3(0,0,0) + for i := int32(1); i <= np; i++ { + ji := BodyJoints[di, 0, i] + f := JointPForce(ji) + tf = tf.Add(f) + t := JointPTorque(ji) + tt = tt.Add(t) + } + for i := int32(1); i <= nc; i++ { + ji := BodyJoints[di, 1, i] + f := JointCForce(ji) + tf = tf.Add(f) + t := JointCTorque(ji) + tt = tt.Add(t) + } + SetDynamicForce(di, params.Next, tf) + SetDynamicTorque(di, params.Next, tt) +} + +// DeltasFromJoints gathers deltas, angDeltas from joints per dynamic +func DeltasFromJoints(i uint32) { //gosl:kernel + params := GetParams(0) + di := int32(i) + if di >= params.DynamicsN { + return + } + np := BodyJoints[di, 0, 0] + nc := BodyJoints[di, 1, 0] + + td := math32.Vec3(0,0,0) + ta := math32.Vec3(0,0,0) + for i := int32(1); i <= np; i++ { + ji := BodyJoints[di, 0, i] + d := JointPDelta(ji) + td = td.Add(d) + a := JointPAngDelta(ji) + ta = ta.Add(a) + } + for i := int32(1); i <= nc; i++ { + ji := BodyJoints[di, 1, i] + d := JointCDelta(ji) + td = td.Add(d) + a := JointCAngDelta(ji) + ta = ta.Add(a) + } + SetDynamicDelta(di, params.Next, td) + SetDynamicAngDelta(di, params.Next, ta) +} // StepIntegrateBodies applies forces to update pos and deltas func StepIntegrateBodies(i uint32) { //gosl:kernel - pars := GetParams(0) + params := GetParams(0) di := int32(i) - if di >= pars.DynamicsN { + if di >= params.DynamicsN { return } - bi := DynamicIndex(di, pars.Cur) + bi := DynamicIndex(di, params.Cur) invMass := Bodies[bi, BodyInvMass] inertia := BodyInertia(bi) @@ -69,50 +127,50 @@ func StepIntegrateBodies(i uint32) { //gosl:kernel com := BodyCom(bi) // current pos - p0 := DynamicPos(di, pars.Cur) - q0 := DynamicRot(di, pars.Cur) + p0 := DynamicPos(di, params.Cur) + q0 := DynamicRot(di, params.Cur) // current deltas - v0 := DynamicDelta(di, pars.Cur) - w0 := DynamicAngDelta(di, pars.Cur) + v0 := DynamicDelta(di, params.Cur) + w0 := DynamicAngDelta(di, params.Cur) // new forces integrated from joints - f0 := DynamicForce(di, pars.Next) - t0 := DynamicTorque(di, pars.Next) + f0 := DynamicForce(di, params.Next) + t0 := DynamicTorque(di, params.Next) pcom := slmath.MulQuatVector(q0, com).Add(p0) // linear part - v1 := v0.Add(f0.MulScalar(invMass).Add(pars.Gravity.V().MulScalar(OneIfNonzero(invMass))).MulScalar(pars.Dt)) - p1 := pcom.Add(v1.MulScalar(pars.Dt)) + v1 := v0.Add(f0.MulScalar(invMass).Add(params.Gravity.V().MulScalar(OneIfNonzero(invMass))).MulScalar(params.Dt)) + p1 := pcom.Add(v1.MulScalar(params.Dt)) // angular part (compute in body frame) wb := slmath.MulQuatVectorInverse(q0, w0) tb := slmath.MulQuatVectorInverse(q0, t0).Sub(slmath.Cross3(wb, inertia.MulVector3(wb))) // coriolis forces - w1 := slmath.MulQuatVector(q0, wb.Add(invInertia.MulVector3(tb).MulScalar(pars.Dt))) - q1 := slmath.MulQuats(math32.NewQuat(w1.X, w1.Y, w1.Z, 0), q0).MulScalar(0.5 * pars.Dt) + w1 := slmath.MulQuatVector(q0, wb.Add(invInertia.MulVector3(tb).MulScalar(params.Dt))) + q1 := slmath.MulQuats(math32.NewQuat(w1.X, w1.Y, w1.Z, 0), q0).MulScalar(0.5 * params.Dt) q1 = slmath.QuatNormalize(q1) // angular damping - w1 = w1.MulScalar(1.0 - pars.AngularDamping*pars.Dt) + w1 = w1.MulScalar(1.0 - params.AngularDamping*params.Dt) p1a := p1.Sub(slmath.MulQuatVector(q1, com)) // pos corrected to nominal center. - SetDynamicPos(di, pars.Next, p1a) - SetDynamicRot(di, pars.Next, q1) - SetDynamicDelta(di, pars.Next, v1) - SetDynamicAngDelta(di, pars.Next, w1) + SetDynamicPos(di, params.Next, p1a) + SetDynamicRot(di, params.Next, q1) + SetDynamicDelta(di, params.Next, v1) + SetDynamicAngDelta(di, params.Next, w1) } // StepBodyDeltas updates Next position with deltas. func StepBodyDeltas(i uint32) { //gosl:kernel - pars := GetParams(0) + params := GetParams(0) di := int32(i) - if di >= pars.DynamicsN { + if di >= params.DynamicsN { return } - bi := DynamicIndex(di, pars.Cur) + bi := DynamicIndex(di, params.Cur) invMass := Bodies[bi, BodyInvMass] if invMass == 0 { @@ -122,12 +180,12 @@ func StepBodyDeltas(i uint32) { //gosl:kernel invInertia := BodyInvInertia(bi) // starting pos (from force integration) - p0 := DynamicPos(di, pars.Next) - q0 := DynamicRot(di, pars.Next) + p0 := DynamicPos(di, params.Next) + q0 := DynamicRot(di, params.Next) // starting deltas - v0 := DynamicDelta(di, pars.Next) - w0 := DynamicAngDelta(di, pars.Next) + v0 := DynamicDelta(di, params.Next) + w0 := DynamicAngDelta(di, params.Next) weight := float32(1.0) // todo: this is rigid_contact_inv_weight in solver_xpbd.py: from contacts, with restitution @@ -143,10 +201,10 @@ func StepBodyDeltas(i uint32) { //gosl:kernel dwb := invInertia.MulVector3(slmath.MulQuatVectorInverse(q0, dq)) // coriolis forces delta from dwb = (wb + dwb) I (wb + dwb) - wb I wb tb := slmath.Cross3(dwb, inertia.MulVector3(wb.Add(dwb))).Add(slmath.Cross3(wb, inertia.MulVector3(dwb))) - dw1 := slmath.MulQuatVector(q0, dwb.Sub(invInertia.MulVector3(tb).MulScalar(pars.Dt))) + dw1 := slmath.MulQuatVector(q0, dwb.Sub(invInertia.MulVector3(tb).MulScalar(params.Dt))) // update orientation - q1 := q0.Add(slmath.MulQuats(math32.NewQuat(dw1.X, dw1.Y, dw1.Z, 0), q0).MulScalar(0.5 * pars.Dt)) + q1 := q0.Add(slmath.MulQuats(math32.NewQuat(dw1.X, dw1.Y, dw1.Z, 0), q0).MulScalar(0.5 * params.Dt)) // q1 := q0 + 0.5 * wp.quat(dw1 * dt, 0.0) * q0 q1 = slmath.QuatNormalize(q1) @@ -154,7 +212,7 @@ func StepBodyDeltas(i uint32) { //gosl:kernel com := BodyCom(bi) pcom := slmath.MulQuatVector(q0, com).Add(p0) - p1 := pcom.Add(dp.MulScalar(pars.Dt)) + p1 := pcom.Add(dp.MulScalar(params.Dt)) p1 = p1.Sub(slmath.MulQuatVector(q1, com)) // update linear and angular velocity @@ -169,10 +227,10 @@ func StepBodyDeltas(i uint32) { //gosl:kernel w1 = math32.Vec3(0, 0, 0) } - SetDynamicPos(di, pars.Next, p1) - SetDynamicRot(di, pars.Next, q1) - SetDynamicDelta(di, pars.Next, v1) - SetDynamicAngDelta(di, pars.Next, w1) + SetDynamicPos(di, params.Next, p1) + SetDynamicRot(di, params.Next, q1) + SetDynamicDelta(di, params.Next, v1) + SetDynamicAngDelta(di, params.Next, w1) } //gosl:end diff --git a/physics/step_joint.go b/physics/step_joint.go index 051a200a..92b3b225 100644 --- a/physics/step_joint.go +++ b/physics/step_joint.go @@ -19,16 +19,16 @@ import ( // StepJointForces computes joint forces. func StepJointForces(i uint32) { //gosl:kernel - pars := GetParams(0) + params := GetParams(0) ji := int32(i) - if ji >= pars.JointsN { + if ji >= params.JointsN { return } // todo: enabled jpi := JointParentIndex(ji) - jpbi := DynamicIndex(jpi, pars.Cur) + jpbi := DynamicIndex(jpi, params.Cur) jci := JointChildIndex(ji) - jcbi := DynamicIndex(jci, pars.Cur) + jcbi := DynamicIndex(jci, params.Cur) jt := GetJointType(ji) jpP := JointPPos(ji) @@ -42,16 +42,16 @@ func StepJointForces(i uint32) { //gosl:kernel var comp math32.Vector3 if jpi >= 0 { // can be fixed - posepP = DynamicPos(jpi, pars.Cur) - posepQ = DynamicRot(jpi, pars.Cur) + posepP = DynamicPos(jpi, params.Cur) + posepQ = DynamicRot(jpi, params.Cur) slmath.MulQPTransforms(posepP, posepQ, jpP, jpQ, &xwpP, &xwpQ) comp = BodyCom(jpbi) } rp := xwpP.Sub(slmath.MulQPPoint(posepP, posepQ, comp)) // parent moment arm // child world transform - posecP := DynamicPos(jci, pars.Cur) - posecQ := DynamicRot(jci, pars.Cur) + posecP := DynamicPos(jci, params.Cur) + posecQ := DynamicRot(jci, params.Cur) xwcP := posecP // xwcQ := posecQ comc := BodyCom(jcbi) @@ -85,17 +85,17 @@ func StepJointForces(i uint32) { //gosl:kernel // StepSolveJoints fixes joints after updating bodies. func StepSolveJoints(i uint32) { //gosl:kernel - pars := GetParams(0) + params := GetParams(0) ji := int32(i) - if ji >= pars.JointsN { + if ji >= params.JointsN { return } // todo: enabled jpi := JointParentIndex(ji) - jpbi := DynamicIndex(jpi, pars.Cur) + jpbi := DynamicIndex(jpi, params.Cur) jci := JointChildIndex(ji) - jcbi := DynamicIndex(jci, pars.Cur) + jcbi := DynamicIndex(jci, params.Cur) jt := GetJointType(ji) if jt == Free { @@ -115,19 +115,19 @@ func StepSolveJoints(i uint32) { //gosl:kernel // parent transform and moment arm if jpi >= 0 { - posepP = DynamicPos(jpi, pars.Next) // now using next - posepQ = DynamicRot(jpi, pars.Next) + posepP = DynamicPos(jpi, params.Next) // now using next + posepQ = DynamicRot(jpi, params.Next) slmath.MulQPTransforms(posepP, posepQ, jpP, jpQ, &xwpP, &xwpQ) comp = BodyCom(jpbi) mInvp = Bodies.Value(int(jpbi), int(BodyInvMass)) iInvp = BodyInvInertia(jpbi) - velp = DynamicDelta(jpi, pars.Next) - omegap = DynamicAngDelta(jpi, pars.Next) + velp = DynamicDelta(jpi, params.Next) + omegap = DynamicAngDelta(jpi, params.Next) } // child transform and moment arm - posecP := DynamicPos(jci, pars.Next) - posecQ := DynamicRot(jci, pars.Next) + posecP := DynamicPos(jci, params.Next) + posecQ := DynamicRot(jci, params.Next) jcP := JointCPos(ji) jcQ := JointCRot(ji) xwcP := jcP @@ -136,8 +136,8 @@ func StepSolveJoints(i uint32) { //gosl:kernel comc := BodyCom(jcbi) mInvc := Bodies.Value(int(jcbi), int(BodyInvMass)) iInvc := BodyInvInertia(jcbi) - velc := DynamicDelta(jci, pars.Next) - omegac := DynamicAngDelta(jci, pars.Next) + velc := DynamicDelta(jci, params.Next) + omegac := DynamicAngDelta(jci, params.Next) if mInvp == 0.0 && mInvc == 0.0 { // connection between two immovable bodies return @@ -226,9 +226,9 @@ func StepSolveJoints(i uint32) { //gosl:kernel // dt, // ) // - // linDelta_p += linear_p * (d_lambda * pars.JointLinearRelax) + // linDelta_p += linear_p * (d_lambda * params.JointLinearRelax) // angDelta_p += angular_p * (d_lambda * angular_relaxation) - // linDelta_c += linear_c * (d_lambda * pars.JointLinearRelax) + // linDelta_c += linear_c * (d_lambda * params.JointLinearRelax) // angDelta_c += angular_c * (d_lambda * angular_relaxation) } else { // compute joint target, stiffness, damping var axisLimitsD, axisLimitsA math32.Vector3 @@ -322,7 +322,7 @@ func StepSolveJoints(i uint32) { //gosl:kernel derr := slmath.Dot3(linearp, velp) + slmath.Dot3(linearc, velc) + slmath.Dot3(angularp, omegap) + slmath.Dot3(angularc, omegac) err := float32(0.0) - compliance := pars.JointLinearComply + compliance := params.JointLinearComply damping := float32(0.0) targetVel := axisTargetVelKdD.X // [dim] @@ -351,12 +351,12 @@ func StepSolveJoints(i uint32) { //gosl:kernel if math32.Abs(err) > 1e-9 || math32.Abs(derrRel) > 1e-9 { lambdaIn := float32(0.0) dLambda := PositionalCorrection(err, derrRel, posepQ, posecQ, mInvp, mInvc, - iInvp, iInvc, linearp, linearc, angularp, angularc, lambdaIn, compliance, damping, pars.Dt) + iInvp, iInvc, linearp, linearc, angularp, angularc, lambdaIn, compliance, damping, params.Dt) - linDeltaP = linDeltaP.Add(linearp.MulScalar(dLambda * pars.JointLinearRelax)) - angDeltaP = angDeltaP.Add(angularp.MulScalar(dLambda * pars.JointAngularRelax)) - linDeltaC = linDeltaC.Add(linearc.MulScalar(dLambda * pars.JointLinearRelax)) - angDeltaC = angDeltaC.Add(angularc.MulScalar(dLambda * pars.JointAngularRelax)) + linDeltaP = linDeltaP.Add(linearp.MulScalar(dLambda * params.JointLinearRelax)) + angDeltaP = angDeltaP.Add(angularp.MulScalar(dLambda * params.JointAngularRelax)) + linDeltaC = linDeltaC.Add(linearc.MulScalar(dLambda * params.JointLinearRelax)) + angDeltaC = angDeltaC.Add(angularc.MulScalar(dLambda * params.JointAngularRelax)) } } } diff --git a/physics/step_joint.goal b/physics/step_joint.goal index 591953d7..6219c800 100644 --- a/physics/step_joint.goal +++ b/physics/step_joint.goal @@ -17,16 +17,16 @@ import ( // StepJointForces computes joint forces. func StepJointForces(i uint32) { //gosl:kernel - pars := GetParams(0) + params := GetParams(0) ji := int32(i) - if ji >= pars.JointsN { + if ji >= params.JointsN { return } // todo: enabled jpi := JointParentIndex(ji) - jpbi := DynamicIndex(jpi, pars.Cur) + jpbi := DynamicIndex(jpi, params.Cur) jci := JointChildIndex(ji) - jcbi := DynamicIndex(jci, pars.Cur) + jcbi := DynamicIndex(jci, params.Cur) jt := GetJointType(ji) jpP := JointPPos(ji) @@ -40,16 +40,16 @@ func StepJointForces(i uint32) { //gosl:kernel var comp math32.Vector3 if jpi >= 0 { // can be fixed - posepP = DynamicPos(jpi, pars.Cur) - posepQ = DynamicRot(jpi, pars.Cur) + posepP = DynamicPos(jpi, params.Cur) + posepQ = DynamicRot(jpi, params.Cur) slmath.MulQPTransforms(posepP, posepQ, jpP, jpQ, &xwpP, &xwpQ) comp = BodyCom(jpbi) } rp := xwpP.Sub(slmath.MulQPPoint(posepP, posepQ, comp)) // parent moment arm // child world transform - posecP := DynamicPos(jci, pars.Cur) - posecQ := DynamicRot(jci, pars.Cur) + posecP := DynamicPos(jci, params.Cur) + posecQ := DynamicRot(jci, params.Cur) xwcP := posecP // xwcQ := posecQ comc := BodyCom(jcbi) @@ -83,17 +83,17 @@ func StepJointForces(i uint32) { //gosl:kernel // StepSolveJoints fixes joints after updating bodies. func StepSolveJoints(i uint32) { //gosl:kernel - pars := GetParams(0) + params := GetParams(0) ji := int32(i) - if ji >= pars.JointsN { + if ji >= params.JointsN { return } // todo: enabled jpi := JointParentIndex(ji) - jpbi := DynamicIndex(jpi, pars.Cur) + jpbi := DynamicIndex(jpi, params.Cur) jci := JointChildIndex(ji) - jcbi := DynamicIndex(jci, pars.Cur) + jcbi := DynamicIndex(jci, params.Cur) jt := GetJointType(ji) if jt == Free { @@ -113,19 +113,19 @@ func StepSolveJoints(i uint32) { //gosl:kernel // parent transform and moment arm if jpi >= 0 { - posepP = DynamicPos(jpi, pars.Next) // now using next - posepQ = DynamicRot(jpi, pars.Next) + posepP = DynamicPos(jpi, params.Next) // now using next + posepQ = DynamicRot(jpi, params.Next) slmath.MulQPTransforms(posepP, posepQ, jpP, jpQ, &xwpP, &xwpQ) comp = BodyCom(jpbi) mInvp = Bodies[jpbi, BodyInvMass] iInvp = BodyInvInertia(jpbi) - velp = DynamicDelta(jpi, pars.Next) - omegap = DynamicAngDelta(jpi, pars.Next) + velp = DynamicDelta(jpi, params.Next) + omegap = DynamicAngDelta(jpi, params.Next) } // child transform and moment arm - posecP := DynamicPos(jci, pars.Next) - posecQ := DynamicRot(jci, pars.Next) + posecP := DynamicPos(jci, params.Next) + posecQ := DynamicRot(jci, params.Next) jcP := JointCPos(ji) jcQ := JointCRot(ji) xwcP := jcP @@ -134,8 +134,8 @@ func StepSolveJoints(i uint32) { //gosl:kernel comc := BodyCom(jcbi) mInvc := Bodies[jcbi, BodyInvMass] iInvc := BodyInvInertia(jcbi) - velc := DynamicDelta(jci, pars.Next) - omegac := DynamicAngDelta(jci, pars.Next) + velc := DynamicDelta(jci, params.Next) + omegac := DynamicAngDelta(jci, params.Next) if mInvp == 0.0 && mInvc == 0.0 { // connection between two immovable bodies return @@ -218,9 +218,9 @@ func StepSolveJoints(i uint32) { //gosl:kernel // dt, // ) // - // linDelta_p += linear_p * (d_lambda * pars.JointLinearRelax) + // linDelta_p += linear_p * (d_lambda * params.JointLinearRelax) // angDelta_p += angular_p * (d_lambda * angular_relaxation) - // linDelta_c += linear_c * (d_lambda * pars.JointLinearRelax) + // linDelta_c += linear_c * (d_lambda * params.JointLinearRelax) // angDelta_c += angular_c * (d_lambda * angular_relaxation) // } else { // compute joint target, stiffness, damping @@ -311,7 +311,7 @@ func StepSolveJoints(i uint32) { //gosl:kernel derr := slmath.Dot3(linearp, velp) + slmath.Dot3(linearc, velc) + slmath.Dot3(angularp, omegap) + slmath.Dot3(angularc, omegac) err := float32(0.0) - compliance := pars.JointLinearComply + compliance := params.JointLinearComply damping := float32(0.0) targetVel := axisTargetVelKdD.X // [dim] @@ -340,12 +340,12 @@ func StepSolveJoints(i uint32) { //gosl:kernel if math32.Abs(err) > 1e-9 || math32.Abs(derrRel) > 1e-9 { lambdaIn := float32(0.0) dLambda := PositionalCorrection(err, derrRel, posepQ, posecQ, mInvp, mInvc, - iInvp, iInvc, linearp, linearc, angularp, angularc, lambdaIn, compliance, damping, pars.Dt) + iInvp, iInvc, linearp, linearc, angularp, angularc, lambdaIn, compliance, damping, params.Dt) - linDeltaP = linDeltaP.Add(linearp.MulScalar(dLambda * pars.JointLinearRelax)) - angDeltaP = angDeltaP.Add(angularp.MulScalar(dLambda * pars.JointAngularRelax)) - linDeltaC = linDeltaC.Add(linearc.MulScalar(dLambda * pars.JointLinearRelax)) - angDeltaC = angDeltaC.Add(angularc.MulScalar(dLambda * pars.JointAngularRelax)) + linDeltaP = linDeltaP.Add(linearp.MulScalar(dLambda * params.JointLinearRelax)) + angDeltaP = angDeltaP.Add(angularp.MulScalar(dLambda * params.JointAngularRelax)) + linDeltaC = linDeltaC.Add(linearc.MulScalar(dLambda * params.JointLinearRelax)) + angDeltaC = angDeltaC.Add(angularc.MulScalar(dLambda * params.JointAngularRelax)) } } } diff --git a/physics/typegen.go b/physics/typegen.go index fc7e0c72..4022d21d 100644 --- a/physics/typegen.go +++ b/physics/typegen.go @@ -22,10 +22,10 @@ var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.JointVars", var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.JointTypes", IDName: "joint-types", Doc: "JointTypes are joint types that determine nature of interaction."}) -var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.PhysParams", IDName: "phys-params", Doc: "PhysParams are the physics parameters", Directives: []types.Directive{{Tool: "gosl", Directive: "start"}}, Fields: []types.Field{{Name: "DynamicsN", Doc: "DynamicsN is number of dynamics bodies."}, {Name: "JointsN", Doc: "JointsN is number of joints."}, {Name: "Cur", Doc: "Index for the current state (0 or 1, alternates with Next)."}, {Name: "Next", Doc: "Index for the next state (1 or 0, alternates with Cur)."}, {Name: "Iters", Doc: "Iters is the number of iterations to perform."}, {Name: "Dt", Doc: "Dt is the integration stepsize."}, {Name: "SoftRelax", Doc: "SoftRelax is soft-body relaxation constant."}, {Name: "JointLinearRelax", Doc: "JointLinearRelax is joint linear relaxation constant."}, {Name: "JointAngularRelax", Doc: "JointAngularRelax is joint angular relaxation constant."}, {Name: "JointLinearComply", Doc: "JointLinearComply is joint linear compliance constant."}, {Name: "JointAngularComply", Doc: "JointAngularComply is joint angular compliance constant."}, {Name: "ContactRelax", Doc: "ContactRelax is rigid contact relaxation constant."}, {Name: "AngularDamping", Doc: "AngularDamping is damping of angular motion."}, {Name: "ContactWeighting", Doc: "Contact weighting"}, {Name: "Restitution", Doc: "Restitution"}, {Name: "pad"}, {Name: "Gravity", Doc: "Gravity is the gravity acceleration function"}}}) +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.PhysParams", IDName: "phys-params", Doc: "PhysParams are the physics parameters", Directives: []types.Directive{{Tool: "gosl", Directive: "start"}}, Fields: []types.Field{{Name: "Iters", Doc: "Iters is the number of iterations to perform."}, {Name: "Dt", Doc: "Dt is the integration stepsize."}, {Name: "SoftRelax", Doc: "SoftRelax is soft-body relaxation constant."}, {Name: "JointLinearRelax", Doc: "JointLinearRelax is joint linear relaxation constant."}, {Name: "JointAngularRelax", Doc: "JointAngularRelax is joint angular relaxation constant."}, {Name: "JointLinearComply", Doc: "JointLinearComply is joint linear compliance constant."}, {Name: "JointAngularComply", Doc: "JointAngularComply is joint angular compliance constant."}, {Name: "ContactRelax", Doc: "ContactRelax is rigid contact relaxation constant."}, {Name: "AngularDamping", Doc: "AngularDamping is damping of angular motion."}, {Name: "ContactWeighting", Doc: "Contact weighting"}, {Name: "Restitution", Doc: "Restitution"}, {Name: "DynamicsN", Doc: "DynamicsN is number of dynamics bodies."}, {Name: "JointsN", Doc: "JointsN is number of joints."}, {Name: "BodyJointsMax", Doc: "BodyJointsMax is max number of joints per body + 1 for actual n."}, {Name: "Cur", Doc: "Index for the current state (0 or 1, alternates with Next)."}, {Name: "Next", Doc: "Index for the next state (1 or 0, alternates with Cur)."}, {Name: "Gravity", Doc: "Gravity is the gravity acceleration function"}}}) var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.Shapes", IDName: "shapes", Doc: "Shapes are elemental shapes for rigid bodies."}) var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.State", IDName: "state", Doc: "State contains the basic physical state including position, orientation, velocity.\nThese are only the values that can be either relative or absolute -- other physical\nstate values such as Mass should go in Rigid.", Methods: []types.Method{{Name: "MoveOnAxis", Doc: "MoveOnAxis moves (translates) the specified distance on the specified local axis,\nrelative to the current rotation orientation.\nThe axis is normalized prior to aplying the distance factor.\nSets the LinVel to motion vector.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z", "dist"}}, {Name: "MoveOnAxisAbs", Doc: "MoveOnAxisAbs moves (translates) the specified distance on the specified local axis,\nin absolute X,Y,Z coordinates (does not apply the Quat rotation factor.\nThe axis is normalized prior to aplying the distance factor.\nSets the LinVel to motion vector.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z", "dist"}}, {Name: "SetEulerRotation", Doc: "SetEulerRotation sets the rotation in Euler angles (degrees).", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z"}}, {Name: "EulerRotation", Doc: "EulerRotation returns the current rotation in Euler angles (degrees).", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Returns: []string{"Vector3"}}, {Name: "SetAxisRotation", Doc: "SetAxisRotation sets rotation from local axis and angle in degrees.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z", "angle"}}, {Name: "RotateOnAxis", Doc: "RotateOnAxis rotates around the specified local axis the specified angle in degrees.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z", "angle"}}, {Name: "RotateEuler", Doc: "RotateEuler rotates by given Euler angles (in degrees) relative to existing rotation.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z"}}}, Fields: []types.Field{{Name: "Pos", Doc: "Pos is the position of center of mass of object."}, {Name: "Quat", Doc: "Quat is the rotation specified as a quaternion."}, {Name: "LinVel", Doc: "LinVel is the linear velocity."}, {Name: "AngVel", Doc: "AngVel is the angular velocity."}}}) -var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.World", IDName: "world", Doc: "World contains and manages all of the physics elements.", Directives: []types.Directive{{Tool: "go", Directive: "generate", Args: []string{"core", "generate", "-add-types", "-gosl"}}}, Fields: []types.Field{{Name: "GPU", Doc: "GPU determines whether to use GPU (else CPU)."}, {Name: "Params", Doc: "Params are global parameters."}, {Name: "Bodies", Doc: "Bodies are the rigid body elements (dynamic and static),\nspecifying the constant, non-dynamic properties,\nwhich is initial state for dynamics.\n[body][BodyVarsN]"}, {Name: "Joints", Doc: "Joints is a list of permanent joints connecting bodies,\nwhich do not change (no dynamic variables).\n[joint][JointVarsN]"}, {Name: "BodyJoints", Doc: "BodyJoints is a list of joint indexes for each dynamic body, for aggregating.\n[dyn body][parent, child][maxjointsperbody]"}, {Name: "Dynamics", Doc: "Dynamics are the dynamic rigid body elements: these actually move.\nThe first set of variables are for initial values, and the second current.\n[body][cur/next][DynamicVarsN]"}, {Name: "Contacts", Doc: "Contacts are points of contact between bodies.\n[contact][ContactVarsN]"}, {Name: "JointControls", Doc: "JointControls are dynamic joint control inputs.\n[joint][JointControlVarsN]"}}}) +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.World", IDName: "world", Doc: "World contains and manages all of the physics elements.", Directives: []types.Directive{{Tool: "go", Directive: "generate", Args: []string{"core", "generate", "-add-types", "-gosl"}}}, Fields: []types.Field{{Name: "GPU", Doc: "GPU determines whether to use GPU (else CPU)."}, {Name: "Params", Doc: "Params are global parameters."}, {Name: "Bodies", Doc: "Bodies are the rigid body elements (dynamic and static),\nspecifying the constant, non-dynamic properties,\nwhich is initial state for dynamics.\n[body][BodyVarsN]"}, {Name: "Joints", Doc: "Joints is a list of permanent joints connecting bodies,\nwhich do not change (no dynamic variables).\n[joint][JointVarsN]"}, {Name: "BodyJoints", Doc: "BodyJoints is a list of joint indexes for each dynamic body, for aggregating.\n[dyn body][parent, child][Params.BodyJointsMax]"}, {Name: "Dynamics", Doc: "Dynamics are the dynamic rigid body elements: these actually move.\nThe first set of variables are for initial values, and the second current.\n[body][cur/next][DynamicVarsN]"}, {Name: "Contacts", Doc: "Contacts are points of contact between bodies.\n[contact][ContactVarsN]"}, {Name: "JointControls", Doc: "JointControls are dynamic joint control inputs.\n[joint][JointControlVarsN]"}}}) diff --git a/physics/world.go b/physics/world.go index 9c88c0ba..f6593d27 100644 --- a/physics/world.go +++ b/physics/world.go @@ -33,7 +33,7 @@ type World struct { Joints *tensor.Float32 // BodyJoints is a list of joint indexes for each dynamic body, for aggregating. - // [dyn body][parent, child][maxjointsperbody] + // [dyn body][parent, child][Params.BodyJointsMax] BodyJoints *tensor.Int32 // Dynamics are the dynamic rigid body elements: these actually move. diff --git a/physics/world.goal b/physics/world.goal index 8fe38c7d..8772774d 100644 --- a/physics/world.goal +++ b/physics/world.goal @@ -31,7 +31,7 @@ type World struct { Joints *tensor.Float32 // BodyJoints is a list of joint indexes for each dynamic body, for aggregating. - // [dyn body][parent, child][maxjointsperbody] + // [dyn body][parent, child][Params.BodyJointsMax] BodyJoints *tensor.Int32 // Dynamics are the dynamic rigid body elements: these actually move. From 7d18f9d2f191bd65e7eeb32b15a36d32f653f863 Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Tue, 16 Dec 2025 15:30:16 +0100 Subject: [PATCH 19/97] physics: test case working -- super basic functionality all seems to be working --- physics/body.go | 12 ++ physics/body.goal | 12 ++ physics/config.go | 27 +++- physics/config.goal | 28 +++- physics/control.go | 47 +++++-- physics/control.goal | 47 +++++-- physics/dynamics.go | 14 ++ physics/dynamics.goal | 14 ++ physics/enumgen.go | 2 +- physics/examples/test1/test1.go | 115 ++++++++++++++++ physics/examples/virtroom/virtroom.go | 10 +- physics/joint.go | 17 ++- physics/joint.goal | 17 ++- physics/params.go | 6 +- physics/shaders/DeltasFromJoints.wgsl | 2 +- physics/shaders/DynamicsCurToNext.wgsl | 2 +- physics/shaders/ForcesFromJoints.wgsl | 2 +- physics/shaders/InitDynamics.wgsl | 2 +- physics/shaders/StepBodyDeltas.wgsl | 2 +- physics/shaders/StepIntegrateBodies.wgsl | 2 +- physics/shaders/StepJointForces.wgsl | 7 +- physics/shaders/StepSolveJoints.wgsl | 7 +- physics/shapes.go | 167 ++++++++++++++++++++++- physics/step.go | 31 +++++ physics/step.goal | 31 +++++ physics/step_body.go | 1 + physics/step_body.goal | 1 + physics/step_joint.go | 18 ++- physics/step_joint.goal | 15 +- physics/typegen.go | 4 +- physics/world.go | 32 +++-- physics/world.goal | 32 +++-- physics/world/view.go | 9 +- 33 files changed, 620 insertions(+), 115 deletions(-) create mode 100644 physics/examples/test1/test1.go diff --git a/physics/body.go b/physics/body.go index b71b399d..44d003a2 100644 --- a/physics/body.go +++ b/physics/body.go @@ -146,4 +146,16 @@ func BodyInvInertia(idx int32) math32.Matrix3 { Bodies.Value(int(idx), int(BodyInvInertiaXZ)), Bodies.Value(int(idx), int(BodyInvInertiaYZ)), Bodies.Value(int(idx), int(BodyInvInertiaZZ))) } +func SetBodyInertia(idx int32, inertia math32.Matrix3) { + for i := range 9 { + Bodies.Set(inertia[i], int(idx), int(int(BodyInertiaXX)+i)) + } +} + +func SetBodyInvInertia(idx int32, invInertia math32.Matrix3) { + for i := range 9 { + Bodies.Set(invInertia[i], int(idx), int(int(BodyInvInertiaXX)+i)) + } +} + //gosl:end diff --git a/physics/body.goal b/physics/body.goal index 41318489..a99f5b5f 100644 --- a/physics/body.goal +++ b/physics/body.goal @@ -144,4 +144,16 @@ func BodyInvInertia(idx int32) math32.Matrix3 { Bodies[idx, BodyInvInertiaXZ], Bodies[idx, BodyInvInertiaYZ], Bodies[idx, BodyInvInertiaZZ]) } +func SetBodyInertia(idx int32, inertia math32.Matrix3) { + for i := range 9 { + Bodies[idx, int(BodyInertiaXX)+i] = inertia[i] + } +} + +func SetBodyInvInertia(idx int32, invInertia math32.Matrix3) { + for i := range 9 { + Bodies[idx, int(BodyInvInertiaXX)+i] = invInertia[i] + } +} + //gosl:end diff --git a/physics/config.go b/physics/config.go index 6dcd8406..f05a78cd 100644 --- a/physics/config.go +++ b/physics/config.go @@ -6,17 +6,24 @@ package physics +// import "fmt" + // Config does final configuration prior to running -// after everything has been added. +// after everything has been added. Does SetAsCurrent, GPUInit. func (wl *World) Config() { wl.ConfigJoints() + // todo: other things + wl.SetAsCurrent() + wl.GPUInit() + wl.InitState() } // ConfigJoints does all of the initialization associated with joints. func (wl *World) ConfigJoints() { // accumulate parent and child joints per dynamic - nj := wl.Params[0].JointsN - nd := wl.Params[0].DynamicsN + params := &wl.Params[0] + nj := params.JointsN + nd := params.DynamicsN bjp := make([][]int32, nd) bjc := make([][]int32, nd) @@ -25,13 +32,15 @@ func (wl *World) ConfigJoints() { for ji := range nj { jpi := JointParentIndex(ji) jci := JointChildIndex(ji) - bjp[jpi] = append(bjp[jpi], ji) + if jpi >= 0 { + bjp[jpi] = append(bjp[jpi], ji) + maxi = max(maxi, len(bjp[jpi])) + } bjc[jci] = append(bjc[jci], ji) - maxi = max(maxi, len(bjp[jpi])) maxi = max(maxi, len(bjc[jci])) } maxi = maxi + 1 // extra for n - wl.Params[0].BodyJointsMax = int32(maxi) + params.BodyJointsMax = int32(maxi) wl.BodyJoints.SetShapeSizes(int(nd), 2, maxi) for di := range nd { np := int32(len(bjp[di])) @@ -46,3 +55,9 @@ func (wl *World) ConfigJoints() { } } } + +// InitState initializes the simulation state. +func (wl *World) InitState() { + params := GetParams(0) + RunInitDynamics(int(params.DynamicsN)) +} diff --git a/physics/config.goal b/physics/config.goal index b5c60d56..c208e8c0 100644 --- a/physics/config.goal +++ b/physics/config.goal @@ -4,17 +4,24 @@ package physics +// import "fmt" + // Config does final configuration prior to running -// after everything has been added. +// after everything has been added. Does SetAsCurrent, GPUInit. func (wl *World) Config() { wl.ConfigJoints() + // todo: other things + wl.SetAsCurrent() + wl.GPUInit() + wl.InitState() } // ConfigJoints does all of the initialization associated with joints. func (wl *World) ConfigJoints() { // accumulate parent and child joints per dynamic - nj := wl.Params[0].JointsN - nd := wl.Params[0].DynamicsN + params := &wl.Params[0] + nj := params.JointsN + nd := params.DynamicsN bjp := make([][]int32, nd) bjc := make([][]int32, nd) @@ -23,13 +30,15 @@ func (wl *World) ConfigJoints() { for ji := range nj { jpi := JointParentIndex(ji) jci := JointChildIndex(ji) - bjp[jpi] = append(bjp[jpi], ji) + if jpi >= 0 { + bjp[jpi] = append(bjp[jpi], ji) + maxi = max(maxi, len(bjp[jpi])) + } bjc[jci] = append(bjc[jci], ji) - maxi = max(maxi, len(bjp[jpi])) maxi = max(maxi, len(bjc[jci])) } maxi = maxi+1 // extra for n - wl.Params[0].BodyJointsMax = int32(maxi) + params.BodyJointsMax = int32(maxi) wl.BodyJoints.SetShapeSizes(int(nd), 2, maxi) for di := range nd { np := int32(len(bjp[di])) @@ -44,3 +53,10 @@ func (wl *World) ConfigJoints() { } } } + +// InitState initializes the simulation state. +func (wl *World) InitState() { + params := GetParams(0) + RunInitDynamics(int(params.DynamicsN)) +} + diff --git a/physics/control.go b/physics/control.go index cfb24612..dc7eeac9 100644 --- a/physics/control.go +++ b/physics/control.go @@ -12,16 +12,19 @@ import "cogentcore.org/core/math32" // JointControlVars are external joint control input variables stored in tensor.Float32. // These must be in one-to-one correspondence with the joints. +// The X,Y,Z indexes are used progressively for the increasing degrees of freedom for +// the joints. type JointControlVars int32 //enums:enum const ( // Joint force and torque inputs - JointCtrlForceX JointControlVars = iota - JointCtrlForceY - JointCtrlForceZ - JointCtrlTorqueX - JointCtrlTorqueY - JointCtrlTorqueZ + JointControlForceX JointControlVars = iota + JointControlForceY + JointControlForceZ + + JointControlTorqueX + JointControlTorqueY + JointControlTorqueZ // target values (1 DoF use JointTargetPosX) JointTargetPosX @@ -45,23 +48,23 @@ const ( ) func JointControlForce(idx int32) math32.Vector3 { - return math32.Vec3(JointControls.Value(int(idx), int(JointCtrlForceX)), JointControls.Value(int(idx), int(JointCtrlForceY)), JointControls.Value(int(idx), int(JointCtrlForceZ))) + return math32.Vec3(JointControls.Value(int(idx), int(JointControlForceX)), JointControls.Value(int(idx), int(JointControlForceY)), JointControls.Value(int(idx), int(JointControlForceZ))) } func SetJointControlForce(idx int32, f math32.Vector3) { - JointControls.Set(f.X, int(idx), int(JointCtrlForceX)) - JointControls.Set(f.Y, int(idx), int(JointCtrlForceY)) - JointControls.Set(f.Z, int(idx), int(JointCtrlForceZ)) + JointControls.Set(f.X, int(idx), int(JointControlForceX)) + JointControls.Set(f.Y, int(idx), int(JointControlForceY)) + JointControls.Set(f.Z, int(idx), int(JointControlForceZ)) } func JointControlTorque(idx int32) math32.Vector3 { - return math32.Vec3(JointControls.Value(int(idx), int(JointCtrlTorqueX)), JointControls.Value(int(idx), int(JointCtrlTorqueY)), JointControls.Value(int(idx), int(JointCtrlTorqueZ))) + return math32.Vec3(JointControls.Value(int(idx), int(JointControlTorqueX)), JointControls.Value(int(idx), int(JointControlTorqueY)), JointControls.Value(int(idx), int(JointControlTorqueZ))) } func SetJointControlTorque(idx int32, f math32.Vector3) { - JointControls.Set(f.X, int(idx), int(JointCtrlTorqueX)) - JointControls.Set(f.Y, int(idx), int(JointCtrlTorqueY)) - JointControls.Set(f.Z, int(idx), int(JointCtrlTorqueZ)) + JointControls.Set(f.X, int(idx), int(JointControlTorqueX)) + JointControls.Set(f.Y, int(idx), int(JointControlTorqueY)) + JointControls.Set(f.Z, int(idx), int(JointControlTorqueZ)) } func JointTargetPos(idx int32) math32.Vector3 { @@ -105,3 +108,19 @@ func SetJointTargetAngVel(idx int32, f math32.Vector3) { } //gosl:end + +func (wl *World) SetJointTargetPos(idx int32, dim math32.Dims, value float32) { + JointControls.Set(value, int(idx), int(int(JointTargetPosX)+int(dim))) +} + +func (wl *World) SetJointTargetRot(idx int32, dim math32.Dims, value float32) { + JointControls.Set(value, int(idx), int(int(JointTargetRotX)+int(dim))) +} + +func (wl *World) SetJointControlForce(idx int32, dim math32.Dims, value float32) { + JointControls.Set(value, int(idx), int(int(JointControlForceX)+int(dim))) +} + +func (wl *World) SetJointControlTorque(idx int32, dim math32.Dims, value float32) { + JointControls.Set(value, int(idx), int(int(JointControlTorqueX)+int(dim))) +} diff --git a/physics/control.goal b/physics/control.goal index b29dec56..edc6d184 100644 --- a/physics/control.goal +++ b/physics/control.goal @@ -10,16 +10,19 @@ import "cogentcore.org/core/math32" // JointControlVars are external joint control input variables stored in tensor.Float32. // These must be in one-to-one correspondence with the joints. +// The X,Y,Z indexes are used progressively for the increasing degrees of freedom for +// the joints. type JointControlVars int32 //enums:enum const ( // Joint force and torque inputs - JointCtrlForceX JointControlVars = iota - JointCtrlForceY - JointCtrlForceZ - JointCtrlTorqueX - JointCtrlTorqueY - JointCtrlTorqueZ + JointControlForceX JointControlVars = iota + JointControlForceY + JointControlForceZ + + JointControlTorqueX + JointControlTorqueY + JointControlTorqueZ // target values (1 DoF use JointTargetPosX) JointTargetPosX @@ -43,23 +46,23 @@ const ( ) func JointControlForce(idx int32) math32.Vector3 { - return math32.Vec3(JointControls[idx, JointCtrlForceX], JointControls[idx, JointCtrlForceY], JointControls[idx, JointCtrlForceZ]) + return math32.Vec3(JointControls[idx, JointControlForceX], JointControls[idx, JointControlForceY], JointControls[idx, JointControlForceZ]) } func SetJointControlForce(idx int32, f math32.Vector3) { - JointControls[idx, JointCtrlForceX] = f.X - JointControls[idx, JointCtrlForceY] = f.Y - JointControls[idx, JointCtrlForceZ] = f.Z + JointControls[idx, JointControlForceX] = f.X + JointControls[idx, JointControlForceY] = f.Y + JointControls[idx, JointControlForceZ] = f.Z } func JointControlTorque(idx int32) math32.Vector3 { - return math32.Vec3(JointControls[idx, JointCtrlTorqueX], JointControls[idx, JointCtrlTorqueY], JointControls[idx, JointCtrlTorqueZ]) + return math32.Vec3(JointControls[idx, JointControlTorqueX], JointControls[idx, JointControlTorqueY], JointControls[idx, JointControlTorqueZ]) } func SetJointControlTorque(idx int32, f math32.Vector3) { - JointControls[idx, JointCtrlTorqueX] = f.X - JointControls[idx, JointCtrlTorqueY] = f.Y - JointControls[idx, JointCtrlTorqueZ] = f.Z + JointControls[idx, JointControlTorqueX] = f.X + JointControls[idx, JointControlTorqueY] = f.Y + JointControls[idx, JointControlTorqueZ] = f.Z } func JointTargetPos(idx int32) math32.Vector3 { @@ -103,3 +106,19 @@ func SetJointTargetAngVel(idx int32, f math32.Vector3) { } //gosl:end + +func (wl *World) SetJointTargetPos(idx int32, dim math32.Dims, value float32) { + JointControls[idx, int(JointTargetPosX)+int(dim)] = value +} + +func (wl *World) SetJointTargetRot(idx int32, dim math32.Dims, value float32) { + JointControls[idx, int(JointTargetRotX)+int(dim)] = value +} + +func (wl *World) SetJointControlForce(idx int32, dim math32.Dims, value float32) { + JointControls[idx, int(JointControlForceX)+int(dim)] = value +} + +func (wl *World) SetJointControlTorque(idx int32, dim math32.Dims, value float32) { + JointControls[idx, int(JointControlTorqueX)+int(dim)] = value +} diff --git a/physics/dynamics.go b/physics/dynamics.go index f45cb2fa..bced7d8d 100644 --- a/physics/dynamics.go +++ b/physics/dynamics.go @@ -183,3 +183,17 @@ func SetDynamicAngDelta(idx, cni int32, angDelta math32.Vector3) { } //gosl:end + +// SetMass sets the mass of given body object (only relevant for dynamics), +// including a default inertia tensor based on solid shape of given size. +func (wl *World) SetMass(idx int32, shape Shapes, size math32.Vector3, mass float32) { + Bodies.Set(mass, int(idx), int(BodyMass)) + invm := mass + if mass > 0 { + invm = 1.0 / mass + } + Bodies.Set(invm, int(idx), int(BodyInvMass)) + inertia := shape.Inertia(size, mass) + SetBodyInertia(idx, inertia) + SetBodyInvInertia(idx, inertia.Inverse()) +} diff --git a/physics/dynamics.goal b/physics/dynamics.goal index dc2a7e7d..f361a2dc 100644 --- a/physics/dynamics.goal +++ b/physics/dynamics.goal @@ -181,3 +181,17 @@ func SetDynamicAngDelta(idx, cni int32, angDelta math32.Vector3) { } //gosl:end + +// SetMass sets the mass of given body object (only relevant for dynamics), +// including a default inertia tensor based on solid shape of given size. +func (wl *World) SetMass(idx int32, shape Shapes, size math32.Vector3, mass float32) { + Bodies[idx, BodyMass] = mass + invm := mass + if mass > 0 { + invm = 1.0 / mass + } + Bodies[idx, BodyInvMass] = invm + inertia := shape.Inertia(size, mass) + SetBodyInertia(idx, inertia) + SetBodyInvInertia(idx, inertia.Inverse()) +} diff --git a/physics/enumgen.go b/physics/enumgen.go index 600b0471..ca61585d 100644 --- a/physics/enumgen.go +++ b/physics/enumgen.go @@ -248,7 +248,7 @@ const JointVarsN JointVars = 53 var _JointVarsValueMap = map[string]JointVars{`JointType`: 0, `JointEnabled`: 1, `JointParent`: 2, `JointChild`: 3, `JointPPosX`: 4, `JointPPosY`: 5, `JointPPosZ`: 6, `JointPRotX`: 7, `JointPRotY`: 8, `JointPRotZ`: 9, `JointPRotW`: 10, `JointCPosX`: 11, `JointCPosY`: 12, `JointCPosZ`: 13, `JointCRotX`: 14, `JointCRotY`: 15, `JointCRotZ`: 16, `JointCRotW`: 17, `JointAxisX`: 18, `JointAxisY`: 19, `JointAxisZ`: 20, `JointLimitLower`: 21, `JointLimitUpper`: 22, `JointStiffX`: 23, `JointStiffY`: 24, `JointStiffZ`: 25, `JointDampX`: 26, `JointDampY`: 27, `JointDampZ`: 28, `JointPForceX`: 29, `JointPForceY`: 30, `JointPForceZ`: 31, `JointPTorqueX`: 32, `JointPTorqueY`: 33, `JointPTorqueZ`: 34, `JointCForceX`: 35, `JointCForceY`: 36, `JointCForceZ`: 37, `JointCTorqueX`: 38, `JointCTorqueY`: 39, `JointCTorqueZ`: 40, `JointPDeltaX`: 41, `JointPDeltaY`: 42, `JointPDeltaZ`: 43, `JointPAngDeltaX`: 44, `JointPAngDeltaY`: 45, `JointPAngDeltaZ`: 46, `JointCDeltaX`: 47, `JointCDeltaY`: 48, `JointCDeltaZ`: 49, `JointCAngDeltaX`: 50, `JointCAngDeltaY`: 51, `JointCAngDeltaZ`: 52} -var _JointVarsDescMap = map[JointVars]string{0: `JointType (as an int32 from bits).`, 1: `JointEnabled allows joints to be dynamically enabled.`, 2: `JointParent is the dynamic body index for parent body. Can be -1 for a fixed parent for absolute anchor.`, 3: `JointChild is the dynamic body index for child body.`, 4: `position of joint, in parent frame.`, 5: ``, 6: ``, 7: `orientation of joint, in parent frame.`, 8: ``, 9: ``, 10: ``, 11: `position of joint, in child frame.`, 12: ``, 13: ``, 14: `orientation of joint, in child frame.`, 15: ``, 16: ``, 17: ``, 18: ``, 19: ``, 20: ``, 21: `joint limits`, 22: ``, 23: `joint stiffness target (ke)`, 24: ``, 25: ``, 26: `joint damping target (kd)`, 27: ``, 28: ``, 29: `Computed parent joint force value.`, 30: ``, 31: ``, 32: `Computed parent joint torque value.`, 33: ``, 34: ``, 35: `Computed child joint force value.`, 36: ``, 37: ``, 38: `Computed child joint torque value.`, 39: ``, 40: ``, 41: `Computed parent joint delta value.`, 42: ``, 43: ``, 44: `Computed parent joint angdelta value.`, 45: ``, 46: ``, 47: `Computed child joint delta value.`, 48: ``, 49: ``, 50: `Computed child joint angdelta value.`, 51: ``, 52: ``} +var _JointVarsDescMap = map[JointVars]string{0: `JointType (as an int32 from bits).`, 1: `JointEnabled allows joints to be dynamically enabled.`, 2: `JointParent is the dynamic body index for parent body. Can be -1 for a fixed parent for absolute anchor.`, 3: `JointChild is the dynamic body index for child body.`, 4: `relative position of joint, in parent frame.`, 5: ``, 6: ``, 7: `relative orientation of joint, in parent frame.`, 8: ``, 9: ``, 10: ``, 11: `relative position of joint, in child frame.`, 12: ``, 13: ``, 14: `relative orientation of joint, in child frame.`, 15: ``, 16: ``, 17: ``, 18: `axis of articulation for the joint`, 19: ``, 20: ``, 21: `joint limits`, 22: ``, 23: `joint stiffness target (ke)`, 24: ``, 25: ``, 26: `joint damping target (kd)`, 27: ``, 28: ``, 29: `Computed parent joint force value.`, 30: ``, 31: ``, 32: `Computed parent joint torque value.`, 33: ``, 34: ``, 35: `Computed child joint force value.`, 36: ``, 37: ``, 38: `Computed child joint torque value.`, 39: ``, 40: ``, 41: `Computed parent joint delta value.`, 42: ``, 43: ``, 44: `Computed parent joint angdelta value.`, 45: ``, 46: ``, 47: `Computed child joint delta value.`, 48: ``, 49: ``, 50: `Computed child joint angdelta value.`, 51: ``, 52: ``} var _JointVarsMap = map[JointVars]string{0: `JointType`, 1: `JointEnabled`, 2: `JointParent`, 3: `JointChild`, 4: `JointPPosX`, 5: `JointPPosY`, 6: `JointPPosZ`, 7: `JointPRotX`, 8: `JointPRotY`, 9: `JointPRotZ`, 10: `JointPRotW`, 11: `JointCPosX`, 12: `JointCPosY`, 13: `JointCPosZ`, 14: `JointCRotX`, 15: `JointCRotY`, 16: `JointCRotZ`, 17: `JointCRotW`, 18: `JointAxisX`, 19: `JointAxisY`, 20: `JointAxisZ`, 21: `JointLimitLower`, 22: `JointLimitUpper`, 23: `JointStiffX`, 24: `JointStiffY`, 25: `JointStiffZ`, 26: `JointDampX`, 27: `JointDampY`, 28: `JointDampZ`, 29: `JointPForceX`, 30: `JointPForceY`, 31: `JointPForceZ`, 32: `JointPTorqueX`, 33: `JointPTorqueY`, 34: `JointPTorqueZ`, 35: `JointCForceX`, 36: `JointCForceY`, 37: `JointCForceZ`, 38: `JointCTorqueX`, 39: `JointCTorqueY`, 40: `JointCTorqueZ`, 41: `JointPDeltaX`, 42: `JointPDeltaY`, 43: `JointPDeltaZ`, 44: `JointPAngDeltaX`, 45: `JointPAngDeltaY`, 46: `JointPAngDeltaZ`, 47: `JointCDeltaX`, 48: `JointCDeltaY`, 49: `JointCDeltaZ`, 50: `JointCAngDeltaX`, 51: `JointCAngDeltaY`, 52: `JointCAngDeltaZ`} diff --git a/physics/examples/test1/test1.go b/physics/examples/test1/test1.go new file mode 100644 index 00000000..136818e1 --- /dev/null +++ b/physics/examples/test1/test1.go @@ -0,0 +1,115 @@ +// Copyright (c) 2019, Cogent Core. All rights reserved. +// Use of this source code is governed by a BSD-style +// license that can be found in the LICENSE file. + +package main + +//go:generate core generate + +import ( + "cogentcore.org/core/colors" + "cogentcore.org/core/core" + "cogentcore.org/core/events" + "cogentcore.org/core/icons" + "cogentcore.org/core/math32" + "cogentcore.org/core/styles" + "cogentcore.org/core/styles/abilities" + "cogentcore.org/core/tree" + "cogentcore.org/core/xyz" + "cogentcore.org/core/xyz/xyzcore" + "cogentcore.org/lab/physics" + "cogentcore.org/lab/physics/world" +) + +func main() { + // gpu.Debug = true + b := core.NewBody("test1").SetTitle("Physics Test") + split := core.NewSplits(b) + // tv := core.NewTree(core.NewFrame(split)) + fv := core.NewForm(split) + tbvw := core.NewTabs(split) + scfr, _ := tbvw.NewTab("3D View") + + se := xyzcore.NewSceneEditor(scfr) + se.UpdateWidget() + sc := se.SceneXYZ() + + sc.Background = colors.Scheme.Select.Container + xyz.NewAmbient(sc, "ambient", 0.3, xyz.DirectSun) + + dir := xyz.NewDirectional(sc, "dir", 1, xyz.DirectSun) + dir.Pos.Set(0, 2, 1) + + wr := world.NewWorld(sc) + + wl := physics.NewWorld() + fv.SetStruct(wl) + + split.SetSplits(0.2, 0.8) + + rot := math32.NewQuat(0, 0, 0, 1) + thick := float32(0.1) + wr.NewBody(wl, "floor", physics.Box, "grey", math32.Vec3(10, thick, 10), + math32.Vec3(0, -thick/2, 0), rot) + + height := float32(1) + width := height * .4 + depth := height * .15 + b1 := wr.NewDynamic(wl, "body", physics.Box, "purple", 1.0, math32.Vec3(width, height, depth), + math32.Vec3(0, height/2, 0), rot) + bj := wl.NewJoint(physics.Revolute, -1, b1.DynamicIndex, math32.Vec3(-width, 0, 0), math32.Vec3(0, 0, 0), math32.Vec3(1, 0, 0)) + _ = bj + wl.SetJointControlForce(bj, math32.X, 5) + + wl.Config() + params := physics.GetParams(0) + params.Dt = 0.05 + params.Gravity.Y = 0 + + wr.Init(wl) + wr.Update() + + sc.Camera.Pose.Pos = math32.Vec3(0, 40, 3.5) + sc.Camera.LookAt(math32.Vec3(0, 5, 0), math32.Vec3(0, 1, 0)) + sc.SaveCamera("3") + + sc.Camera.Pose.Pos = math32.Vec3(0, 20, 30) + sc.Camera.LookAt(math32.Vec3(0, 5, 0), math32.Vec3(0, 1, 0)) + sc.SaveCamera("2") + + sc.Camera.Pose.Pos = math32.Vec3(-.86, .97, 2.7) + sc.Camera.LookAt(math32.Vec3(0, .8, 0), math32.Vec3(0, 1, 0)) + sc.SaveCamera("1") + sc.SaveCamera("default") + + b.AddTopBar(func(bar *core.Frame) { + core.NewToolbar(bar).Maker(func(p *tree.Plan) { + tree.Add(p, func(w *core.Button) { + w.SetText("Init").SetIcon(icons.Reset). + SetTooltip("Reset state"). + OnClick(func(e events.Event) { + wl.InitState() + wr.Update() + if se.IsVisible() { + se.NeedsRender() + } + }) + }) + tree.Add(p, func(w *core.Button) { + w.SetText("Step").SetIcon(icons.PlayArrow). + SetTooltip("Step state"). + OnClick(func(e events.Event) { + wl.Step() + wr.Update() + if se.IsVisible() { + se.NeedsRender() + } + }) + w.Styler(func(s *styles.Style) { + s.SetAbilities(true, abilities.RepeatClickable) + }) + }) + }) + }) + b.RunMainWindow() +} diff --git a/physics/examples/virtroom/virtroom.go b/physics/examples/virtroom/virtroom.go index c060313f..0006c729 100644 --- a/physics/examples/virtroom/virtroom.go +++ b/physics/examples/virtroom/virtroom.go @@ -269,13 +269,13 @@ func (ev *Env) MakeEmer(name string, height float32) { eyesz := headsz * .2 hhsz := .5 * headsz rot := math32.NewQuat(0, 0, 0, 1) - ev.Emer = wr.NewBody(wl, name+"_body", physics.Box, "purple", math32.Vec3(width, height, depth), + ev.Emer = wr.NewDynamic(wl, name+"_body", physics.Box, "purple", math32.Vec3(width, height, depth), math32.Vec3(0, height/2, 0), rot) // body := physics.NewCapsule(emr, "body", math32.Vec3(0, height / 2, 0), height, width/2) // body := physics.NewCylinder(emr, "body", math32.Vec3(0, height / 2, 0), height, width/2) headPos := math32.Vec3(0, height+hhsz, 0) - vw := wr.NewBody(wl, name+"_head", physics.Box, "tan", math32.Vec3(headsz, headsz, headsz), + vw := wr.NewDynamic(wl, name+"_head", physics.Box, "tan", math32.Vec3(headsz, headsz, headsz), headPos, rot) vw.InitView = func(sld *xyz.Solid) { vw.BoxInit(sld) @@ -287,11 +287,11 @@ func (ev *Env) MakeEmer(name string, height float32) { vw.UpdateColor(clr, sld) }) } - wl.NewJoint(physics.Glue, ev.Emer.Index, vw.Index, vw.Pos) - vw = wr.NewBody(wl, name+"_eye-l", physics.Box, "green", math32.Vec3(eyesz, eyesz*.5, eyesz*.2), + wl.NewJoint(physics.Fixed, ev.Emer.Index, vw.Index, vw.Pos) + vw = wr.NewDynamic(wl, name+"_eye-l", physics.Box, "green", math32.Vec3(eyesz, eyesz*.5, eyesz*.2), headPos.Add(math32.Vec3(-hhsz*.6, headsz*.1, -(hhsz+eyesz*.3))), rot) wl.NewJoint(physics.Glue, ev.Emer.Index, vw.Index, vw.Pos) - ev.EyeR = wr.NewBody(wl, name+"_eye-r", physics.Box, "green", math32.Vec3(eyesz, eyesz*.5, eyesz*.2), + ev.EyeR = wr.NewDynamic(wl, name+"_eye-r", physics.Box, "green", math32.Vec3(eyesz, eyesz*.5, eyesz*.2), headPos.Add(math32.Vec3(hhsz*.6, headsz*.1, -(hhsz+eyesz*.3))), rot) wl.NewJoint(physics.Glue, ev.Emer.Index, ev.EyeR.Index, ev.EyeR.Pos) } diff --git a/physics/joint.go b/physics/joint.go index f565f296..6c0306d6 100644 --- a/physics/joint.go +++ b/physics/joint.go @@ -33,28 +33,29 @@ const ( // JointChild is the dynamic body index for child body. JointChild - // position of joint, in parent frame. + // relative position of joint, in parent frame. JointPPosX JointPPosY JointPPosZ - // orientation of joint, in parent frame. + // relative orientation of joint, in parent frame. JointPRotX JointPRotY JointPRotZ JointPRotW - // position of joint, in child frame. + // relative position of joint, in child frame. JointCPosX JointCPosY JointCPosZ - // orientation of joint, in child frame. + // relative orientation of joint, in child frame. JointCRotX JointCRotY JointCRotZ JointCRotW + // axis of articulation for the joint JointAxisX JointAxisY JointAxisZ @@ -319,3 +320,11 @@ const ( ) //gosl:end + +func (wl *World) JointDefaults(idx int32) { + rot := math32.NewQuat(0, 0, 0, 1) + SetJointPRot(idx, rot) + SetJointCRot(idx, rot) + SetJointStiff(idx, math32.Vector3Scalar(1.0e4)) + SetJointDamp(idx, math32.Vector3Scalar(1.0)) +} diff --git a/physics/joint.goal b/physics/joint.goal index fe7165cd..b38bb34a 100644 --- a/physics/joint.goal +++ b/physics/joint.goal @@ -31,28 +31,29 @@ const ( // JointChild is the dynamic body index for child body. JointChild - // position of joint, in parent frame. + // relative position of joint, in parent frame. JointPPosX JointPPosY JointPPosZ - // orientation of joint, in parent frame. + // relative orientation of joint, in parent frame. JointPRotX JointPRotY JointPRotZ JointPRotW - // position of joint, in child frame. + // relative position of joint, in child frame. JointCPosX JointCPosY JointCPosZ - // orientation of joint, in child frame. + // relative orientation of joint, in child frame. JointCRotX JointCRotY JointCRotZ JointCRotW + // axis of articulation for the joint JointAxisX JointAxisY JointAxisZ @@ -317,3 +318,11 @@ const ( ) //gosl:end + +func (wl *World) JointDefaults(idx int32) { + rot := math32.NewQuat(0, 0, 0, 1) + SetJointPRot(idx, rot) + SetJointCRot(idx, rot) + SetJointStiff(idx, math32.Vector3Scalar(1.0e4)) + SetJointDamp(idx, math32.Vector3Scalar(1.0)) +} diff --git a/physics/params.go b/physics/params.go index 65169b3d..e0f18528 100644 --- a/physics/params.go +++ b/physics/params.go @@ -13,8 +13,8 @@ import ( // PhysParams are the physics parameters type PhysParams struct { - // Iters is the number of iterations to perform. - Iters int32 `default:"2"` + // Iterations is the number of iterations to perform. + Iterations int32 `default:"2"` // Dt is the integration stepsize. Dt float32 `default:"0.01"` @@ -66,7 +66,7 @@ type PhysParams struct { } func (pr *PhysParams) Defaults() { - pr.Iters = 2 + pr.Iterations = 2 pr.Dt = 0.01 pr.Gravity.Set(0, -9.81, 0) pr.SoftRelax = 0.9 diff --git a/physics/shaders/DeltasFromJoints.wgsl b/physics/shaders/DeltasFromJoints.wgsl index 517b6ea1..88c0662b 100644 --- a/physics/shaders/DeltasFromJoints.wgsl +++ b/physics/shaders/DeltasFromJoints.wgsl @@ -242,7 +242,7 @@ const D6: JointTypes = 6; //////// import: "params.go" struct PhysParams { - Iters: i32, + Iterations: i32, Dt: f32, SoftRelax: f32, JointLinearRelax: f32, diff --git a/physics/shaders/DynamicsCurToNext.wgsl b/physics/shaders/DynamicsCurToNext.wgsl index 81f131a5..5a76488e 100644 --- a/physics/shaders/DynamicsCurToNext.wgsl +++ b/physics/shaders/DynamicsCurToNext.wgsl @@ -212,7 +212,7 @@ const D6: JointTypes = 6; //////// import: "params.go" struct PhysParams { - Iters: i32, + Iterations: i32, Dt: f32, SoftRelax: f32, JointLinearRelax: f32, diff --git a/physics/shaders/ForcesFromJoints.wgsl b/physics/shaders/ForcesFromJoints.wgsl index 0bb58bca..da885296 100644 --- a/physics/shaders/ForcesFromJoints.wgsl +++ b/physics/shaders/ForcesFromJoints.wgsl @@ -242,7 +242,7 @@ const D6: JointTypes = 6; //////// import: "params.go" struct PhysParams { - Iters: i32, + Iterations: i32, Dt: f32, SoftRelax: f32, JointLinearRelax: f32, diff --git a/physics/shaders/InitDynamics.wgsl b/physics/shaders/InitDynamics.wgsl index 31834a3f..5973078d 100644 --- a/physics/shaders/InitDynamics.wgsl +++ b/physics/shaders/InitDynamics.wgsl @@ -221,7 +221,7 @@ const D6: JointTypes = 6; //////// import: "params.go" struct PhysParams { - Iters: i32, + Iterations: i32, Dt: f32, SoftRelax: f32, JointLinearRelax: f32, diff --git a/physics/shaders/StepBodyDeltas.wgsl b/physics/shaders/StepBodyDeltas.wgsl index ea04a75b..083112d7 100644 --- a/physics/shaders/StepBodyDeltas.wgsl +++ b/physics/shaders/StepBodyDeltas.wgsl @@ -267,7 +267,7 @@ const D6: JointTypes = 6; //////// import: "params.go" struct PhysParams { - Iters: i32, + Iterations: i32, Dt: f32, SoftRelax: f32, JointLinearRelax: f32, diff --git a/physics/shaders/StepIntegrateBodies.wgsl b/physics/shaders/StepIntegrateBodies.wgsl index 2b229a11..cec97e27 100644 --- a/physics/shaders/StepIntegrateBodies.wgsl +++ b/physics/shaders/StepIntegrateBodies.wgsl @@ -273,7 +273,7 @@ const D6: JointTypes = 6; //////// import: "params.go" struct PhysParams { - Iters: i32, + Iterations: i32, Dt: f32, SoftRelax: f32, JointLinearRelax: f32, diff --git a/physics/shaders/StepJointForces.wgsl b/physics/shaders/StepJointForces.wgsl index 1eafb8a8..3af34451 100644 --- a/physics/shaders/StepJointForces.wgsl +++ b/physics/shaders/StepJointForces.wgsl @@ -278,7 +278,7 @@ const D6: JointTypes = 6; //////// import: "params.go" struct PhysParams { - Iters: i32, + Iterations: i32, Dt: f32, SoftRelax: f32, JointLinearRelax: f32, @@ -346,7 +346,10 @@ fn StepJointForces(i: u32) { //gosl:kernel return; } var jpi = JointParentIndex(ji); - var jpbi = DynamicIndex(jpi, params.Cur); + var jpbi = i32(-1); + if (jpi >= 0) { + jpbi = DynamicIndex(jpi, params.Cur); + } var jci = JointChildIndex(ji); var jcbi = DynamicIndex(jci, params.Cur); var jt = GetJointType(ji); diff --git a/physics/shaders/StepSolveJoints.wgsl b/physics/shaders/StepSolveJoints.wgsl index 6c91e937..d43722ab 100644 --- a/physics/shaders/StepSolveJoints.wgsl +++ b/physics/shaders/StepSolveJoints.wgsl @@ -301,7 +301,7 @@ const D6: JointTypes = 6; //////// import: "params.go" struct PhysParams { - Iters: i32, + Iterations: i32, Dt: f32, SoftRelax: f32, JointLinearRelax: f32, @@ -421,7 +421,10 @@ fn StepSolveJoints(i: u32) { //gosl:kernel return; } var jpi = JointParentIndex(ji); - var jpbi = DynamicIndex(jpi, params.Cur); + var jpbi = i32(-1); + if (jpi >= 0) { + jpbi = DynamicIndex(jpi, params.Cur); + } var jci = JointChildIndex(ji); var jcbi = DynamicIndex(jci, params.Cur); var jt = GetJointType(ji); diff --git a/physics/shapes.go b/physics/shapes.go index 5cce8684..013ed713 100644 --- a/physics/shapes.go +++ b/physics/shapes.go @@ -6,6 +6,8 @@ package physics import "cogentcore.org/core/math32" +// see: newton/geometry for lots of helpful methods. + //gosl:start // Shapes are elemental shapes for rigid bodies. @@ -31,7 +33,8 @@ const ( //gosl:end -func (sh Shapes) ShapeBBox(sz math32.Vector3) math32.Box3 { +// BBox returns the bounding box for shape of given size. +func (sh Shapes) BBox(sz math32.Vector3) math32.Box3 { var bb math32.Box3 switch sh { @@ -51,3 +54,165 @@ func (sh Shapes) ShapeBBox(sz math32.Vector3) math32.Box3 { // bb.Volume = sz.X * sz.Y * sz.Z return bb } + +// Inertia returns the inertia tensor for solid shape of given size, +// with uniform density and given mass. +func (sh Shapes) Inertia(sz math32.Vector3, mass float32) math32.Matrix3 { + var inertia math32.Matrix3 + switch sh { + case Sphere: + r := sz.X + // v := 4.0 / 3.0 * math32.Pi * r * r * r + ia := 2.0 / 5.0 * mass * r * r + inertia = math32.Mat3(ia, 0.0, 0.0, 0.0, ia, 0.0, 0.0, 0.0, ia) + case Box: + w := sz.X + h := sz.Y + d := sz.Z + ia := 1.0 / 12.0 * mass * (h*h + d*d) + ib := 1.0 / 12.0 * mass * (w*w + d*d) + ic := 1.0 / 12.0 * mass * (w*w + h*h) + inertia = math32.Mat3(ia, 0.0, 0.0, 0.0, ib, 0.0, 0.0, 0.0, ic) + } + return inertia +} + +/* +def compute_capsule_inertia(density: float, r: float, h: float) -> tuple[float, wp.vec3, wp.mat33]: + """Helper to compute mass and inertia of a solid capsule extending along the z-axis + + Args: + density: The capsule density + r: The capsule radius + h: The capsule height (full height of the interior cylinder) + + Returns: + + A tuple of (mass, inertia) with inertia specified around the origin + """ + + ms = density * (4.0 / 3.0) * wp.pi * r * r * r + mc = density * wp.pi * r * r * h + + # total mass + m = ms + mc + + # adapted from ODE + Ia = mc * (0.25 * r * r + (1.0 / 12.0) * h * h) + ms * (0.4 * r * r + 0.375 * r * h + 0.25 * h * h) + Ib = (mc * 0.5 + ms * 0.4) * r * r + + # For Z-axis orientation: I_xx = I_yy = Ia, I_zz = Ib + I = wp.mat33([[Ia, 0.0, 0.0], [0.0, Ia, 0.0], [0.0, 0.0, Ib]]) + + return (m, wp.vec3(), I) + + +def compute_cylinder_inertia(density: float, r: float, h: float) -> tuple[float, wp.vec3, wp.mat33]: + """Helper to compute mass and inertia of a solid cylinder extending along the z-axis + + Args: + density: The cylinder density + r: The cylinder radius + h: The cylinder height (extent along the z-axis) + + Returns: + + A tuple of (mass, inertia) with inertia specified around the origin + """ + + m = density * wp.pi * r * r * h + + Ia = 1 / 12 * m * (3 * r * r + h * h) + Ib = 1 / 2 * m * r * r + + # For Z-axis orientation: I_xx = I_yy = Ia, I_zz = Ib + I = wp.mat33([[Ia, 0.0, 0.0], [0.0, Ia, 0.0], [0.0, 0.0, Ib]]) + + return (m, wp.vec3(), I) + + +def compute_cone_inertia(density: float, r: float, h: float) -> tuple[float, wp.vec3, wp.mat33]: + """Helper to compute mass and inertia of a solid cone extending along the z-axis + + Args: + density: The cone density + r: The cone radius + h: The cone height (extent along the z-axis) + + Returns: + + A tuple of (mass, center of mass, inertia) with inertia specified around the center of mass + """ + + m = density * wp.pi * r * r * h / 3.0 + + # Center of mass is at -h/4 from the geometric center + # Since the cone has base at -h/2 and apex at +h/2, the COM is 1/4 of the height from base toward apex + com = wp.vec3(0.0, 0.0, -h / 4.0) + + # Inertia about the center of mass + Ia = 3 / 20 * m * r * r + 3 / 80 * m * h * h + Ib = 3 / 10 * m * r * r + + # For Z-axis orientation: I_xx = I_yy = Ia, I_zz = Ib + I = wp.mat33([[Ia, 0.0, 0.0], [0.0, Ia, 0.0], [0.0, 0.0, Ib]]) + + return (m, com, I) + + +def compute_ellipsoid_inertia(density: float, a: float, b: float, c: float) -> tuple[float, wp.vec3, wp.mat33]: + """Helper to compute mass and inertia of a solid ellipsoid + + The ellipsoid is centered at the origin with semi-axes a, b, c along the x, y, z axes respectively. + + Args: + density: The ellipsoid density + a: The semi-axis along the x-axis + b: The semi-axis along the y-axis + c: The semi-axis along the z-axis + + Returns: + + A tuple of (mass, center of mass, inertia) with inertia specified around the center of mass + """ + # Volume of ellipsoid: V = (4/3) * pi * a * b * c + v = (4.0 / 3.0) * wp.pi * a * b * c + m = density * v + + # Inertia tensor for a solid ellipsoid about its center of mass: + # Ixx = (1/5) * m * (b² + c²) + # Iyy = (1/5) * m * (a² + c²) + # Izz = (1/5) * m * (a² + b²) + Ixx = (1.0 / 5.0) * m * (b * b + c * c) + Iyy = (1.0 / 5.0) * m * (a * a + c * c) + Izz = (1.0 / 5.0) * m * (a * a + b * b) + + I = wp.mat33([[Ixx, 0.0, 0.0], [0.0, Iyy, 0.0], [0.0, 0.0, Izz]]) + + return (m, wp.vec3(), I) + +*/ + +/* +def compute_box_inertia(density: float, w: float, h: float, d: float) -> tuple[float, wp.vec3, wp.mat33]: + """Helper to compute mass and inertia of a solid box + + Args: + density: The box density + w: The box width along the x-axis + h: The box height along the y-axis + d: The box depth along the z-axis + + Returns: + + A tuple of (mass, inertia) with inertia specified around the origin + """ + + v = w * h * d + m = density * v + I = compute_box_inertia_from_mass(m, w, h, d) + + return (m, wp.vec3(), I) + +} +*/ diff --git a/physics/step.go b/physics/step.go index 869c99d2..e7a68e01 100644 --- a/physics/step.go +++ b/physics/step.go @@ -47,7 +47,38 @@ func OneIfNonzero(f float32) float32 { //gosl:end +func (wl *World) Step() { + params := GetParams(0) + if params.Cur == 0 { + params.Cur = 1 + params.Next = 0 + } else { + params.Cur = 0 + params.Next = 1 + } + ToGPU(JointControlsVar) + wl.StepJointForces() + wl.StepIntegrateBodies() + + for range params.Iterations { + wl.StepSolveJoints() + } + RunDone(DynamicsVar) +} + func (wl *World) StepJointForces() { params := GetParams(0) RunStepJointForces(int(params.JointsN)) + RunForcesFromJoints(int(params.DynamicsN)) +} + +func (wl *World) StepIntegrateBodies() { + params := GetParams(0) + RunStepIntegrateBodies(int(params.JointsN)) +} + +func (wl *World) StepSolveJoints() { + params := GetParams(0) + RunStepSolveJoints(int(params.JointsN)) + RunDeltasFromJoints(int(params.DynamicsN)) } diff --git a/physics/step.goal b/physics/step.goal index a52c600c..f2ef8de6 100644 --- a/physics/step.goal +++ b/physics/step.goal @@ -45,7 +45,38 @@ func OneIfNonzero(f float32) float32 { //gosl:end +func (wl *World) Step() { + params := GetParams(0) + if params.Cur == 0 { + params.Cur = 1 + params.Next = 0 + } else { + params.Cur = 0 + params.Next = 1 + } + ToGPU(JointControlsVar) + wl.StepJointForces() + wl.StepIntegrateBodies() + + for range params.Iterations { + wl.StepSolveJoints() + } + RunDone(DynamicsVar) +} + func (wl *World) StepJointForces() { params := GetParams(0) RunStepJointForces(int(params.JointsN)) + RunForcesFromJoints(int(params.DynamicsN)) +} + +func (wl *World) StepIntegrateBodies() { + params := GetParams(0) + RunStepIntegrateBodies(int(params.JointsN)) +} + +func (wl *World) StepSolveJoints() { + params := GetParams(0) + RunStepSolveJoints(int(params.JointsN)) + RunDeltasFromJoints(int(params.DynamicsN)) } diff --git a/physics/step_body.go b/physics/step_body.go index 04813d3d..22234fae 100644 --- a/physics/step_body.go +++ b/physics/step_body.go @@ -10,6 +10,7 @@ package physics import ( + // "fmt" "cogentcore.org/core/math32" "cogentcore.org/lab/gosl/slmath" ) diff --git a/physics/step_body.goal b/physics/step_body.goal index 9a2646eb..57753979 100644 --- a/physics/step_body.goal +++ b/physics/step_body.goal @@ -8,6 +8,7 @@ package physics import ( + // "fmt" "cogentcore.org/core/math32" "cogentcore.org/lab/gosl/slmath" ) diff --git a/physics/step_joint.go b/physics/step_joint.go index 92b3b225..8834da14 100644 --- a/physics/step_joint.go +++ b/physics/step_joint.go @@ -26,7 +26,10 @@ func StepJointForces(i uint32) { //gosl:kernel } // todo: enabled jpi := JointParentIndex(ji) - jpbi := DynamicIndex(jpi, params.Cur) + jpbi := int32(-1) + if jpi >= 0 { + jpbi = DynamicIndex(jpi, params.Cur) + } jci := JointChildIndex(ji) jcbi := DynamicIndex(jci, params.Cur) jt := GetJointType(ji) @@ -93,7 +96,10 @@ func StepSolveJoints(i uint32) { //gosl:kernel // todo: enabled jpi := JointParentIndex(ji) - jpbi := DynamicIndex(jpi, params.Cur) + jpbi := int32(-1) + if jpi >= 0 { + jpbi = DynamicIndex(jpi, params.Cur) + } jci := JointChildIndex(ji) jcbi := DynamicIndex(jci, params.Cur) jt := GetJointType(ji) @@ -553,14 +559,6 @@ func StepSolveJoints(i uint32) { //gosl:kernel SetJointPAngDelta(ji, angDeltaP) SetJointCDelta(ji, linDeltaC) SetJointCAngDelta(ji, angDeltaC) - - // if id_p >= 0: - // - // wp.atomic_add(deltas, id_p, wp.spatial_vector(linDelta_p, angDelta_p)) - // - // if id_c >= 0: - // - // wp.atomic_add(deltas, id_c, wp.spatial_vector(linDelta_c, angDelta_c)) } func UpdateJointAxisWeightedTarget(axis math32.Vector3, targ, weight float32, axisTargets, axisWeights *math32.Vector3) { diff --git a/physics/step_joint.goal b/physics/step_joint.goal index 6219c800..14e76841 100644 --- a/physics/step_joint.goal +++ b/physics/step_joint.goal @@ -24,7 +24,10 @@ func StepJointForces(i uint32) { //gosl:kernel } // todo: enabled jpi := JointParentIndex(ji) - jpbi := DynamicIndex(jpi, params.Cur) + jpbi := int32(-1) + if jpi >= 0 { + jpbi = DynamicIndex(jpi, params.Cur) + } jci := JointChildIndex(ji) jcbi := DynamicIndex(jci, params.Cur) jt := GetJointType(ji) @@ -91,7 +94,10 @@ func StepSolveJoints(i uint32) { //gosl:kernel // todo: enabled jpi := JointParentIndex(ji) - jpbi := DynamicIndex(jpi, params.Cur) + jpbi := int32(-1) + if jpi >= 0 { + jpbi = DynamicIndex(jpi, params.Cur) + } jci := JointChildIndex(ji) jcbi := DynamicIndex(jci, params.Cur) jt := GetJointType(ji) @@ -542,11 +548,6 @@ func StepSolveJoints(i uint32) { //gosl:kernel SetJointPAngDelta(ji, angDeltaP) SetJointCDelta(ji, linDeltaC) SetJointCAngDelta(ji, angDeltaC) - - // if id_p >= 0: - // wp.atomic_add(deltas, id_p, wp.spatial_vector(linDelta_p, angDelta_p)) - // if id_c >= 0: - // wp.atomic_add(deltas, id_c, wp.spatial_vector(linDelta_c, angDelta_c)) } func UpdateJointAxisWeightedTarget(axis math32.Vector3, targ, weight float32, axisTargets, axisWeights *math32.Vector3) { diff --git a/physics/typegen.go b/physics/typegen.go index 4022d21d..1a8e6966 100644 --- a/physics/typegen.go +++ b/physics/typegen.go @@ -12,7 +12,7 @@ var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.BodyVars", I var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.ContactVars", IDName: "contact-vars", Doc: "Contact is one pairwise point of contact between two bodies.\nContacts are represented in spherical terms relative to the\nspherical BBox of A and B."}) -var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.JointControlVars", IDName: "joint-control-vars", Doc: "JointControlVars are external joint control input variables stored in tensor.Float32.\nThese must be in one-to-one correspondence with the joints."}) +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.JointControlVars", IDName: "joint-control-vars", Doc: "JointControlVars are external joint control input variables stored in tensor.Float32.\nThese must be in one-to-one correspondence with the joints.\nThe X,Y,Z indexes are used progressively for the increasing degrees of freedom for\nthe joints."}) var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.DynamicVars", IDName: "dynamic-vars", Doc: "DynamicVars are dynamic body variables stored in tensor.Float32."}) @@ -22,7 +22,7 @@ var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.JointVars", var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.JointTypes", IDName: "joint-types", Doc: "JointTypes are joint types that determine nature of interaction."}) -var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.PhysParams", IDName: "phys-params", Doc: "PhysParams are the physics parameters", Directives: []types.Directive{{Tool: "gosl", Directive: "start"}}, Fields: []types.Field{{Name: "Iters", Doc: "Iters is the number of iterations to perform."}, {Name: "Dt", Doc: "Dt is the integration stepsize."}, {Name: "SoftRelax", Doc: "SoftRelax is soft-body relaxation constant."}, {Name: "JointLinearRelax", Doc: "JointLinearRelax is joint linear relaxation constant."}, {Name: "JointAngularRelax", Doc: "JointAngularRelax is joint angular relaxation constant."}, {Name: "JointLinearComply", Doc: "JointLinearComply is joint linear compliance constant."}, {Name: "JointAngularComply", Doc: "JointAngularComply is joint angular compliance constant."}, {Name: "ContactRelax", Doc: "ContactRelax is rigid contact relaxation constant."}, {Name: "AngularDamping", Doc: "AngularDamping is damping of angular motion."}, {Name: "ContactWeighting", Doc: "Contact weighting"}, {Name: "Restitution", Doc: "Restitution"}, {Name: "DynamicsN", Doc: "DynamicsN is number of dynamics bodies."}, {Name: "JointsN", Doc: "JointsN is number of joints."}, {Name: "BodyJointsMax", Doc: "BodyJointsMax is max number of joints per body + 1 for actual n."}, {Name: "Cur", Doc: "Index for the current state (0 or 1, alternates with Next)."}, {Name: "Next", Doc: "Index for the next state (1 or 0, alternates with Cur)."}, {Name: "Gravity", Doc: "Gravity is the gravity acceleration function"}}}) +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.PhysParams", IDName: "phys-params", Doc: "PhysParams are the physics parameters", Directives: []types.Directive{{Tool: "gosl", Directive: "start"}}, Fields: []types.Field{{Name: "Iterations", Doc: "Iterations is the number of iterations to perform."}, {Name: "Dt", Doc: "Dt is the integration stepsize."}, {Name: "SoftRelax", Doc: "SoftRelax is soft-body relaxation constant."}, {Name: "JointLinearRelax", Doc: "JointLinearRelax is joint linear relaxation constant."}, {Name: "JointAngularRelax", Doc: "JointAngularRelax is joint angular relaxation constant."}, {Name: "JointLinearComply", Doc: "JointLinearComply is joint linear compliance constant."}, {Name: "JointAngularComply", Doc: "JointAngularComply is joint angular compliance constant."}, {Name: "ContactRelax", Doc: "ContactRelax is rigid contact relaxation constant."}, {Name: "AngularDamping", Doc: "AngularDamping is damping of angular motion."}, {Name: "ContactWeighting", Doc: "Contact weighting"}, {Name: "Restitution", Doc: "Restitution"}, {Name: "DynamicsN", Doc: "DynamicsN is number of dynamics bodies."}, {Name: "JointsN", Doc: "JointsN is number of joints."}, {Name: "BodyJointsMax", Doc: "BodyJointsMax is max number of joints per body + 1 for actual n."}, {Name: "Cur", Doc: "Index for the current state (0 or 1, alternates with Next)."}, {Name: "Next", Doc: "Index for the next state (1 or 0, alternates with Cur)."}, {Name: "Gravity", Doc: "Gravity is the gravity acceleration function"}}}) var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.Shapes", IDName: "shapes", Doc: "Shapes are elemental shapes for rigid bodies."}) diff --git a/physics/world.go b/physics/world.go index f6593d27..8048daa7 100644 --- a/physics/world.go +++ b/physics/world.go @@ -25,29 +25,29 @@ type World struct { // specifying the constant, non-dynamic properties, // which is initial state for dynamics. // [body][BodyVarsN] - Bodies *tensor.Float32 + Bodies *tensor.Float32 `display:"no-inline"` // Joints is a list of permanent joints connecting bodies, // which do not change (no dynamic variables). // [joint][JointVarsN] - Joints *tensor.Float32 + Joints *tensor.Float32 `display:"no-inline"` // BodyJoints is a list of joint indexes for each dynamic body, for aggregating. // [dyn body][parent, child][Params.BodyJointsMax] - BodyJoints *tensor.Int32 + BodyJoints *tensor.Int32 `display:"no-inline"` // Dynamics are the dynamic rigid body elements: these actually move. // The first set of variables are for initial values, and the second current. // [body][cur/next][DynamicVarsN] - Dynamics *tensor.Float32 + Dynamics *tensor.Float32 `display:"no-inline"` // Contacts are points of contact between bodies. // [contact][ContactVarsN] - Contacts *tensor.Float32 + Contacts *tensor.Float32 `display:"no-inline"` // JointControls are dynamic joint control inputs. // [joint][JointControlVarsN] - JointControls *tensor.Float32 + JointControls *tensor.Float32 `display:"no-inline"` } func NewWorld() *World { @@ -58,7 +58,8 @@ func NewWorld() *World { // Init makes initial vars. func (wl *World) Init() { - wl.Params = []PhysParams{} + wl.Params = make([]PhysParams, 1) + wl.Params[0].Defaults() wl.Bodies = tensor.NewFloat32(0, int(BodyVarsN)) wl.Joints = tensor.NewFloat32(0, int(JointVarsN)) wl.BodyJoints = tensor.NewInt32(0, 2, 2) @@ -82,31 +83,38 @@ func (wl *World) NewBody(shape Shapes, size, pos math32.Vector3, rot math32.Quat } // NewDynamic adds a new dynamic body with given parameters. Returns the index. -func (wl *World) NewDynamic(shape Shapes, size, pos math32.Vector3, rot math32.Quat) (bodyIdx, dynIdx int32) { +func (wl *World) NewDynamic(shape Shapes, mass float32, size, pos math32.Vector3, rot math32.Quat) (bodyIdx, dynIdx int32) { bodyIdx = wl.NewBody(shape, size, pos, rot) sizes := wl.Dynamics.ShapeSizes() dynIdx = int32(sizes[0]) wl.Dynamics.SetShapeSizes(int(dynIdx+1), 2, int(DynamicVarsN)) SetDynamicIndex(dynIdx, 0, bodyIdx) SetDynamicIndex(dynIdx, 1, bodyIdx) + wl.SetMass(bodyIdx, shape, size, mass) wl.Params[0].DynamicsN = dynIdx + 1 return } // NewJoint adds a new joint between parent and child dynamic object indexes. -func (wl *World) NewJoint(joint JointTypes, parent, child int32, pos math32.Vector3) int32 { +// Use -1 for parent to add a world-anchored joint. +// ppos, cpos are the relative positions from the parent, child. +// Sets relative rotation matricies to identity by default. +// axis is the axis of articulation for the joint. +func (wl *World) NewJoint(joint JointTypes, parent, child int32, ppos, cpos, axis math32.Vector3) int32 { sizes := wl.Joints.ShapeSizes() idx := int32(sizes[0]) wl.Joints.SetShapeSizes(int(idx+1), int(JointVarsN)) + wl.JointControls.SetShapeSizes(int(idx+1), int(JointControlVarsN)) SetJointParent(idx, parent) SetJointChild(idx, child) - SetJointPPos(idx, pos) + SetJointPPos(idx, ppos) + SetJointCPos(idx, cpos) + SetJointAxis(idx, axis) + wl.JointDefaults(idx) wl.Params[0].JointsN = idx + 1 return idx } -// todo: init bodyjoints - // SetAsCurrent sets these as the current global values that are // processed in the code (on the GPU). If this was not the setter of // the current variables, then the parameter variables are copied up diff --git a/physics/world.goal b/physics/world.goal index 8772774d..771f1764 100644 --- a/physics/world.goal +++ b/physics/world.goal @@ -23,29 +23,29 @@ type World struct { // specifying the constant, non-dynamic properties, // which is initial state for dynamics. // [body][BodyVarsN] - Bodies *tensor.Float32 + Bodies *tensor.Float32 `display:"no-inline"` // Joints is a list of permanent joints connecting bodies, // which do not change (no dynamic variables). // [joint][JointVarsN] - Joints *tensor.Float32 + Joints *tensor.Float32 `display:"no-inline"` // BodyJoints is a list of joint indexes for each dynamic body, for aggregating. // [dyn body][parent, child][Params.BodyJointsMax] - BodyJoints *tensor.Int32 + BodyJoints *tensor.Int32 `display:"no-inline"` // Dynamics are the dynamic rigid body elements: these actually move. // The first set of variables are for initial values, and the second current. // [body][cur/next][DynamicVarsN] - Dynamics *tensor.Float32 + Dynamics *tensor.Float32 `display:"no-inline"` // Contacts are points of contact between bodies. // [contact][ContactVarsN] - Contacts *tensor.Float32 + Contacts *tensor.Float32 `display:"no-inline"` // JointControls are dynamic joint control inputs. // [joint][JointControlVarsN] - JointControls *tensor.Float32 + JointControls *tensor.Float32 `display:"no-inline"` } func NewWorld() *World { @@ -56,7 +56,8 @@ func NewWorld() *World { // Init makes initial vars. func (wl *World) Init() { - wl.Params = []PhysParams{} + wl.Params = make([]PhysParams, 1) + wl.Params[0].Defaults() wl.Bodies = tensor.NewFloat32(0, int(BodyVarsN)) wl.Joints = tensor.NewFloat32(0, int(JointVarsN)) wl.BodyJoints = tensor.NewInt32(0, 2, 2) @@ -80,31 +81,38 @@ func (wl *World) NewBody(shape Shapes, size, pos math32.Vector3, rot math32.Quat } // NewDynamic adds a new dynamic body with given parameters. Returns the index. -func (wl *World) NewDynamic(shape Shapes, size, pos math32.Vector3, rot math32.Quat) (bodyIdx, dynIdx int32) { +func (wl *World) NewDynamic(shape Shapes, mass float32, size, pos math32.Vector3, rot math32.Quat) (bodyIdx, dynIdx int32) { bodyIdx = wl.NewBody(shape, size, pos, rot) sizes := wl.Dynamics.ShapeSizes() dynIdx = int32(sizes[0]) wl.Dynamics.SetShapeSizes(int(dynIdx+1), 2, int(DynamicVarsN)) SetDynamicIndex(dynIdx, 0, bodyIdx) SetDynamicIndex(dynIdx, 1, bodyIdx) + wl.SetMass(bodyIdx, shape, size, mass) wl.Params[0].DynamicsN = dynIdx + 1 return } // NewJoint adds a new joint between parent and child dynamic object indexes. -func (wl *World) NewJoint(joint JointTypes, parent, child int32, pos math32.Vector3) int32 { +// Use -1 for parent to add a world-anchored joint. +// ppos, cpos are the relative positions from the parent, child. +// Sets relative rotation matricies to identity by default. +// axis is the axis of articulation for the joint. +func (wl *World) NewJoint(joint JointTypes, parent, child int32, ppos, cpos, axis math32.Vector3) int32 { sizes := wl.Joints.ShapeSizes() idx := int32(sizes[0]) wl.Joints.SetShapeSizes(int(idx+1), int(JointVarsN)) + wl.JointControls.SetShapeSizes(int(idx+1), int(JointControlVarsN)) SetJointParent(idx, parent) SetJointChild(idx, child) - SetJointPPos(idx, pos) + SetJointPPos(idx, ppos) + SetJointCPos(idx, cpos) + SetJointAxis(idx, axis) + wl.JointDefaults(idx) wl.Params[0].JointsN = idx + 1 return idx } -// todo: init bodyjoints - // SetAsCurrent sets these as the current global values that are // processed in the code (on the GPU). If this was not the setter of // the current variables, then the parameter variables are copied up diff --git a/physics/world/view.go b/physics/world/view.go index d736a31e..b6d74f62 100644 --- a/physics/world/view.go +++ b/physics/world/view.go @@ -62,8 +62,8 @@ func (wr *World) NewBody(wl *physics.World, name string, shape physics.Shapes, c // NewDynamic adds a new dynamic body with given parameters. // Returns the View which can then be further customized. -func (wr *World) NewDynamic(wl *physics.World, name string, shape physics.Shapes, clr string, size, pos math32.Vector3, rot math32.Quat) *View { - idx, dyIdx := wl.NewDynamic(shape, size, pos, rot) +func (wr *World) NewDynamic(wl *physics.World, name string, shape physics.Shapes, clr string, mass float32, size, pos math32.Vector3, rot math32.Quat) *View { + idx, dyIdx := wl.NewDynamic(shape, mass, size, pos, rot) vw := &View{Name: name, Index: idx, DynamicIndex: dyIdx, Shape: shape, Color: clr, Size: size, Pos: pos, Rot: rot} wr.Views = append(wr.Views, vw) return vw @@ -71,10 +71,11 @@ func (wr *World) NewDynamic(wl *physics.World, name string, shape physics.Shapes // UpdateFromPhysics updates the View from physics state. func (vw *View) UpdateFromPhysics() { + params := physics.GetParams(0) if vw.DynamicIndex >= 0 { ix := int32(vw.DynamicIndex) - vw.Pos = physics.DynamicPos(ix) - vw.Rot = physics.DynamicRot(ix) + vw.Pos = physics.DynamicPos(ix, params.Cur) + vw.Rot = physics.DynamicRot(ix, params.Cur) } else { ix := int32(vw.Index) vw.Pos = physics.BodyPos(ix) From eea8655d4ee5f94b32c8cf0bbd0da110538096e8 Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Tue, 16 Dec 2025 17:48:17 +0100 Subject: [PATCH 20/97] physics: update to proper separate dof records --- physics/control.go | 114 +++----------- physics/control.goal | 114 +++----------- physics/enumgen.go | 125 ++++++++++----- physics/examples/test1/test1.go | 4 +- physics/gosl.go | 46 ++++-- physics/joint.go | 166 ++++++++++++-------- physics/joint.goal | 169 ++++++++++++-------- physics/params.go | 5 + physics/shaders/DeltasFromJoints.wgsl | 148 +++++++++--------- physics/shaders/DynamicsCurToNext.wgsl | 128 ++++++++------- physics/shaders/ForcesFromJoints.wgsl | 148 +++++++++--------- physics/shaders/InitDynamics.wgsl | 144 ++++++++--------- physics/shaders/StepBodyDeltas.wgsl | 162 +++++++++---------- physics/shaders/StepIntegrateBodies.wgsl | 166 ++++++++++---------- physics/shaders/StepJointForces.wgsl | 165 ++++++++++---------- physics/shaders/StepSolveJoints.wgsl | 191 +++++++++++------------ physics/step_joint.go | 43 ++--- physics/step_joint.goal | 42 ++--- physics/typegen.go | 10 +- physics/vars.go | 11 +- physics/world.go | 38 ++++- physics/world.goal | 38 ++++- 22 files changed, 1080 insertions(+), 1097 deletions(-) diff --git a/physics/control.go b/physics/control.go index dc7eeac9..cdec59f2 100644 --- a/physics/control.go +++ b/physics/control.go @@ -6,121 +6,43 @@ package physics -import "cogentcore.org/core/math32" - //gosl:start // JointControlVars are external joint control input variables stored in tensor.Float32. -// These must be in one-to-one correspondence with the joints. -// The X,Y,Z indexes are used progressively for the increasing degrees of freedom for -// the joints. +// These must be in one-to-one correspondence with the JointDoFs. type JointControlVars int32 //enums:enum const ( // Joint force and torque inputs - JointControlForceX JointControlVars = iota - JointControlForceY - JointControlForceZ - - JointControlTorqueX - JointControlTorqueY - JointControlTorqueZ - - // target values (1 DoF use JointTargetPosX) - JointTargetPosX - JointTargetPosY - JointTargetPosZ - - JointTargetRotX - JointTargetRotY - JointTargetRotZ - JointTargetRotW - - // target velocity - JointTargetVelX - JointTargetVelY - JointTargetVelZ - - // target angular velocity - JointTargetAngVelX - JointTargetAngVelY - JointTargetAngVelZ + JointControlForce JointControlVars = iota + JointTargetPos + JointTargetVel ) -func JointControlForce(idx int32) math32.Vector3 { - return math32.Vec3(JointControls.Value(int(idx), int(JointControlForceX)), JointControls.Value(int(idx), int(JointControlForceY)), JointControls.Value(int(idx), int(JointControlForceZ))) -} - -func SetJointControlForce(idx int32, f math32.Vector3) { - JointControls.Set(f.X, int(idx), int(JointControlForceX)) - JointControls.Set(f.Y, int(idx), int(JointControlForceY)) - JointControls.Set(f.Z, int(idx), int(JointControlForceZ)) -} - -func JointControlTorque(idx int32) math32.Vector3 { - return math32.Vec3(JointControls.Value(int(idx), int(JointControlTorqueX)), JointControls.Value(int(idx), int(JointControlTorqueY)), JointControls.Value(int(idx), int(JointControlTorqueZ))) -} - -func SetJointControlTorque(idx int32, f math32.Vector3) { - JointControls.Set(f.X, int(idx), int(JointControlTorqueX)) - JointControls.Set(f.Y, int(idx), int(JointControlTorqueY)) - JointControls.Set(f.Z, int(idx), int(JointControlTorqueZ)) -} - -func JointTargetPos(idx int32) math32.Vector3 { - return math32.Vec3(JointControls.Value(int(idx), int(JointTargetPosX)), JointControls.Value(int(idx), int(JointTargetPosY)), JointControls.Value(int(idx), int(JointTargetPosZ))) -} - -func SetJointTargetPos(idx int32, f math32.Vector3) { - JointControls.Set(f.X, int(idx), int(JointTargetPosX)) - JointControls.Set(f.Y, int(idx), int(JointTargetPosY)) - JointControls.Set(f.Z, int(idx), int(JointTargetPosZ)) +// SetJointControlForce sets the control force for given dof for given joint +// to given value. +func SetJointControlForce(idx, dof int32, value float32) { + JointControls.Set(value, int(JointDoFIndex(idx, dof)), int(JointControlForce)) } -func JointTargetRot(idx int32) math32.Vector3 { - return math32.Vec3(JointControls.Value(int(idx), int(JointTargetRotX)), JointControls.Value(int(idx), int(JointTargetRotY)), JointControls.Value(int(idx), int(JointTargetRotZ))) +func GetJointControlForce(idx, dof int32) float32 { + return JointControls.Value(int(JointDoFIndex(idx, dof)), int(JointControlForce)) } -func SetJointTargetRot(idx int32, f math32.Vector3) { - JointControls.Set(f.X, int(idx), int(JointTargetRotX)) - JointControls.Set(f.Y, int(idx), int(JointTargetRotY)) - JointControls.Set(f.Z, int(idx), int(JointTargetRotZ)) +func SetJointTargetPos(idx, dof int32, value float32) { + JointControls.Set(value, int(JointDoFIndex(idx, dof)), int(JointTargetPos)) } -func JointTargetVel(idx int32) math32.Vector3 { - return math32.Vec3(JointControls.Value(int(idx), int(JointTargetVelX)), JointControls.Value(int(idx), int(JointTargetVelY)), JointControls.Value(int(idx), int(JointTargetVelZ))) +func GetJointTargetPos(idx, dof int32) float32 { + return JointControls.Value(int(JointDoFIndex(idx, dof)), int(JointTargetPos)) } -func SetJointTargetVel(idx int32, f math32.Vector3) { - JointControls.Set(f.X, int(idx), int(JointTargetVelX)) - JointControls.Set(f.Y, int(idx), int(JointTargetVelY)) - JointControls.Set(f.Z, int(idx), int(JointTargetVelZ)) +func SetJointTargetVel(idx, dof int32, value float32) { + JointControls.Set(value, int(JointDoFIndex(idx, dof)), int(JointTargetVel)) } -func JointTargetAngVel(idx int32) math32.Vector3 { - return math32.Vec3(JointControls.Value(int(idx), int(JointTargetAngVelX)), JointControls.Value(int(idx), int(JointTargetAngVelY)), JointControls.Value(int(idx), int(JointTargetAngVelZ))) -} - -func SetJointTargetAngVel(idx int32, f math32.Vector3) { - JointControls.Set(f.X, int(idx), int(JointTargetAngVelX)) - JointControls.Set(f.Y, int(idx), int(JointTargetAngVelY)) - JointControls.Set(f.Z, int(idx), int(JointTargetAngVelZ)) +func GetJointTargetVel(idx, dof int32) float32 { + return JointControls.Value(int(JointDoFIndex(idx, dof)), int(JointTargetVel)) } //gosl:end - -func (wl *World) SetJointTargetPos(idx int32, dim math32.Dims, value float32) { - JointControls.Set(value, int(idx), int(int(JointTargetPosX)+int(dim))) -} - -func (wl *World) SetJointTargetRot(idx int32, dim math32.Dims, value float32) { - JointControls.Set(value, int(idx), int(int(JointTargetRotX)+int(dim))) -} - -func (wl *World) SetJointControlForce(idx int32, dim math32.Dims, value float32) { - JointControls.Set(value, int(idx), int(int(JointControlForceX)+int(dim))) -} - -func (wl *World) SetJointControlTorque(idx int32, dim math32.Dims, value float32) { - JointControls.Set(value, int(idx), int(int(JointControlTorqueX)+int(dim))) -} diff --git a/physics/control.goal b/physics/control.goal index edc6d184..65999e2b 100644 --- a/physics/control.goal +++ b/physics/control.goal @@ -4,121 +4,43 @@ package physics -import "cogentcore.org/core/math32" - //gosl:start // JointControlVars are external joint control input variables stored in tensor.Float32. -// These must be in one-to-one correspondence with the joints. -// The X,Y,Z indexes are used progressively for the increasing degrees of freedom for -// the joints. +// These must be in one-to-one correspondence with the JointDoFs. type JointControlVars int32 //enums:enum const ( // Joint force and torque inputs - JointControlForceX JointControlVars = iota - JointControlForceY - JointControlForceZ - - JointControlTorqueX - JointControlTorqueY - JointControlTorqueZ - - // target values (1 DoF use JointTargetPosX) - JointTargetPosX - JointTargetPosY - JointTargetPosZ - - JointTargetRotX - JointTargetRotY - JointTargetRotZ - JointTargetRotW - - // target velocity - JointTargetVelX - JointTargetVelY - JointTargetVelZ - - // target angular velocity - JointTargetAngVelX - JointTargetAngVelY - JointTargetAngVelZ + JointControlForce JointControlVars = iota + JointTargetPos + JointTargetVel ) -func JointControlForce(idx int32) math32.Vector3 { - return math32.Vec3(JointControls[idx, JointControlForceX], JointControls[idx, JointControlForceY], JointControls[idx, JointControlForceZ]) -} - -func SetJointControlForce(idx int32, f math32.Vector3) { - JointControls[idx, JointControlForceX] = f.X - JointControls[idx, JointControlForceY] = f.Y - JointControls[idx, JointControlForceZ] = f.Z -} - -func JointControlTorque(idx int32) math32.Vector3 { - return math32.Vec3(JointControls[idx, JointControlTorqueX], JointControls[idx, JointControlTorqueY], JointControls[idx, JointControlTorqueZ]) -} - -func SetJointControlTorque(idx int32, f math32.Vector3) { - JointControls[idx, JointControlTorqueX] = f.X - JointControls[idx, JointControlTorqueY] = f.Y - JointControls[idx, JointControlTorqueZ] = f.Z -} - -func JointTargetPos(idx int32) math32.Vector3 { - return math32.Vec3(JointControls[idx, JointTargetPosX], JointControls[idx, JointTargetPosY], JointControls[idx, JointTargetPosZ]) +// SetJointControlForce sets the control force for given dof for given joint +// to given value. +func SetJointControlForce(idx, dof int32, value float32) { + JointControls[JointDoFIndex(idx, dof), JointControlForce] = value } -func SetJointTargetPos(idx int32, f math32.Vector3) { - JointControls[idx, JointTargetPosX] = f.X - JointControls[idx, JointTargetPosY] = f.Y - JointControls[idx, JointTargetPosZ] = f.Z +func GetJointControlForce(idx, dof int32) float32 { + return JointControls[JointDoFIndex(idx, dof), JointControlForce] } -func JointTargetRot(idx int32) math32.Vector3 { - return math32.Vec3(JointControls[idx, JointTargetRotX], JointControls[idx, JointTargetRotY], JointControls[idx, JointTargetRotZ]) +func SetJointTargetPos(idx, dof int32, value float32) { + JointControls[JointDoFIndex(idx, dof), JointTargetPos] = value } -func SetJointTargetRot(idx int32, f math32.Vector3) { - JointControls[idx, JointTargetRotX] = f.X - JointControls[idx, JointTargetRotY] = f.Y - JointControls[idx, JointTargetRotZ] = f.Z +func GetJointTargetPos(idx, dof int32) float32 { + return JointControls[JointDoFIndex(idx, dof), JointTargetPos] } -func JointTargetVel(idx int32) math32.Vector3 { - return math32.Vec3(JointControls[idx, JointTargetVelX], JointControls[idx, JointTargetVelY], JointControls[idx, JointTargetVelZ]) +func SetJointTargetVel(idx, dof int32, value float32) { + JointControls[JointDoFIndex(idx, dof), JointTargetVel] = value } -func SetJointTargetVel(idx int32, f math32.Vector3) { - JointControls[idx, JointTargetVelX] = f.X - JointControls[idx, JointTargetVelY] = f.Y - JointControls[idx, JointTargetVelZ] = f.Z -} - -func JointTargetAngVel(idx int32) math32.Vector3 { - return math32.Vec3(JointControls[idx, JointTargetAngVelX], JointControls[idx, JointTargetAngVelY], JointControls[idx, JointTargetAngVelZ]) -} - -func SetJointTargetAngVel(idx int32, f math32.Vector3) { - JointControls[idx, JointTargetAngVelX] = f.X - JointControls[idx, JointTargetAngVelY] = f.Y - JointControls[idx, JointTargetAngVelZ] = f.Z +func GetJointTargetVel(idx, dof int32) float32 { + return JointControls[JointDoFIndex(idx, dof), JointTargetVel] } //gosl:end - -func (wl *World) SetJointTargetPos(idx int32, dim math32.Dims, value float32) { - JointControls[idx, int(JointTargetPosX)+int(dim)] = value -} - -func (wl *World) SetJointTargetRot(idx int32, dim math32.Dims, value float32) { - JointControls[idx, int(JointTargetRotX)+int(dim)] = value -} - -func (wl *World) SetJointControlForce(idx int32, dim math32.Dims, value float32) { - JointControls[idx, int(JointControlForceX)+int(dim)] = value -} - -func (wl *World) SetJointControlTorque(idx int32, dim math32.Dims, value float32) { - JointControls[idx, int(JointControlTorqueX)+int(dim)] = value -} diff --git a/physics/enumgen.go b/physics/enumgen.go index ca61585d..b44dbf62 100644 --- a/physics/enumgen.go +++ b/physics/enumgen.go @@ -98,20 +98,20 @@ func (i *ContactVars) UnmarshalText(text []byte) error { return enums.UnmarshalText(i, text, "ContactVars") } -var _JointControlVarsValues = []JointControlVars{0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18} +var _JointControlVarsValues = []JointControlVars{0, 1, 2} // JointControlVarsN is the highest valid value for type JointControlVars, plus one. // //gosl:start -const JointControlVarsN JointControlVars = 19 +const JointControlVarsN JointControlVars = 3 //gosl:end -var _JointControlVarsValueMap = map[string]JointControlVars{`JointCtrlForceX`: 0, `JointCtrlForceY`: 1, `JointCtrlForceZ`: 2, `JointCtrlTorqueX`: 3, `JointCtrlTorqueY`: 4, `JointCtrlTorqueZ`: 5, `JointTargetPosX`: 6, `JointTargetPosY`: 7, `JointTargetPosZ`: 8, `JointTargetRotX`: 9, `JointTargetRotY`: 10, `JointTargetRotZ`: 11, `JointTargetRotW`: 12, `JointTargetVelX`: 13, `JointTargetVelY`: 14, `JointTargetVelZ`: 15, `JointTargetAngVelX`: 16, `JointTargetAngVelY`: 17, `JointTargetAngVelZ`: 18} +var _JointControlVarsValueMap = map[string]JointControlVars{`JointControlForce`: 0, `JointTargetPos`: 1, `JointTargetVel`: 2} -var _JointControlVarsDescMap = map[JointControlVars]string{0: `Joint force and torque inputs`, 1: ``, 2: ``, 3: ``, 4: ``, 5: ``, 6: `target values (1 DoF use JointTargetPosX)`, 7: ``, 8: ``, 9: ``, 10: ``, 11: ``, 12: ``, 13: `target velocity`, 14: ``, 15: ``, 16: `target angular velocity`, 17: ``, 18: ``} +var _JointControlVarsDescMap = map[JointControlVars]string{0: `Joint force and torque inputs`, 1: ``, 2: ``} -var _JointControlVarsMap = map[JointControlVars]string{0: `JointCtrlForceX`, 1: `JointCtrlForceY`, 2: `JointCtrlForceZ`, 3: `JointCtrlTorqueX`, 4: `JointCtrlTorqueY`, 5: `JointCtrlTorqueZ`, 6: `JointTargetPosX`, 7: `JointTargetPosY`, 8: `JointTargetPosZ`, 9: `JointTargetRotX`, 10: `JointTargetRotY`, 11: `JointTargetRotZ`, 12: `JointTargetRotW`, 13: `JointTargetVelX`, 14: `JointTargetVelY`, 15: `JointTargetVelZ`, 16: `JointTargetAngVelX`, 17: `JointTargetAngVelY`, 18: `JointTargetAngVelZ`} +var _JointControlVarsMap = map[JointControlVars]string{0: `JointControlForce`, 1: `JointTargetPos`, 2: `JointTargetVel`} // String returns the string representation of this JointControlVars value. func (i JointControlVars) String() string { return enums.String(i, _JointControlVarsMap) } @@ -192,20 +192,20 @@ func (i *DynamicVars) UnmarshalText(text []byte) error { return enums.UnmarshalText(i, text, "DynamicVars") } -var _GPUVarsValues = []GPUVars{0, 1, 2, 3, 4, 5, 6} +var _GPUVarsValues = []GPUVars{0, 1, 2, 3, 4, 5, 6, 7} // GPUVarsN is the highest valid value for type GPUVars, plus one. // //gosl:start -const GPUVarsN GPUVars = 7 +const GPUVarsN GPUVars = 8 //gosl:end -var _GPUVarsValueMap = map[string]GPUVars{`ParamsVar`: 0, `BodiesVar`: 1, `JointsVar`: 2, `BodyJointsVar`: 3, `DynamicsVar`: 4, `ContactsVar`: 5, `JointControlsVar`: 6} +var _GPUVarsValueMap = map[string]GPUVars{`ParamsVar`: 0, `BodiesVar`: 1, `JointsVar`: 2, `JointDoFsVar`: 3, `BodyJointsVar`: 4, `DynamicsVar`: 5, `ContactsVar`: 6, `JointControlsVar`: 7} -var _GPUVarsDescMap = map[GPUVars]string{0: ``, 1: ``, 2: ``, 3: ``, 4: ``, 5: ``, 6: ``} +var _GPUVarsDescMap = map[GPUVars]string{0: ``, 1: ``, 2: ``, 3: ``, 4: ``, 5: ``, 6: ``, 7: ``} -var _GPUVarsMap = map[GPUVars]string{0: `ParamsVar`, 1: `BodiesVar`, 2: `JointsVar`, 3: `BodyJointsVar`, 4: `DynamicsVar`, 5: `ContactsVar`, 6: `JointControlsVar`} +var _GPUVarsMap = map[GPUVars]string{0: `ParamsVar`, 1: `BodiesVar`, 2: `JointsVar`, 3: `JointDoFsVar`, 4: `BodyJointsVar`, 5: `DynamicsVar`, 6: `ContactsVar`, 7: `JointControlsVar`} // String returns the string representation of this GPUVars value. func (i GPUVars) String() string { return enums.String(i, _GPUVarsMap) } @@ -237,20 +237,67 @@ func (i GPUVars) MarshalText() ([]byte, error) { return []byte(i.String()), nil // UnmarshalText implements the [encoding.TextUnmarshaler] interface. func (i *GPUVars) UnmarshalText(text []byte) error { return enums.UnmarshalText(i, text, "GPUVars") } -var _JointVarsValues = []JointVars{0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 51, 52} +var _JointTypesValues = []JointTypes{0, 1, 2, 3, 4, 5, 6} + +// JointTypesN is the highest valid value for type JointTypes, plus one. +// +//gosl:start +const JointTypesN JointTypes = 7 + +//gosl:end + +var _JointTypesValueMap = map[string]JointTypes{`Prismatic`: 0, `Revolute`: 1, `Ball`: 2, `Fixed`: 3, `Free`: 4, `Distance`: 5, `D6`: 6} + +var _JointTypesDescMap = map[JointTypes]string{0: `Prismatic allows translation along a single axis (slider): 1 DoF.`, 1: `Revolute allows rotation about a single axis (axel): 1 DoF.`, 2: `Ball allows rotation about all three axes (3 DoF, quaternion).`, 3: `Fixed locks all relative motion: 0 DoF.`, 4: `Free allows full 6-DoF motion (translation and rotation).`, 5: `Distance keeps two bodies a distance within joint limits: 6 DoF.`, 6: `D6 is a generic 6-DoF joint.`} + +var _JointTypesMap = map[JointTypes]string{0: `Prismatic`, 1: `Revolute`, 2: `Ball`, 3: `Fixed`, 4: `Free`, 5: `Distance`, 6: `D6`} + +// String returns the string representation of this JointTypes value. +func (i JointTypes) String() string { return enums.String(i, _JointTypesMap) } + +// SetString sets the JointTypes value from its string representation, +// and returns an error if the string is invalid. +func (i *JointTypes) SetString(s string) error { + return enums.SetString(i, s, _JointTypesValueMap, "JointTypes") +} + +// Int64 returns the JointTypes value as an int64. +func (i JointTypes) Int64() int64 { return int64(i) } + +// SetInt64 sets the JointTypes value from an int64. +func (i *JointTypes) SetInt64(in int64) { *i = JointTypes(in) } + +// Desc returns the description of the JointTypes value. +func (i JointTypes) Desc() string { return enums.Desc(i, _JointTypesDescMap) } + +// JointTypesValues returns all possible values for the type JointTypes. +func JointTypesValues() []JointTypes { return _JointTypesValues } + +// Values returns all possible values for the type JointTypes. +func (i JointTypes) Values() []enums.Enum { return enums.Values(_JointTypesValues) } + +// MarshalText implements the [encoding.TextMarshaler] interface. +func (i JointTypes) MarshalText() ([]byte, error) { return []byte(i.String()), nil } + +// UnmarshalText implements the [encoding.TextUnmarshaler] interface. +func (i *JointTypes) UnmarshalText(text []byte) error { + return enums.UnmarshalText(i, text, "JointTypes") +} + +var _JointVarsValues = []JointVars{0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48} // JointVarsN is the highest valid value for type JointVars, plus one. // //gosl:start -const JointVarsN JointVars = 53 +const JointVarsN JointVars = 49 //gosl:end -var _JointVarsValueMap = map[string]JointVars{`JointType`: 0, `JointEnabled`: 1, `JointParent`: 2, `JointChild`: 3, `JointPPosX`: 4, `JointPPosY`: 5, `JointPPosZ`: 6, `JointPRotX`: 7, `JointPRotY`: 8, `JointPRotZ`: 9, `JointPRotW`: 10, `JointCPosX`: 11, `JointCPosY`: 12, `JointCPosZ`: 13, `JointCRotX`: 14, `JointCRotY`: 15, `JointCRotZ`: 16, `JointCRotW`: 17, `JointAxisX`: 18, `JointAxisY`: 19, `JointAxisZ`: 20, `JointLimitLower`: 21, `JointLimitUpper`: 22, `JointStiffX`: 23, `JointStiffY`: 24, `JointStiffZ`: 25, `JointDampX`: 26, `JointDampY`: 27, `JointDampZ`: 28, `JointPForceX`: 29, `JointPForceY`: 30, `JointPForceZ`: 31, `JointPTorqueX`: 32, `JointPTorqueY`: 33, `JointPTorqueZ`: 34, `JointCForceX`: 35, `JointCForceY`: 36, `JointCForceZ`: 37, `JointCTorqueX`: 38, `JointCTorqueY`: 39, `JointCTorqueZ`: 40, `JointPDeltaX`: 41, `JointPDeltaY`: 42, `JointPDeltaZ`: 43, `JointPAngDeltaX`: 44, `JointPAngDeltaY`: 45, `JointPAngDeltaZ`: 46, `JointCDeltaX`: 47, `JointCDeltaY`: 48, `JointCDeltaZ`: 49, `JointCAngDeltaX`: 50, `JointCAngDeltaY`: 51, `JointCAngDeltaZ`: 52} +var _JointVarsValueMap = map[string]JointVars{`JointType`: 0, `JointEnabled`: 1, `JointParent`: 2, `JointChild`: 3, `JointPPosX`: 4, `JointPPosY`: 5, `JointPPosZ`: 6, `JointPRotX`: 7, `JointPRotY`: 8, `JointPRotZ`: 9, `JointPRotW`: 10, `JointCPosX`: 11, `JointCPosY`: 12, `JointCPosZ`: 13, `JointCRotX`: 14, `JointCRotY`: 15, `JointCRotZ`: 16, `JointCRotW`: 17, `JointDoFN`: 18, `JointDoF1`: 19, `JointDoF2`: 20, `JointDoF3`: 21, `JointDoF4`: 22, `JointDoF5`: 23, `JointDoF6`: 24, `JointPForceX`: 25, `JointPForceY`: 26, `JointPForceZ`: 27, `JointPTorqueX`: 28, `JointPTorqueY`: 29, `JointPTorqueZ`: 30, `JointCForceX`: 31, `JointCForceY`: 32, `JointCForceZ`: 33, `JointCTorqueX`: 34, `JointCTorqueY`: 35, `JointCTorqueZ`: 36, `JointPDeltaX`: 37, `JointPDeltaY`: 38, `JointPDeltaZ`: 39, `JointPAngDeltaX`: 40, `JointPAngDeltaY`: 41, `JointPAngDeltaZ`: 42, `JointCDeltaX`: 43, `JointCDeltaY`: 44, `JointCDeltaZ`: 45, `JointCAngDeltaX`: 46, `JointCAngDeltaY`: 47, `JointCAngDeltaZ`: 48} -var _JointVarsDescMap = map[JointVars]string{0: `JointType (as an int32 from bits).`, 1: `JointEnabled allows joints to be dynamically enabled.`, 2: `JointParent is the dynamic body index for parent body. Can be -1 for a fixed parent for absolute anchor.`, 3: `JointChild is the dynamic body index for child body.`, 4: `relative position of joint, in parent frame.`, 5: ``, 6: ``, 7: `relative orientation of joint, in parent frame.`, 8: ``, 9: ``, 10: ``, 11: `relative position of joint, in child frame.`, 12: ``, 13: ``, 14: `relative orientation of joint, in child frame.`, 15: ``, 16: ``, 17: ``, 18: `axis of articulation for the joint`, 19: ``, 20: ``, 21: `joint limits`, 22: ``, 23: `joint stiffness target (ke)`, 24: ``, 25: ``, 26: `joint damping target (kd)`, 27: ``, 28: ``, 29: `Computed parent joint force value.`, 30: ``, 31: ``, 32: `Computed parent joint torque value.`, 33: ``, 34: ``, 35: `Computed child joint force value.`, 36: ``, 37: ``, 38: `Computed child joint torque value.`, 39: ``, 40: ``, 41: `Computed parent joint delta value.`, 42: ``, 43: ``, 44: `Computed parent joint angdelta value.`, 45: ``, 46: ``, 47: `Computed child joint delta value.`, 48: ``, 49: ``, 50: `Computed child joint angdelta value.`, 51: ``, 52: ``} +var _JointVarsDescMap = map[JointVars]string{0: `JointType (as an int32 from bits).`, 1: `JointEnabled allows joints to be dynamically enabled.`, 2: `JointParent is the dynamic body index for parent body. Can be -1 for a fixed parent for absolute anchor.`, 3: `JointChild is the dynamic body index for child body.`, 4: `relative position of joint, in parent frame.`, 5: ``, 6: ``, 7: `relative orientation of joint, in parent frame.`, 8: ``, 9: ``, 10: ``, 11: `relative position of joint, in child frame.`, 12: ``, 13: ``, 14: `relative orientation of joint, in child frame.`, 15: ``, 16: ``, 17: ``, 18: `JointDoFN is the number of degrees-of-freedom for the joint.`, 19: `indexes in JointDoFs for each DoF`, 20: ``, 21: ``, 22: `angular starts here for Free, D6`, 23: ``, 24: ``, 25: `Computed parent joint force value.`, 26: ``, 27: ``, 28: `Computed parent joint torque value.`, 29: ``, 30: ``, 31: `Computed child joint force value.`, 32: ``, 33: ``, 34: `Computed child joint torque value.`, 35: ``, 36: ``, 37: `Computed parent joint delta value.`, 38: ``, 39: ``, 40: `Computed parent joint angdelta value.`, 41: ``, 42: ``, 43: `Computed child joint delta value.`, 44: ``, 45: ``, 46: `Computed child joint angdelta value.`, 47: ``, 48: ``} -var _JointVarsMap = map[JointVars]string{0: `JointType`, 1: `JointEnabled`, 2: `JointParent`, 3: `JointChild`, 4: `JointPPosX`, 5: `JointPPosY`, 6: `JointPPosZ`, 7: `JointPRotX`, 8: `JointPRotY`, 9: `JointPRotZ`, 10: `JointPRotW`, 11: `JointCPosX`, 12: `JointCPosY`, 13: `JointCPosZ`, 14: `JointCRotX`, 15: `JointCRotY`, 16: `JointCRotZ`, 17: `JointCRotW`, 18: `JointAxisX`, 19: `JointAxisY`, 20: `JointAxisZ`, 21: `JointLimitLower`, 22: `JointLimitUpper`, 23: `JointStiffX`, 24: `JointStiffY`, 25: `JointStiffZ`, 26: `JointDampX`, 27: `JointDampY`, 28: `JointDampZ`, 29: `JointPForceX`, 30: `JointPForceY`, 31: `JointPForceZ`, 32: `JointPTorqueX`, 33: `JointPTorqueY`, 34: `JointPTorqueZ`, 35: `JointCForceX`, 36: `JointCForceY`, 37: `JointCForceZ`, 38: `JointCTorqueX`, 39: `JointCTorqueY`, 40: `JointCTorqueZ`, 41: `JointPDeltaX`, 42: `JointPDeltaY`, 43: `JointPDeltaZ`, 44: `JointPAngDeltaX`, 45: `JointPAngDeltaY`, 46: `JointPAngDeltaZ`, 47: `JointCDeltaX`, 48: `JointCDeltaY`, 49: `JointCDeltaZ`, 50: `JointCAngDeltaX`, 51: `JointCAngDeltaY`, 52: `JointCAngDeltaZ`} +var _JointVarsMap = map[JointVars]string{0: `JointType`, 1: `JointEnabled`, 2: `JointParent`, 3: `JointChild`, 4: `JointPPosX`, 5: `JointPPosY`, 6: `JointPPosZ`, 7: `JointPRotX`, 8: `JointPRotY`, 9: `JointPRotZ`, 10: `JointPRotW`, 11: `JointCPosX`, 12: `JointCPosY`, 13: `JointCPosZ`, 14: `JointCRotX`, 15: `JointCRotY`, 16: `JointCRotZ`, 17: `JointCRotW`, 18: `JointDoFN`, 19: `JointDoF1`, 20: `JointDoF2`, 21: `JointDoF3`, 22: `JointDoF4`, 23: `JointDoF5`, 24: `JointDoF6`, 25: `JointPForceX`, 26: `JointPForceY`, 27: `JointPForceZ`, 28: `JointPTorqueX`, 29: `JointPTorqueY`, 30: `JointPTorqueZ`, 31: `JointCForceX`, 32: `JointCForceY`, 33: `JointCForceZ`, 34: `JointCTorqueX`, 35: `JointCTorqueY`, 36: `JointCTorqueZ`, 37: `JointPDeltaX`, 38: `JointPDeltaY`, 39: `JointPDeltaZ`, 40: `JointPAngDeltaX`, 41: `JointPAngDeltaY`, 42: `JointPAngDeltaZ`, 43: `JointCDeltaX`, 44: `JointCDeltaY`, 45: `JointCDeltaZ`, 46: `JointCAngDeltaX`, 47: `JointCAngDeltaY`, 48: `JointCAngDeltaZ`} // String returns the string representation of this JointVars value. func (i JointVars) String() string { return enums.String(i, _JointVarsMap) } @@ -284,51 +331,51 @@ func (i *JointVars) UnmarshalText(text []byte) error { return enums.UnmarshalText(i, text, "JointVars") } -var _JointTypesValues = []JointTypes{0, 1, 2, 3, 4, 5, 6} +var _JointDoFVarsValues = []JointDoFVars{0, 1, 2, 3, 4, 5, 6} -// JointTypesN is the highest valid value for type JointTypes, plus one. +// JointDoFVarsN is the highest valid value for type JointDoFVars, plus one. // //gosl:start -const JointTypesN JointTypes = 7 +const JointDoFVarsN JointDoFVars = 7 //gosl:end -var _JointTypesValueMap = map[string]JointTypes{`Prismatic`: 0, `Revolute`: 1, `Ball`: 2, `Fixed`: 3, `Free`: 4, `Distance`: 5, `D6`: 6} +var _JointDoFVarsValueMap = map[string]JointDoFVars{`JointAxisX`: 0, `JointAxisY`: 1, `JointAxisZ`: 2, `JointLimitLower`: 3, `JointLimitUpper`: 4, `JointStiff`: 5, `JointDamp`: 6} -var _JointTypesDescMap = map[JointTypes]string{0: `Prismatic allows translation along a single axis (slider): 1 DoF.`, 1: `Revolute allows rotation about a single axis (axel): 1 DoF.`, 2: `Ball allows rotation about all three axes (3 DoF, quaternion).`, 3: `Fixed locks all relative motion: 0 DoF.`, 4: `Free allows full 6-DoF motion (translation and rotation).`, 5: `Distance keeps two bodies a distance within joint limits: 6 DoF.`, 6: `D6 is a generic 6-DoF joint.`} +var _JointDoFVarsDescMap = map[JointDoFVars]string{0: `axis of articulation for the DoF`, 1: ``, 2: ``, 3: `joint limits`, 4: ``, 5: `joint stiffness target (ke)`, 6: `joint damping target (kd)`} -var _JointTypesMap = map[JointTypes]string{0: `Prismatic`, 1: `Revolute`, 2: `Ball`, 3: `Fixed`, 4: `Free`, 5: `Distance`, 6: `D6`} +var _JointDoFVarsMap = map[JointDoFVars]string{0: `JointAxisX`, 1: `JointAxisY`, 2: `JointAxisZ`, 3: `JointLimitLower`, 4: `JointLimitUpper`, 5: `JointStiff`, 6: `JointDamp`} -// String returns the string representation of this JointTypes value. -func (i JointTypes) String() string { return enums.String(i, _JointTypesMap) } +// String returns the string representation of this JointDoFVars value. +func (i JointDoFVars) String() string { return enums.String(i, _JointDoFVarsMap) } -// SetString sets the JointTypes value from its string representation, +// SetString sets the JointDoFVars value from its string representation, // and returns an error if the string is invalid. -func (i *JointTypes) SetString(s string) error { - return enums.SetString(i, s, _JointTypesValueMap, "JointTypes") +func (i *JointDoFVars) SetString(s string) error { + return enums.SetString(i, s, _JointDoFVarsValueMap, "JointDoFVars") } -// Int64 returns the JointTypes value as an int64. -func (i JointTypes) Int64() int64 { return int64(i) } +// Int64 returns the JointDoFVars value as an int64. +func (i JointDoFVars) Int64() int64 { return int64(i) } -// SetInt64 sets the JointTypes value from an int64. -func (i *JointTypes) SetInt64(in int64) { *i = JointTypes(in) } +// SetInt64 sets the JointDoFVars value from an int64. +func (i *JointDoFVars) SetInt64(in int64) { *i = JointDoFVars(in) } -// Desc returns the description of the JointTypes value. -func (i JointTypes) Desc() string { return enums.Desc(i, _JointTypesDescMap) } +// Desc returns the description of the JointDoFVars value. +func (i JointDoFVars) Desc() string { return enums.Desc(i, _JointDoFVarsDescMap) } -// JointTypesValues returns all possible values for the type JointTypes. -func JointTypesValues() []JointTypes { return _JointTypesValues } +// JointDoFVarsValues returns all possible values for the type JointDoFVars. +func JointDoFVarsValues() []JointDoFVars { return _JointDoFVarsValues } -// Values returns all possible values for the type JointTypes. -func (i JointTypes) Values() []enums.Enum { return enums.Values(_JointTypesValues) } +// Values returns all possible values for the type JointDoFVars. +func (i JointDoFVars) Values() []enums.Enum { return enums.Values(_JointDoFVarsValues) } // MarshalText implements the [encoding.TextMarshaler] interface. -func (i JointTypes) MarshalText() ([]byte, error) { return []byte(i.String()), nil } +func (i JointDoFVars) MarshalText() ([]byte, error) { return []byte(i.String()), nil } // UnmarshalText implements the [encoding.TextUnmarshaler] interface. -func (i *JointTypes) UnmarshalText(text []byte) error { - return enums.UnmarshalText(i, text, "JointTypes") +func (i *JointDoFVars) UnmarshalText(text []byte) error { + return enums.UnmarshalText(i, text, "JointDoFVars") } var _ShapesValues = []Shapes{0, 1, 2, 3} diff --git a/physics/examples/test1/test1.go b/physics/examples/test1/test1.go index 136818e1..500f2417 100644 --- a/physics/examples/test1/test1.go +++ b/physics/examples/test1/test1.go @@ -57,9 +57,9 @@ func main() { depth := height * .15 b1 := wr.NewDynamic(wl, "body", physics.Box, "purple", 1.0, math32.Vec3(width, height, depth), math32.Vec3(0, height/2, 0), rot) - bj := wl.NewJoint(physics.Revolute, -1, b1.DynamicIndex, math32.Vec3(-width, 0, 0), math32.Vec3(0, 0, 0), math32.Vec3(1, 0, 0)) + bj := wl.NewJoint1D(physics.Revolute, -1, b1.DynamicIndex, math32.Vec3(-width, 0, 0), math32.Vec3(0, 0, 0), math32.Vec3(1, 0, 0)) _ = bj - wl.SetJointControlForce(bj, math32.X, 5) + physics.SetJointControlForce(bj, 0, 5) wl.Config() params := physics.GetParams(0) diff --git a/physics/gosl.go b/physics/gosl.go index 290c75d2..c76f9dfc 100644 --- a/physics/gosl.go +++ b/physics/gosl.go @@ -42,10 +42,11 @@ const ( ParamsVar GPUVars = 0 BodiesVar GPUVars = 1 JointsVar GPUVars = 2 - BodyJointsVar GPUVars = 3 - DynamicsVar GPUVars = 4 - ContactsVar GPUVars = 5 - JointControlsVar GPUVars = 6 + JointDoFsVar GPUVars = 3 + BodyJointsVar GPUVars = 4 + DynamicsVar GPUVars = 5 + ContactsVar GPUVars = 6 + JointControlsVar GPUVars = 7 ) // Tensor stride variables @@ -87,6 +88,7 @@ func GPUInit() { _ = vr vr = sgp.Add("Bodies", gpu.Float32, 1, gpu.ComputeShader) vr = sgp.Add("Joints", gpu.Float32, 1, gpu.ComputeShader) + vr = sgp.Add("JointDoFs", gpu.Float32, 1, gpu.ComputeShader) vr = sgp.Add("BodyJoints", gpu.Int32, 1, gpu.ComputeShader) sgp.SetNValues(1) } @@ -142,6 +144,7 @@ func GPUInit() { pl.AddVarUsed(1, "Bodies") pl.AddVarUsed(2, "Dynamics") pl.AddVarUsed(3, "JointControls") + pl.AddVarUsed(1, "JointDoFs") pl.AddVarUsed(1, "Joints") pl.AddVarUsed(0, "Params") pl = gpu.NewComputePipelineShaderFS(shaders, "shaders/StepSolveJoints.wgsl", sy) @@ -149,6 +152,7 @@ func GPUInit() { pl.AddVarUsed(1, "Bodies") pl.AddVarUsed(2, "Dynamics") pl.AddVarUsed(3, "JointControls") + pl.AddVarUsed(1, "JointDoFs") pl.AddVarUsed(1, "Joints") pl.AddVarUsed(0, "Params") sy.Config() @@ -540,6 +544,9 @@ func ToGPU(vars ...GPUVars) { case JointsVar: v, _ := syVars.ValueByIndex(1, "Joints", 0) gpu.SetValueFrom(v, Joints.Values) + case JointDoFsVar: + v, _ := syVars.ValueByIndex(1, "JointDoFs", 0) + gpu.SetValueFrom(v, JointDoFs.Values) case BodyJointsVar: v, _ := syVars.ValueByIndex(1, "BodyJoints", 0) gpu.SetValueFrom(v, BodyJoints.Values) @@ -573,21 +580,23 @@ func ToGPUTensorStrides() { } sy := GPUSystem syVars := sy.Vars() - TensorStrides.SetShapeSizes(60) + TensorStrides.SetShapeSizes(70) TensorStrides.SetInt1D(Bodies.Shape().Strides[0], 0) TensorStrides.SetInt1D(Bodies.Shape().Strides[1], 1) TensorStrides.SetInt1D(Joints.Shape().Strides[0], 10) TensorStrides.SetInt1D(Joints.Shape().Strides[1], 11) - TensorStrides.SetInt1D(BodyJoints.Shape().Strides[0], 20) - TensorStrides.SetInt1D(BodyJoints.Shape().Strides[1], 21) - TensorStrides.SetInt1D(BodyJoints.Shape().Strides[2], 22) - TensorStrides.SetInt1D(Dynamics.Shape().Strides[0], 30) - TensorStrides.SetInt1D(Dynamics.Shape().Strides[1], 31) - TensorStrides.SetInt1D(Dynamics.Shape().Strides[2], 32) - TensorStrides.SetInt1D(Contacts.Shape().Strides[0], 40) - TensorStrides.SetInt1D(Contacts.Shape().Strides[1], 41) - TensorStrides.SetInt1D(JointControls.Shape().Strides[0], 50) - TensorStrides.SetInt1D(JointControls.Shape().Strides[1], 51) + TensorStrides.SetInt1D(JointDoFs.Shape().Strides[0], 20) + TensorStrides.SetInt1D(JointDoFs.Shape().Strides[1], 21) + TensorStrides.SetInt1D(BodyJoints.Shape().Strides[0], 30) + TensorStrides.SetInt1D(BodyJoints.Shape().Strides[1], 31) + TensorStrides.SetInt1D(BodyJoints.Shape().Strides[2], 32) + TensorStrides.SetInt1D(Dynamics.Shape().Strides[0], 40) + TensorStrides.SetInt1D(Dynamics.Shape().Strides[1], 41) + TensorStrides.SetInt1D(Dynamics.Shape().Strides[2], 42) + TensorStrides.SetInt1D(Contacts.Shape().Strides[0], 50) + TensorStrides.SetInt1D(Contacts.Shape().Strides[1], 51) + TensorStrides.SetInt1D(JointControls.Shape().Strides[0], 60) + TensorStrides.SetInt1D(JointControls.Shape().Strides[1], 61) v, _ := syVars.ValueByIndex(0, "TensorStrides", 0) gpu.SetValueFrom(v, TensorStrides.Values) } @@ -607,6 +616,9 @@ func ReadFromGPU(vars ...GPUVars) { case JointsVar: v, _ := syVars.ValueByIndex(1, "Joints", 0) v.GPUToRead(sy.CommandEncoder) + case JointDoFsVar: + v, _ := syVars.ValueByIndex(1, "JointDoFs", 0) + v.GPUToRead(sy.CommandEncoder) case BodyJointsVar: v, _ := syVars.ValueByIndex(1, "BodyJoints", 0) v.GPUToRead(sy.CommandEncoder) @@ -641,6 +653,10 @@ func SyncFromGPU(vars ...GPUVars) { v, _ := syVars.ValueByIndex(1, "Joints", 0) v.ReadSync() gpu.ReadToBytes(v, Joints.Values) + case JointDoFsVar: + v, _ := syVars.ValueByIndex(1, "JointDoFs", 0) + v.ReadSync() + gpu.ReadToBytes(v, JointDoFs.Values) case BodyJointsVar: v, _ := syVars.ValueByIndex(1, "BodyJoints", 0) v.ReadSync() diff --git a/physics/joint.go b/physics/joint.go index 6c0306d6..2e68e901 100644 --- a/physics/joint.go +++ b/physics/joint.go @@ -14,6 +14,35 @@ import ( //gosl:start +// Sentinel value for unlimited joint limits +const JointLimitUnlimited = 1e10 + +// JointTypes are joint types that determine nature of interaction. +type JointTypes int32 //enums:enum + +const ( + // Prismatic allows translation along a single axis (slider): 1 DoF. + Prismatic JointTypes = iota + + // Revolute allows rotation about a single axis (axel): 1 DoF. + Revolute + + // Ball allows rotation about all three axes (3 DoF, quaternion). + Ball + + // Fixed locks all relative motion: 0 DoF. + Fixed + + // Free allows full 6-DoF motion (translation and rotation). + Free + + // Distance keeps two bodies a distance within joint limits: 6 DoF. + Distance + + // D6 is a generic 6-DoF joint. + D6 +) + // JointVars are joint state variables stored in tensor.Float32. // These are all static joint properties; dynamic control variables // in [JointControlVars] and [JointControls]. @@ -55,24 +84,17 @@ const ( JointCRotZ JointCRotW - // axis of articulation for the joint - JointAxisX - JointAxisY - JointAxisZ - - // joint limits - JointLimitLower - JointLimitUpper + // JointDoFN is the number of degrees-of-freedom for the joint. + JointDoFN - // joint stiffness target (ke) - JointStiffX - JointStiffY - JointStiffZ - - // joint damping target (kd) - JointDampX - JointDampY - JointDampZ + // indexes in JointDoFs for each DoF + JointDoF1 + JointDoF2 + JointDoF3 + // angular starts here for Free, D6 + JointDoF4 + JointDoF5 + JointDoF6 // Computed forces (temp storage until aggregated by bodies). @@ -141,6 +163,22 @@ func JointChildIndex(idx int32) int32 { return int32(math.Float32bits(Joints.Value(int(idx), int(JointChild)))) } +func SetJointDoFN(idx, dofN int32) { + Joints.Set(math.Float32frombits(uint32(dofN)), int(idx), int(JointDoFN)) +} + +func GetJointDoFN(idx int32) int32 { + return int32(math.Float32bits(Joints.Value(int(idx), int(JointDoFN)))) +} + +func SetJointDoFIndex(idx, dof, dofIdx int32) { + Joints.Set(math.Float32frombits(uint32(dofIdx)), int(idx), int(int32(JointDoF1)+dof)) +} + +func JointDoFIndex(idx, dof int32) int32 { + return int32(math.Float32bits(Joints.Value(int(idx), int(int32(JointDoF1)+dof)))) +} + func JointPPos(idx int32) math32.Vector3 { return math32.Vec3(Joints.Value(int(idx), int(JointPPosX)), Joints.Value(int(idx), int(JointPPosY)), Joints.Value(int(idx), int(JointPPosZ))) } @@ -183,36 +221,6 @@ func SetJointCRot(idx int32, rot math32.Quat) { Joints.Set(rot.W, int(idx), int(JointCRotW)) } -func JointAxis(idx int32) math32.Vector3 { - return math32.Vec3(Joints.Value(int(idx), int(JointAxisX)), Joints.Value(int(idx), int(JointAxisY)), Joints.Value(int(idx), int(JointAxisZ))) -} - -func SetJointAxis(idx int32, axis math32.Vector3) { - Joints.Set(axis.X, int(idx), int(JointAxisX)) - Joints.Set(axis.Y, int(idx), int(JointAxisY)) - Joints.Set(axis.Z, int(idx), int(JointAxisZ)) -} - -func JointStiff(idx int32) math32.Vector3 { - return math32.Vec3(Joints.Value(int(idx), int(JointStiffX)), Joints.Value(int(idx), int(JointStiffY)), Joints.Value(int(idx), int(JointStiffZ))) -} - -func SetJointStiff(idx int32, stiff math32.Vector3) { - Joints.Set(stiff.X, int(idx), int(JointStiffX)) - Joints.Set(stiff.Y, int(idx), int(JointStiffY)) - Joints.Set(stiff.Z, int(idx), int(JointStiffZ)) -} - -func JointDamp(idx int32) math32.Vector3 { - return math32.Vec3(Joints.Value(int(idx), int(JointDampX)), Joints.Value(int(idx), int(JointDampY)), Joints.Value(int(idx), int(JointDampZ))) -} - -func SetJointDamp(idx int32, damp math32.Vector3) { - Joints.Set(damp.X, int(idx), int(JointDampX)) - Joints.Set(damp.Y, int(idx), int(JointDampY)) - Joints.Set(damp.Z, int(idx), int(JointDampZ)) -} - func JointPForce(idx int32) math32.Vector3 { return math32.Vec3(Joints.Value(int(idx), int(JointPForceX)), Joints.Value(int(idx), int(JointPForceY)), Joints.Value(int(idx), int(JointPForceZ))) } @@ -293,31 +301,52 @@ func SetJointCAngDelta(idx int32, t math32.Vector3) { Joints.Set(t.Z, int(idx), int(JointCAngDeltaZ)) } -// JointTypes are joint types that determine nature of interaction. -type JointTypes int32 //enums:enum +// JointDoFVars are joint DoF state variables stored in tensor.Float32, +// one for each DoF. +type JointDoFVars int32 //enums:enum const ( - // Prismatic allows translation along a single axis (slider): 1 DoF. - Prismatic JointTypes = iota + // axis of articulation for the DoF + JointAxisX JointDoFVars = iota + JointAxisY + JointAxisZ - // Revolute allows rotation about a single axis (axel): 1 DoF. - Revolute + // joint limits + JointLimitLower + JointLimitUpper - // Ball allows rotation about all three axes (3 DoF, quaternion). - Ball + // joint stiffness target (ke) + JointStiff - // Fixed locks all relative motion: 0 DoF. - Fixed + // joint damping target (kd) + JointDamp +) - // Free allows full 6-DoF motion (translation and rotation). - Free +func JointAxisDoF(didx int32) math32.Vector3 { + return math32.Vec3(JointDoFs.Value(int(didx), int(JointAxisX)), JointDoFs.Value(int(didx), int(JointAxisY)), JointDoFs.Value(int(didx), int(JointAxisZ))) +} - // Distance keeps two bodies a distance within joint limits: 6 DoF. - Distance +func SetJointAxisDoF(didx int32, axis math32.Vector3) { + JointDoFs.Set(axis.X, int(didx), int(JointAxisX)) + JointDoFs.Set(axis.Y, int(didx), int(JointAxisY)) + JointDoFs.Set(axis.Z, int(didx), int(JointAxisZ)) +} - // D6 is a generic 6-DoF joint. - D6 -) +func JointAxis(idx, dof int32) math32.Vector3 { + return JointAxisDoF(JointDoFIndex(idx, dof)) +} + +func SetJointAxis(idx, dof int32, axis math32.Vector3) { + SetJointAxisDoF(JointDoFIndex(idx, dof), axis) +} + +func JointDoF(idx, dof int32, vr JointDoFVars) float32 { + return JointDoFs.Value(int(JointDoFIndex(idx, dof)), int(vr)) +} + +func SetJointDoF(idx, dof int32, vr JointDoFVars, value float32) { + JointDoFs.Set(value, int(JointDoFIndex(idx, dof)), int(vr)) +} //gosl:end @@ -325,6 +354,11 @@ func (wl *World) JointDefaults(idx int32) { rot := math32.NewQuat(0, 0, 0, 1) SetJointPRot(idx, rot) SetJointCRot(idx, rot) - SetJointStiff(idx, math32.Vector3Scalar(1.0e4)) - SetJointDamp(idx, math32.Vector3Scalar(1.0)) +} + +func (wl *World) JointDoFDefaults(didx int32) { + JointDoFs.Set(-JointLimitUnlimited, int(didx), int(JointLimitLower)) + JointDoFs.Set(JointLimitUnlimited, int(didx), int(JointLimitUpper)) + JointDoFs.Set(1.0e4, int(didx), int(JointStiff)) + JointDoFs.Set(1.0e1, int(didx), int(JointDamp)) } diff --git a/physics/joint.goal b/physics/joint.goal index b38bb34a..55463d2d 100644 --- a/physics/joint.goal +++ b/physics/joint.goal @@ -12,6 +12,35 @@ import ( //gosl:start +// Sentinel value for unlimited joint limits +const JointLimitUnlimited = 1e10 + +// JointTypes are joint types that determine nature of interaction. +type JointTypes int32 //enums:enum + +const ( + // Prismatic allows translation along a single axis (slider): 1 DoF. + Prismatic JointTypes = iota + + // Revolute allows rotation about a single axis (axel): 1 DoF. + Revolute + + // Ball allows rotation about all three axes (3 DoF, quaternion). + Ball + + // Fixed locks all relative motion: 0 DoF. + Fixed + + // Free allows full 6-DoF motion (translation and rotation). + Free + + // Distance keeps two bodies a distance within joint limits: 6 DoF. + Distance + + // D6 is a generic 6-DoF joint. + D6 +) + // JointVars are joint state variables stored in tensor.Float32. // These are all static joint properties; dynamic control variables // in [JointControlVars] and [JointControls]. @@ -53,24 +82,17 @@ const ( JointCRotZ JointCRotW - // axis of articulation for the joint - JointAxisX - JointAxisY - JointAxisZ - - // joint limits - JointLimitLower - JointLimitUpper + // JointDoFN is the number of degrees-of-freedom for the joint. + JointDoFN - // joint stiffness target (ke) - JointStiffX - JointStiffY - JointStiffZ - - // joint damping target (kd) - JointDampX - JointDampY - JointDampZ + // indexes in JointDoFs for each DoF + JointDoF1 + JointDoF2 + JointDoF3 + // angular starts here for Free, D6 + JointDoF4 + JointDoF5 + JointDoF6 // Computed forces (temp storage until aggregated by bodies). @@ -139,6 +161,22 @@ func JointChildIndex(idx int32) int32 { return int32(math.Float32bits(Joints[idx, JointChild])) } +func SetJointDoFN(idx, dofN int32) { + Joints[idx, JointDoFN] = math.Float32frombits(uint32(dofN)) +} + +func GetJointDoFN(idx int32) int32 { + return int32(math.Float32bits(Joints[idx, JointDoFN])) +} + +func SetJointDoFIndex(idx, dof, dofIdx int32) { + Joints[idx, int32(JointDoF1) + dof] = math.Float32frombits(uint32(dofIdx)) +} + +func JointDoFIndex(idx, dof int32) int32 { + return int32(math.Float32bits(Joints[idx, int32(JointDoF1)+dof])) +} + func JointPPos(idx int32) math32.Vector3 { return math32.Vec3(Joints[idx, JointPPosX], Joints[idx, JointPPosY], Joints[idx, JointPPosZ]) } @@ -181,36 +219,6 @@ func SetJointCRot(idx int32, rot math32.Quat) { Joints[idx, JointCRotW] = rot.W } -func JointAxis(idx int32) math32.Vector3 { - return math32.Vec3(Joints[idx, JointAxisX], Joints[idx, JointAxisY], Joints[idx, JointAxisZ]) -} - -func SetJointAxis(idx int32, axis math32.Vector3) { - Joints[idx, JointAxisX] = axis.X - Joints[idx, JointAxisY] = axis.Y - Joints[idx, JointAxisZ] = axis.Z -} - -func JointStiff(idx int32) math32.Vector3 { - return math32.Vec3(Joints[idx, JointStiffX], Joints[idx, JointStiffY], Joints[idx, JointStiffZ]) -} - -func SetJointStiff(idx int32, stiff math32.Vector3) { - Joints[idx, JointStiffX] = stiff.X - Joints[idx, JointStiffY] = stiff.Y - Joints[idx, JointStiffZ] = stiff.Z -} - -func JointDamp(idx int32) math32.Vector3 { - return math32.Vec3(Joints[idx, JointDampX], Joints[idx, JointDampY], Joints[idx, JointDampZ]) -} - -func SetJointDamp(idx int32, damp math32.Vector3) { - Joints[idx, JointDampX] = damp.X - Joints[idx, JointDampY] = damp.Y - Joints[idx, JointDampZ] = damp.Z -} - func JointPForce(idx int32) math32.Vector3 { return math32.Vec3(Joints[idx, JointPForceX], Joints[idx, JointPForceY], Joints[idx, JointPForceZ]) } @@ -291,31 +299,54 @@ func SetJointCAngDelta(idx int32, t math32.Vector3) { Joints[idx, JointCAngDeltaZ] = t.Z } -// JointTypes are joint types that determine nature of interaction. -type JointTypes int32 //enums:enum +// JointDoFVars are joint DoF state variables stored in tensor.Float32, +// one for each DoF. +type JointDoFVars int32 //enums:enum const ( - // Prismatic allows translation along a single axis (slider): 1 DoF. - Prismatic JointTypes = iota + // axis of articulation for the DoF + JointAxisX JointDoFVars = iota + JointAxisY + JointAxisZ - // Revolute allows rotation about a single axis (axel): 1 DoF. - Revolute + // joint limits + JointLimitLower + JointLimitUpper - // Ball allows rotation about all three axes (3 DoF, quaternion). - Ball + // joint stiffness target (ke) + JointStiff - // Fixed locks all relative motion: 0 DoF. - Fixed + // joint damping target (kd) + JointDamp +) - // Free allows full 6-DoF motion (translation and rotation). - Free +func JointAxisDoF(didx int32) math32.Vector3 { + return math32.Vec3(JointDoFs[didx, JointAxisX], JointDoFs[didx, JointAxisY], JointDoFs[didx, JointAxisZ]) +} + +func SetJointAxisDoF(didx int32, axis math32.Vector3) { + JointDoFs[didx, JointAxisX] = axis.X + JointDoFs[didx, JointAxisY] = axis.Y + JointDoFs[didx, JointAxisZ] = axis.Z +} + +func JointAxis(idx, dof int32) math32.Vector3 { + return JointAxisDoF(JointDoFIndex(idx, dof)) +} + +func SetJointAxis(idx, dof int32, axis math32.Vector3) { + SetJointAxisDoF(JointDoFIndex(idx, dof), axis) +} + +func JointDoF(idx, dof int32, vr JointDoFVars) float32 { + return JointDoFs[JointDoFIndex(idx, dof), vr] +} + +func SetJointDoF(idx, dof int32, vr JointDoFVars, value float32) { + JointDoFs[JointDoFIndex(idx, dof), vr] = value +} - // Distance keeps two bodies a distance within joint limits: 6 DoF. - Distance - // D6 is a generic 6-DoF joint. - D6 -) //gosl:end @@ -323,6 +354,12 @@ func (wl *World) JointDefaults(idx int32) { rot := math32.NewQuat(0, 0, 0, 1) SetJointPRot(idx, rot) SetJointCRot(idx, rot) - SetJointStiff(idx, math32.Vector3Scalar(1.0e4)) - SetJointDamp(idx, math32.Vector3Scalar(1.0)) } + +func (wl *World) JointDoFDefaults(didx int32) { + JointDoFs[didx, JointLimitLower] = -JointLimitUnlimited + JointDoFs[didx, JointLimitUpper] = JointLimitUnlimited + JointDoFs[didx, JointStiff] = 1.0e4 + JointDoFs[didx, JointDamp] = 1.0e1 +} + diff --git a/physics/params.go b/physics/params.go index e0f18528..7a11e505 100644 --- a/physics/params.go +++ b/physics/params.go @@ -52,6 +52,9 @@ type PhysParams struct { // JointsN is number of joints. JointsN int32 `edit:"-"` + // JointDoFsN is number of joint DoFs. + JointDoFsN int32 `edit:"-"` + // BodyJointsMax is max number of joints per body + 1 for actual n. BodyJointsMax int32 `edit:"-"` @@ -61,6 +64,8 @@ type PhysParams struct { // Index for the next state (1 or 0, alternates with Cur). Next int32 `edit:"-"` + pad, pad1, pad2 int32 + // Gravity is the gravity acceleration function Gravity slvec.Vector3 } diff --git a/physics/shaders/DeltasFromJoints.wgsl b/physics/shaders/DeltasFromJoints.wgsl index 88c0662b..e725f94d 100644 --- a/physics/shaders/DeltasFromJoints.wgsl +++ b/physics/shaders/DeltasFromJoints.wgsl @@ -9,12 +9,12 @@ var Params: array; // // Bodies are the rigid body elements (dynamic and static), // specifying the constant, non-dynamic properties, // which is initial state for dynamics. // [body][BodyVarsN] @group(1) @binding(1) var Joints: array; -@group(1) @binding(2) +@group(1) @binding(3) var BodyJoints: array; // // Dynamics are the dynamic rigid body elements: these actually move. // Two alternating states are used: Params.Cur and Params.Next. // [dyn body][cur/next][DynamicVarsN] @group(2) @binding(0) var Dynamics: array; -// // JointControls are dynamic joint control inputs. // [joint][JointControlVarsN] +// // JointControls are dynamic joint control inputs, per joint DoF // (in correspondence with [JointDoFs]). This can be uploaded to the // GPU at every step. // [dof][JointControlVarsN] alias GPUVars = i32; @@ -89,25 +89,9 @@ const ContactDist: ContactVars = 8; //////// import: "control.go" alias JointControlVars = i32; //enums:enum -const JointCtrlForceX: JointControlVars = 0; -const JointCtrlForceY: JointControlVars = 1; -const JointCtrlForceZ: JointControlVars = 2; -const JointCtrlTorqueX: JointControlVars = 3; -const JointCtrlTorqueY: JointControlVars = 4; -const JointCtrlTorqueZ: JointControlVars = 5; -const JointTargetPosX: JointControlVars = 6; -const JointTargetPosY: JointControlVars = 7; -const JointTargetPosZ: JointControlVars = 8; -const JointTargetRotX: JointControlVars = 9; -const JointTargetRotY: JointControlVars = 10; -const JointTargetRotZ: JointControlVars = 11; -const JointTargetRotW: JointControlVars = 12; -const JointTargetVelX: JointControlVars = 13; -const JointTargetVelY: JointControlVars = 14; -const JointTargetVelZ: JointControlVars = 15; -const JointTargetAngVelX: JointControlVars = 16; -const JointTargetAngVelY: JointControlVars = 17; -const JointTargetAngVelZ: JointControlVars = 18; +const JointControlForce: JointControlVars = 0; +const JointTargetPos: JointControlVars = 1; +const JointTargetVel: JointControlVars = 2; //////// import: "dynamics.go" alias DynamicVars = i32; //enums:enum @@ -144,27 +128,37 @@ const DynAngDeltaX: DynamicVars = 29; const DynAngDeltaY: DynamicVars = 30; const DynAngDeltaZ: DynamicVars = 31; fn SetDynamicDelta(idx: i32,cni: i32, delta: vec3) { - Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynDeltaX))] = delta.x; - Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynDeltaY))] = delta.y; - Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynDeltaZ))] = delta.z; + Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynDeltaX))] = delta.x; + Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynDeltaY))] = delta.y; + Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynDeltaZ))] = delta.z; } fn SetDynamicAngDelta(idx: i32,cni: i32, angDelta: vec3) { - Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynAngDeltaX))] = angDelta.x; - Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynAngDeltaY))] = angDelta.y; - Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynAngDeltaZ))] = angDelta.z; + Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynAngDeltaX))] = angDelta.x; + Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynAngDeltaY))] = angDelta.y; + Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynAngDeltaZ))] = angDelta.z; } //////// import: "enumgen.go" const BodyVarsN: BodyVars = 37; const ContactVarsN: ContactVars = 9; -const JointControlVarsN: JointControlVars = 19; +const JointControlVarsN: JointControlVars = 3; const DynamicVarsN: DynamicVars = 32; -const GPUVarsN: GPUVars = 7; -const JointVarsN: JointVars = 53; +const GPUVarsN: GPUVars = 8; const JointTypesN: JointTypes = 7; +const JointVarsN: JointVars = 49; +const JointDoFVarsN: JointDoFVars = 7; const ShapesN: Shapes = 4; //////// import: "joint.go" +const JointLimitUnlimited = 1e10; +alias JointTypes = i32; //enums:enum +const Prismatic: JointTypes = 0; +const Revolute: JointTypes = 1; +const Ball: JointTypes = 2; +const Fixed: JointTypes = 3; +const Free: JointTypes = 4; +const Distance: JointTypes = 5; +const D6: JointTypes = 6; alias JointVars = i32; //enums:enum const JointType: JointVars = 0; const JointEnabled: JointVars = 1; @@ -184,41 +178,37 @@ const JointCRotX: JointVars = 14; const JointCRotY: JointVars = 15; const JointCRotZ: JointVars = 16; const JointCRotW: JointVars = 17; -const JointAxisX: JointVars = 18; -const JointAxisY: JointVars = 19; -const JointAxisZ: JointVars = 20; -const JointLimitLower: JointVars = 21; -const JointLimitUpper: JointVars = 22; -const JointStiffX: JointVars = 23; -const JointStiffY: JointVars = 24; -const JointStiffZ: JointVars = 25; -const JointDampX: JointVars = 26; -const JointDampY: JointVars = 27; -const JointDampZ: JointVars = 28; -const JointPForceX: JointVars = 29; -const JointPForceY: JointVars = 30; -const JointPForceZ: JointVars = 31; -const JointPTorqueX: JointVars = 32; -const JointPTorqueY: JointVars = 33; -const JointPTorqueZ: JointVars = 34; -const JointCForceX: JointVars = 35; -const JointCForceY: JointVars = 36; -const JointCForceZ: JointVars = 37; -const JointCTorqueX: JointVars = 38; -const JointCTorqueY: JointVars = 39; -const JointCTorqueZ: JointVars = 40; -const JointPDeltaX: JointVars = 41; -const JointPDeltaY: JointVars = 42; -const JointPDeltaZ: JointVars = 43; -const JointPAngDeltaX: JointVars = 44; -const JointPAngDeltaY: JointVars = 45; -const JointPAngDeltaZ: JointVars = 46; -const JointCDeltaX: JointVars = 47; -const JointCDeltaY: JointVars = 48; -const JointCDeltaZ: JointVars = 49; -const JointCAngDeltaX: JointVars = 50; -const JointCAngDeltaY: JointVars = 51; -const JointCAngDeltaZ: JointVars = 52; +const JointDoFN: JointVars = 18; +const JointDoF1: JointVars = 19; +const JointDoF2: JointVars = 20; +const JointDoF3: JointVars = 21; +const JointDoF4: JointVars = 22; +const JointDoF5: JointVars = 23; +const JointDoF6: JointVars = 24; +const JointPForceX: JointVars = 25; +const JointPForceY: JointVars = 26; +const JointPForceZ: JointVars = 27; +const JointPTorqueX: JointVars = 28; +const JointPTorqueY: JointVars = 29; +const JointPTorqueZ: JointVars = 30; +const JointCForceX: JointVars = 31; +const JointCForceY: JointVars = 32; +const JointCForceZ: JointVars = 33; +const JointCTorqueX: JointVars = 34; +const JointCTorqueY: JointVars = 35; +const JointCTorqueZ: JointVars = 36; +const JointPDeltaX: JointVars = 37; +const JointPDeltaY: JointVars = 38; +const JointPDeltaZ: JointVars = 39; +const JointPAngDeltaX: JointVars = 40; +const JointPAngDeltaY: JointVars = 41; +const JointPAngDeltaZ: JointVars = 42; +const JointCDeltaX: JointVars = 43; +const JointCDeltaY: JointVars = 44; +const JointCDeltaZ: JointVars = 45; +const JointCAngDeltaX: JointVars = 46; +const JointCAngDeltaY: JointVars = 47; +const JointCAngDeltaZ: JointVars = 48; fn JointPDelta(idx: i32) -> vec3 { return vec3(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPDeltaX))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPDeltaY))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPDeltaZ))]); } @@ -231,14 +221,14 @@ fn JointCDelta(idx: i32) -> vec3 { fn JointCAngDelta(idx: i32) -> vec3 { return vec3(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCAngDeltaX))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCAngDeltaY))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCAngDeltaZ))]); } -alias JointTypes = i32; //enums:enum -const Prismatic: JointTypes = 0; -const Revolute: JointTypes = 1; -const Ball: JointTypes = 2; -const Fixed: JointTypes = 3; -const Free: JointTypes = 4; -const Distance: JointTypes = 5; -const D6: JointTypes = 6; +alias JointDoFVars = i32; //enums:enum +const JointAxisX: JointDoFVars = 0; +const JointAxisY: JointDoFVars = 1; +const JointAxisZ: JointDoFVars = 2; +const JointLimitLower: JointDoFVars = 3; +const JointLimitUpper: JointDoFVars = 4; +const JointStiff: JointDoFVars = 5; +const JointDamp: JointDoFVars = 6; //////// import: "params.go" struct PhysParams { @@ -255,9 +245,13 @@ struct PhysParams { Restitution: i32, DynamicsN: i32, JointsN: i32, + JointDoFsN: i32, BodyJointsMax: i32, Cur: i32, Next: i32, + pad: i32, + pad1: i32, + pad2: i32, Gravity: vec4, } @@ -283,19 +277,19 @@ fn DeltasFromJoints(i: u32) { //gosl:kernel if (di >= params.DynamicsN) { return; } - var np = BodyJoints[Index3D(TensorStrides[20], TensorStrides[21], TensorStrides[22], u32(di), u32(0), u32(0))]; - var nc = BodyJoints[Index3D(TensorStrides[20], TensorStrides[21], TensorStrides[22], u32(di), u32(1), u32(0))]; + var np = BodyJoints[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(di), u32(0), u32(0))]; + var nc = BodyJoints[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(di), u32(1), u32(0))]; var td = vec3(0, 0, 0); var ta = vec3(0, 0, 0); for (var i = i32(1); i <= np; i++) { - var ji = BodyJoints[Index3D(TensorStrides[20], TensorStrides[21], TensorStrides[22], u32(di), u32(0), u32(i))]; + var ji = BodyJoints[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(di), u32(0), u32(i))]; var d = JointPDelta(ji); td = td+(d); var a = JointPAngDelta(ji); ta = ta+(a); } for (var i = i32(1); i <= nc; i++) { - var ji = BodyJoints[Index3D(TensorStrides[20], TensorStrides[21], TensorStrides[22], u32(di), u32(1), u32(i))]; + var ji = BodyJoints[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(di), u32(1), u32(i))]; var d = JointCDelta(ji); td = td+(d); var a = JointCAngDelta(ji); diff --git a/physics/shaders/DynamicsCurToNext.wgsl b/physics/shaders/DynamicsCurToNext.wgsl index 5a76488e..c86b1309 100644 --- a/physics/shaders/DynamicsCurToNext.wgsl +++ b/physics/shaders/DynamicsCurToNext.wgsl @@ -10,7 +10,7 @@ var Params: array; // // Dynamics are the dynamic rigid body elements: these actually move. // Two alternating states are used: Params.Cur and Params.Next. // [dyn body][cur/next][DynamicVarsN] @group(2) @binding(0) var Dynamics: array; -// // JointControls are dynamic joint control inputs. // [joint][JointControlVarsN] +// // JointControls are dynamic joint control inputs, per joint DoF // (in correspondence with [JointDoFs]). This can be uploaded to the // GPU at every step. // [dof][JointControlVarsN] alias GPUVars = i32; @@ -81,25 +81,9 @@ const ContactDist: ContactVars = 8; //////// import: "control.go" alias JointControlVars = i32; //enums:enum -const JointCtrlForceX: JointControlVars = 0; -const JointCtrlForceY: JointControlVars = 1; -const JointCtrlForceZ: JointControlVars = 2; -const JointCtrlTorqueX: JointControlVars = 3; -const JointCtrlTorqueY: JointControlVars = 4; -const JointCtrlTorqueZ: JointControlVars = 5; -const JointTargetPosX: JointControlVars = 6; -const JointTargetPosY: JointControlVars = 7; -const JointTargetPosZ: JointControlVars = 8; -const JointTargetRotX: JointControlVars = 9; -const JointTargetRotY: JointControlVars = 10; -const JointTargetRotZ: JointControlVars = 11; -const JointTargetRotW: JointControlVars = 12; -const JointTargetVelX: JointControlVars = 13; -const JointTargetVelY: JointControlVars = 14; -const JointTargetVelZ: JointControlVars = 15; -const JointTargetAngVelX: JointControlVars = 16; -const JointTargetAngVelY: JointControlVars = 17; -const JointTargetAngVelZ: JointControlVars = 18; +const JointControlForce: JointControlVars = 0; +const JointTargetPos: JointControlVars = 1; +const JointTargetVel: JointControlVars = 2; //////// import: "dynamics.go" alias DynamicVars = i32; //enums:enum @@ -139,14 +123,24 @@ const DynAngDeltaZ: DynamicVars = 31; //////// import: "enumgen.go" const BodyVarsN: BodyVars = 37; const ContactVarsN: ContactVars = 9; -const JointControlVarsN: JointControlVars = 19; +const JointControlVarsN: JointControlVars = 3; const DynamicVarsN: DynamicVars = 32; -const GPUVarsN: GPUVars = 7; -const JointVarsN: JointVars = 53; +const GPUVarsN: GPUVars = 8; const JointTypesN: JointTypes = 7; +const JointVarsN: JointVars = 49; +const JointDoFVarsN: JointDoFVars = 7; const ShapesN: Shapes = 4; //////// import: "joint.go" +const JointLimitUnlimited = 1e10; +alias JointTypes = i32; //enums:enum +const Prismatic: JointTypes = 0; +const Revolute: JointTypes = 1; +const Ball: JointTypes = 2; +const Fixed: JointTypes = 3; +const Free: JointTypes = 4; +const Distance: JointTypes = 5; +const D6: JointTypes = 6; alias JointVars = i32; //enums:enum const JointType: JointVars = 0; const JointEnabled: JointVars = 1; @@ -166,49 +160,45 @@ const JointCRotX: JointVars = 14; const JointCRotY: JointVars = 15; const JointCRotZ: JointVars = 16; const JointCRotW: JointVars = 17; -const JointAxisX: JointVars = 18; -const JointAxisY: JointVars = 19; -const JointAxisZ: JointVars = 20; -const JointLimitLower: JointVars = 21; -const JointLimitUpper: JointVars = 22; -const JointStiffX: JointVars = 23; -const JointStiffY: JointVars = 24; -const JointStiffZ: JointVars = 25; -const JointDampX: JointVars = 26; -const JointDampY: JointVars = 27; -const JointDampZ: JointVars = 28; -const JointPForceX: JointVars = 29; -const JointPForceY: JointVars = 30; -const JointPForceZ: JointVars = 31; -const JointPTorqueX: JointVars = 32; -const JointPTorqueY: JointVars = 33; -const JointPTorqueZ: JointVars = 34; -const JointCForceX: JointVars = 35; -const JointCForceY: JointVars = 36; -const JointCForceZ: JointVars = 37; -const JointCTorqueX: JointVars = 38; -const JointCTorqueY: JointVars = 39; -const JointCTorqueZ: JointVars = 40; -const JointPDeltaX: JointVars = 41; -const JointPDeltaY: JointVars = 42; -const JointPDeltaZ: JointVars = 43; -const JointPAngDeltaX: JointVars = 44; -const JointPAngDeltaY: JointVars = 45; -const JointPAngDeltaZ: JointVars = 46; -const JointCDeltaX: JointVars = 47; -const JointCDeltaY: JointVars = 48; -const JointCDeltaZ: JointVars = 49; -const JointCAngDeltaX: JointVars = 50; -const JointCAngDeltaY: JointVars = 51; -const JointCAngDeltaZ: JointVars = 52; -alias JointTypes = i32; //enums:enum -const Prismatic: JointTypes = 0; -const Revolute: JointTypes = 1; -const Ball: JointTypes = 2; -const Fixed: JointTypes = 3; -const Free: JointTypes = 4; -const Distance: JointTypes = 5; -const D6: JointTypes = 6; +const JointDoFN: JointVars = 18; +const JointDoF1: JointVars = 19; +const JointDoF2: JointVars = 20; +const JointDoF3: JointVars = 21; +const JointDoF4: JointVars = 22; +const JointDoF5: JointVars = 23; +const JointDoF6: JointVars = 24; +const JointPForceX: JointVars = 25; +const JointPForceY: JointVars = 26; +const JointPForceZ: JointVars = 27; +const JointPTorqueX: JointVars = 28; +const JointPTorqueY: JointVars = 29; +const JointPTorqueZ: JointVars = 30; +const JointCForceX: JointVars = 31; +const JointCForceY: JointVars = 32; +const JointCForceZ: JointVars = 33; +const JointCTorqueX: JointVars = 34; +const JointCTorqueY: JointVars = 35; +const JointCTorqueZ: JointVars = 36; +const JointPDeltaX: JointVars = 37; +const JointPDeltaY: JointVars = 38; +const JointPDeltaZ: JointVars = 39; +const JointPAngDeltaX: JointVars = 40; +const JointPAngDeltaY: JointVars = 41; +const JointPAngDeltaZ: JointVars = 42; +const JointCDeltaX: JointVars = 43; +const JointCDeltaY: JointVars = 44; +const JointCDeltaZ: JointVars = 45; +const JointCAngDeltaX: JointVars = 46; +const JointCAngDeltaY: JointVars = 47; +const JointCAngDeltaZ: JointVars = 48; +alias JointDoFVars = i32; //enums:enum +const JointAxisX: JointDoFVars = 0; +const JointAxisY: JointDoFVars = 1; +const JointAxisZ: JointDoFVars = 2; +const JointLimitLower: JointDoFVars = 3; +const JointLimitUpper: JointDoFVars = 4; +const JointStiff: JointDoFVars = 5; +const JointDamp: JointDoFVars = 6; //////// import: "params.go" struct PhysParams { @@ -225,9 +215,13 @@ struct PhysParams { Restitution: i32, DynamicsN: i32, JointsN: i32, + JointDoFsN: i32, BodyJointsMax: i32, Cur: i32, Next: i32, + pad: i32, + pad1: i32, + pad2: i32, Gravity: vec4, } @@ -254,7 +248,7 @@ fn DynamicsCurToNext(i: u32) { //gosl:kernel return; } for (var di = DynIndex; di < DynamicVarsN; di++) { - Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(ii), u32(params.Next), u32(di))] = Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(ii), u32(params.Cur), u32(di))]; + Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(ii), u32(params.Next), u32(di))] = Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(ii), u32(params.Cur), u32(di))]; } } diff --git a/physics/shaders/ForcesFromJoints.wgsl b/physics/shaders/ForcesFromJoints.wgsl index da885296..cbfb8e7a 100644 --- a/physics/shaders/ForcesFromJoints.wgsl +++ b/physics/shaders/ForcesFromJoints.wgsl @@ -9,12 +9,12 @@ var Params: array; // // Bodies are the rigid body elements (dynamic and static), // specifying the constant, non-dynamic properties, // which is initial state for dynamics. // [body][BodyVarsN] @group(1) @binding(1) var Joints: array; -@group(1) @binding(2) +@group(1) @binding(3) var BodyJoints: array; // // Dynamics are the dynamic rigid body elements: these actually move. // Two alternating states are used: Params.Cur and Params.Next. // [dyn body][cur/next][DynamicVarsN] @group(2) @binding(0) var Dynamics: array; -// // JointControls are dynamic joint control inputs. // [joint][JointControlVarsN] +// // JointControls are dynamic joint control inputs, per joint DoF // (in correspondence with [JointDoFs]). This can be uploaded to the // GPU at every step. // [dof][JointControlVarsN] alias GPUVars = i32; @@ -89,25 +89,9 @@ const ContactDist: ContactVars = 8; //////// import: "control.go" alias JointControlVars = i32; //enums:enum -const JointCtrlForceX: JointControlVars = 0; -const JointCtrlForceY: JointControlVars = 1; -const JointCtrlForceZ: JointControlVars = 2; -const JointCtrlTorqueX: JointControlVars = 3; -const JointCtrlTorqueY: JointControlVars = 4; -const JointCtrlTorqueZ: JointControlVars = 5; -const JointTargetPosX: JointControlVars = 6; -const JointTargetPosY: JointControlVars = 7; -const JointTargetPosZ: JointControlVars = 8; -const JointTargetRotX: JointControlVars = 9; -const JointTargetRotY: JointControlVars = 10; -const JointTargetRotZ: JointControlVars = 11; -const JointTargetRotW: JointControlVars = 12; -const JointTargetVelX: JointControlVars = 13; -const JointTargetVelY: JointControlVars = 14; -const JointTargetVelZ: JointControlVars = 15; -const JointTargetAngVelX: JointControlVars = 16; -const JointTargetAngVelY: JointControlVars = 17; -const JointTargetAngVelZ: JointControlVars = 18; +const JointControlForce: JointControlVars = 0; +const JointTargetPos: JointControlVars = 1; +const JointTargetVel: JointControlVars = 2; //////// import: "dynamics.go" alias DynamicVars = i32; //enums:enum @@ -144,27 +128,37 @@ const DynAngDeltaX: DynamicVars = 29; const DynAngDeltaY: DynamicVars = 30; const DynAngDeltaZ: DynamicVars = 31; fn SetDynamicForce(idx: i32,cni: i32, force: vec3) { - Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynForceX))] = force.x; - Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynForceY))] = force.y; - Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynForceZ))] = force.z; + Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynForceX))] = force.x; + Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynForceY))] = force.y; + Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynForceZ))] = force.z; } fn SetDynamicTorque(idx: i32,cni: i32, torque: vec3) { - Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynTorqueX))] = torque.x; - Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynTorqueY))] = torque.y; - Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynTorqueZ))] = torque.z; + Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynTorqueX))] = torque.x; + Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynTorqueY))] = torque.y; + Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynTorqueZ))] = torque.z; } //////// import: "enumgen.go" const BodyVarsN: BodyVars = 37; const ContactVarsN: ContactVars = 9; -const JointControlVarsN: JointControlVars = 19; +const JointControlVarsN: JointControlVars = 3; const DynamicVarsN: DynamicVars = 32; -const GPUVarsN: GPUVars = 7; -const JointVarsN: JointVars = 53; +const GPUVarsN: GPUVars = 8; const JointTypesN: JointTypes = 7; +const JointVarsN: JointVars = 49; +const JointDoFVarsN: JointDoFVars = 7; const ShapesN: Shapes = 4; //////// import: "joint.go" +const JointLimitUnlimited = 1e10; +alias JointTypes = i32; //enums:enum +const Prismatic: JointTypes = 0; +const Revolute: JointTypes = 1; +const Ball: JointTypes = 2; +const Fixed: JointTypes = 3; +const Free: JointTypes = 4; +const Distance: JointTypes = 5; +const D6: JointTypes = 6; alias JointVars = i32; //enums:enum const JointType: JointVars = 0; const JointEnabled: JointVars = 1; @@ -184,41 +178,37 @@ const JointCRotX: JointVars = 14; const JointCRotY: JointVars = 15; const JointCRotZ: JointVars = 16; const JointCRotW: JointVars = 17; -const JointAxisX: JointVars = 18; -const JointAxisY: JointVars = 19; -const JointAxisZ: JointVars = 20; -const JointLimitLower: JointVars = 21; -const JointLimitUpper: JointVars = 22; -const JointStiffX: JointVars = 23; -const JointStiffY: JointVars = 24; -const JointStiffZ: JointVars = 25; -const JointDampX: JointVars = 26; -const JointDampY: JointVars = 27; -const JointDampZ: JointVars = 28; -const JointPForceX: JointVars = 29; -const JointPForceY: JointVars = 30; -const JointPForceZ: JointVars = 31; -const JointPTorqueX: JointVars = 32; -const JointPTorqueY: JointVars = 33; -const JointPTorqueZ: JointVars = 34; -const JointCForceX: JointVars = 35; -const JointCForceY: JointVars = 36; -const JointCForceZ: JointVars = 37; -const JointCTorqueX: JointVars = 38; -const JointCTorqueY: JointVars = 39; -const JointCTorqueZ: JointVars = 40; -const JointPDeltaX: JointVars = 41; -const JointPDeltaY: JointVars = 42; -const JointPDeltaZ: JointVars = 43; -const JointPAngDeltaX: JointVars = 44; -const JointPAngDeltaY: JointVars = 45; -const JointPAngDeltaZ: JointVars = 46; -const JointCDeltaX: JointVars = 47; -const JointCDeltaY: JointVars = 48; -const JointCDeltaZ: JointVars = 49; -const JointCAngDeltaX: JointVars = 50; -const JointCAngDeltaY: JointVars = 51; -const JointCAngDeltaZ: JointVars = 52; +const JointDoFN: JointVars = 18; +const JointDoF1: JointVars = 19; +const JointDoF2: JointVars = 20; +const JointDoF3: JointVars = 21; +const JointDoF4: JointVars = 22; +const JointDoF5: JointVars = 23; +const JointDoF6: JointVars = 24; +const JointPForceX: JointVars = 25; +const JointPForceY: JointVars = 26; +const JointPForceZ: JointVars = 27; +const JointPTorqueX: JointVars = 28; +const JointPTorqueY: JointVars = 29; +const JointPTorqueZ: JointVars = 30; +const JointCForceX: JointVars = 31; +const JointCForceY: JointVars = 32; +const JointCForceZ: JointVars = 33; +const JointCTorqueX: JointVars = 34; +const JointCTorqueY: JointVars = 35; +const JointCTorqueZ: JointVars = 36; +const JointPDeltaX: JointVars = 37; +const JointPDeltaY: JointVars = 38; +const JointPDeltaZ: JointVars = 39; +const JointPAngDeltaX: JointVars = 40; +const JointPAngDeltaY: JointVars = 41; +const JointPAngDeltaZ: JointVars = 42; +const JointCDeltaX: JointVars = 43; +const JointCDeltaY: JointVars = 44; +const JointCDeltaZ: JointVars = 45; +const JointCAngDeltaX: JointVars = 46; +const JointCAngDeltaY: JointVars = 47; +const JointCAngDeltaZ: JointVars = 48; fn JointPForce(idx: i32) -> vec3 { return vec3(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPForceX))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPForceY))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPForceZ))]); } @@ -231,14 +221,14 @@ fn JointCForce(idx: i32) -> vec3 { fn JointCTorque(idx: i32) -> vec3 { return vec3(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCTorqueX))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCTorqueY))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCTorqueZ))]); } -alias JointTypes = i32; //enums:enum -const Prismatic: JointTypes = 0; -const Revolute: JointTypes = 1; -const Ball: JointTypes = 2; -const Fixed: JointTypes = 3; -const Free: JointTypes = 4; -const Distance: JointTypes = 5; -const D6: JointTypes = 6; +alias JointDoFVars = i32; //enums:enum +const JointAxisX: JointDoFVars = 0; +const JointAxisY: JointDoFVars = 1; +const JointAxisZ: JointDoFVars = 2; +const JointLimitLower: JointDoFVars = 3; +const JointLimitUpper: JointDoFVars = 4; +const JointStiff: JointDoFVars = 5; +const JointDamp: JointDoFVars = 6; //////// import: "params.go" struct PhysParams { @@ -255,9 +245,13 @@ struct PhysParams { Restitution: i32, DynamicsN: i32, JointsN: i32, + JointDoFsN: i32, BodyJointsMax: i32, Cur: i32, Next: i32, + pad: i32, + pad1: i32, + pad2: i32, Gravity: vec4, } @@ -283,19 +277,19 @@ fn ForcesFromJoints(i: u32) { //gosl:kernel if (di >= params.DynamicsN) { return; } - var np = BodyJoints[Index3D(TensorStrides[20], TensorStrides[21], TensorStrides[22], u32(di), u32(0), u32(0))]; - var nc = BodyJoints[Index3D(TensorStrides[20], TensorStrides[21], TensorStrides[22], u32(di), u32(1), u32(0))]; + var np = BodyJoints[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(di), u32(0), u32(0))]; + var nc = BodyJoints[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(di), u32(1), u32(0))]; var tf = vec3(0, 0, 0); var tt = vec3(0, 0, 0); for (var i = i32(1); i <= np; i++) { - var ji = BodyJoints[Index3D(TensorStrides[20], TensorStrides[21], TensorStrides[22], u32(di), u32(0), u32(i))]; + var ji = BodyJoints[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(di), u32(0), u32(i))]; var f = JointPForce(ji); tf = tf+(f); var t = JointPTorque(ji); tt = tt+(t); } for (var i = i32(1); i <= nc; i++) { - var ji = BodyJoints[Index3D(TensorStrides[20], TensorStrides[21], TensorStrides[22], u32(di), u32(1), u32(i))]; + var ji = BodyJoints[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(di), u32(1), u32(i))]; var f = JointCForce(ji); tf = tf+(f); var t = JointCTorque(ji); diff --git a/physics/shaders/InitDynamics.wgsl b/physics/shaders/InitDynamics.wgsl index 5973078d..65d875a8 100644 --- a/physics/shaders/InitDynamics.wgsl +++ b/physics/shaders/InitDynamics.wgsl @@ -12,7 +12,7 @@ var Bodies: array; // // Dynamics are the dynamic rigid body elements: these actually move. // Two alternating states are used: Params.Cur and Params.Next. // [dyn body][cur/next][DynamicVarsN] @group(2) @binding(0) var Dynamics: array; -// // JointControls are dynamic joint control inputs. // [joint][JointControlVarsN] +// // JointControls are dynamic joint control inputs, per joint DoF // (in correspondence with [JointDoFs]). This can be uploaded to the // GPU at every step. // [dof][JointControlVarsN] alias GPUVars = i32; @@ -87,25 +87,9 @@ const ContactDist: ContactVars = 8; //////// import: "control.go" alias JointControlVars = i32; //enums:enum -const JointCtrlForceX: JointControlVars = 0; -const JointCtrlForceY: JointControlVars = 1; -const JointCtrlForceZ: JointControlVars = 2; -const JointCtrlTorqueX: JointControlVars = 3; -const JointCtrlTorqueY: JointControlVars = 4; -const JointCtrlTorqueZ: JointControlVars = 5; -const JointTargetPosX: JointControlVars = 6; -const JointTargetPosY: JointControlVars = 7; -const JointTargetPosZ: JointControlVars = 8; -const JointTargetRotX: JointControlVars = 9; -const JointTargetRotY: JointControlVars = 10; -const JointTargetRotZ: JointControlVars = 11; -const JointTargetRotW: JointControlVars = 12; -const JointTargetVelX: JointControlVars = 13; -const JointTargetVelY: JointControlVars = 14; -const JointTargetVelZ: JointControlVars = 15; -const JointTargetAngVelX: JointControlVars = 16; -const JointTargetAngVelY: JointControlVars = 17; -const JointTargetAngVelZ: JointControlVars = 18; +const JointControlForce: JointControlVars = 0; +const JointTargetPos: JointControlVars = 1; +const JointTargetVel: JointControlVars = 2; //////// import: "dynamics.go" alias DynamicVars = i32; //enums:enum @@ -142,20 +126,30 @@ const DynAngDeltaX: DynamicVars = 29; const DynAngDeltaY: DynamicVars = 30; const DynAngDeltaZ: DynamicVars = 31; fn DynamicIndex(idx: i32,cni: i32) -> i32 { - return i32(bitcast(Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynIndex))])); + return i32(bitcast(Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynIndex))])); } //////// import: "enumgen.go" const BodyVarsN: BodyVars = 37; const ContactVarsN: ContactVars = 9; -const JointControlVarsN: JointControlVars = 19; +const JointControlVarsN: JointControlVars = 3; const DynamicVarsN: DynamicVars = 32; -const GPUVarsN: GPUVars = 7; -const JointVarsN: JointVars = 53; +const GPUVarsN: GPUVars = 8; const JointTypesN: JointTypes = 7; +const JointVarsN: JointVars = 49; +const JointDoFVarsN: JointDoFVars = 7; const ShapesN: Shapes = 4; //////// import: "joint.go" +const JointLimitUnlimited = 1e10; +alias JointTypes = i32; //enums:enum +const Prismatic: JointTypes = 0; +const Revolute: JointTypes = 1; +const Ball: JointTypes = 2; +const Fixed: JointTypes = 3; +const Free: JointTypes = 4; +const Distance: JointTypes = 5; +const D6: JointTypes = 6; alias JointVars = i32; //enums:enum const JointType: JointVars = 0; const JointEnabled: JointVars = 1; @@ -175,49 +169,45 @@ const JointCRotX: JointVars = 14; const JointCRotY: JointVars = 15; const JointCRotZ: JointVars = 16; const JointCRotW: JointVars = 17; -const JointAxisX: JointVars = 18; -const JointAxisY: JointVars = 19; -const JointAxisZ: JointVars = 20; -const JointLimitLower: JointVars = 21; -const JointLimitUpper: JointVars = 22; -const JointStiffX: JointVars = 23; -const JointStiffY: JointVars = 24; -const JointStiffZ: JointVars = 25; -const JointDampX: JointVars = 26; -const JointDampY: JointVars = 27; -const JointDampZ: JointVars = 28; -const JointPForceX: JointVars = 29; -const JointPForceY: JointVars = 30; -const JointPForceZ: JointVars = 31; -const JointPTorqueX: JointVars = 32; -const JointPTorqueY: JointVars = 33; -const JointPTorqueZ: JointVars = 34; -const JointCForceX: JointVars = 35; -const JointCForceY: JointVars = 36; -const JointCForceZ: JointVars = 37; -const JointCTorqueX: JointVars = 38; -const JointCTorqueY: JointVars = 39; -const JointCTorqueZ: JointVars = 40; -const JointPDeltaX: JointVars = 41; -const JointPDeltaY: JointVars = 42; -const JointPDeltaZ: JointVars = 43; -const JointPAngDeltaX: JointVars = 44; -const JointPAngDeltaY: JointVars = 45; -const JointPAngDeltaZ: JointVars = 46; -const JointCDeltaX: JointVars = 47; -const JointCDeltaY: JointVars = 48; -const JointCDeltaZ: JointVars = 49; -const JointCAngDeltaX: JointVars = 50; -const JointCAngDeltaY: JointVars = 51; -const JointCAngDeltaZ: JointVars = 52; -alias JointTypes = i32; //enums:enum -const Prismatic: JointTypes = 0; -const Revolute: JointTypes = 1; -const Ball: JointTypes = 2; -const Fixed: JointTypes = 3; -const Free: JointTypes = 4; -const Distance: JointTypes = 5; -const D6: JointTypes = 6; +const JointDoFN: JointVars = 18; +const JointDoF1: JointVars = 19; +const JointDoF2: JointVars = 20; +const JointDoF3: JointVars = 21; +const JointDoF4: JointVars = 22; +const JointDoF5: JointVars = 23; +const JointDoF6: JointVars = 24; +const JointPForceX: JointVars = 25; +const JointPForceY: JointVars = 26; +const JointPForceZ: JointVars = 27; +const JointPTorqueX: JointVars = 28; +const JointPTorqueY: JointVars = 29; +const JointPTorqueZ: JointVars = 30; +const JointCForceX: JointVars = 31; +const JointCForceY: JointVars = 32; +const JointCForceZ: JointVars = 33; +const JointCTorqueX: JointVars = 34; +const JointCTorqueY: JointVars = 35; +const JointCTorqueZ: JointVars = 36; +const JointPDeltaX: JointVars = 37; +const JointPDeltaY: JointVars = 38; +const JointPDeltaZ: JointVars = 39; +const JointPAngDeltaX: JointVars = 40; +const JointPAngDeltaY: JointVars = 41; +const JointPAngDeltaZ: JointVars = 42; +const JointCDeltaX: JointVars = 43; +const JointCDeltaY: JointVars = 44; +const JointCDeltaZ: JointVars = 45; +const JointCAngDeltaX: JointVars = 46; +const JointCAngDeltaY: JointVars = 47; +const JointCAngDeltaZ: JointVars = 48; +alias JointDoFVars = i32; //enums:enum +const JointAxisX: JointDoFVars = 0; +const JointAxisY: JointDoFVars = 1; +const JointAxisZ: JointDoFVars = 2; +const JointLimitLower: JointDoFVars = 3; +const JointLimitUpper: JointDoFVars = 4; +const JointStiff: JointDoFVars = 5; +const JointDamp: JointDoFVars = 6; //////// import: "params.go" struct PhysParams { @@ -234,9 +224,13 @@ struct PhysParams { Restitution: i32, DynamicsN: i32, JointsN: i32, + JointDoFsN: i32, BodyJointsMax: i32, Cur: i32, Next: i32, + pad: i32, + pad1: i32, + pad2: i32, Gravity: vec4, } @@ -264,15 +258,15 @@ fn InitDynamics(i: u32) { //gosl:kernel } for (var cni=0; cni<2; cni++) { var bi = DynamicIndex(ii, i32(cni)); - Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(ii), u32(cni), u32(DynPosX))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyPosX))]; - Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(ii), u32(cni), u32(DynPosY))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyPosY))]; - Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(ii), u32(cni), u32(DynPosZ))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyPosZ))]; - Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(ii), u32(cni), u32(DynRotX))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyRotX))]; - Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(ii), u32(cni), u32(DynRotY))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyRotY))]; - Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(ii), u32(cni), u32(DynRotZ))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyRotZ))]; - Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(ii), u32(cni), u32(DynRotW))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyRotW))]; + Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(ii), u32(cni), u32(DynPosX))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyPosX))]; + Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(ii), u32(cni), u32(DynPosY))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyPosY))]; + Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(ii), u32(cni), u32(DynPosZ))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyPosZ))]; + Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(ii), u32(cni), u32(DynRotX))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyRotX))]; + Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(ii), u32(cni), u32(DynRotY))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyRotY))]; + Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(ii), u32(cni), u32(DynRotZ))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyRotZ))]; + Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(ii), u32(cni), u32(DynRotW))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyRotW))]; for (var v = DynVelX; v < DynamicVarsN; v++) { - Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], + Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(ii), u32(cni), u32(v))] = 0.0; } } diff --git a/physics/shaders/StepBodyDeltas.wgsl b/physics/shaders/StepBodyDeltas.wgsl index 083112d7..438da3af 100644 --- a/physics/shaders/StepBodyDeltas.wgsl +++ b/physics/shaders/StepBodyDeltas.wgsl @@ -12,7 +12,7 @@ var Bodies: array; // // Dynamics are the dynamic rigid body elements: these actually move. // Two alternating states are used: Params.Cur and Params.Next. // [dyn body][cur/next][DynamicVarsN] @group(2) @binding(0) var Dynamics: array; -// // JointControls are dynamic joint control inputs. // [joint][JointControlVarsN] +// // JointControls are dynamic joint control inputs, per joint DoF // (in correspondence with [JointDoFs]). This can be uploaded to the // GPU at every step. // [dof][JointControlVarsN] alias GPUVars = i32; @@ -100,25 +100,9 @@ const ContactDist: ContactVars = 8; //////// import: "control.go" alias JointControlVars = i32; //enums:enum -const JointCtrlForceX: JointControlVars = 0; -const JointCtrlForceY: JointControlVars = 1; -const JointCtrlForceZ: JointControlVars = 2; -const JointCtrlTorqueX: JointControlVars = 3; -const JointCtrlTorqueY: JointControlVars = 4; -const JointCtrlTorqueZ: JointControlVars = 5; -const JointTargetPosX: JointControlVars = 6; -const JointTargetPosY: JointControlVars = 7; -const JointTargetPosZ: JointControlVars = 8; -const JointTargetRotX: JointControlVars = 9; -const JointTargetRotY: JointControlVars = 10; -const JointTargetRotZ: JointControlVars = 11; -const JointTargetRotW: JointControlVars = 12; -const JointTargetVelX: JointControlVars = 13; -const JointTargetVelY: JointControlVars = 14; -const JointTargetVelZ: JointControlVars = 15; -const JointTargetAngVelX: JointControlVars = 16; -const JointTargetAngVelY: JointControlVars = 17; -const JointTargetAngVelZ: JointControlVars = 18; +const JointControlForce: JointControlVars = 0; +const JointTargetPos: JointControlVars = 1; +const JointTargetVel: JointControlVars = 2; //////// import: "dynamics.go" alias DynamicVars = i32; //enums:enum @@ -155,53 +139,63 @@ const DynAngDeltaX: DynamicVars = 29; const DynAngDeltaY: DynamicVars = 30; const DynAngDeltaZ: DynamicVars = 31; fn DynamicIndex(idx: i32,cni: i32) -> i32 { - return i32(bitcast(Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynIndex))])); + return i32(bitcast(Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynIndex))])); } fn DynamicPos(idx: i32,cni: i32) -> vec3 { - return vec3(Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynPosX))], Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynPosY))], Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynPosZ))]); + return vec3(Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynPosX))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynPosY))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynPosZ))]); } fn SetDynamicPos(idx: i32,cni: i32, pos: vec3) { - Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynPosX))] = pos.x; - Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynPosY))] = pos.y; - Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynPosZ))] = pos.z; + Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynPosX))] = pos.x; + Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynPosY))] = pos.y; + Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynPosZ))] = pos.z; } fn DynamicRot(idx: i32,cni: i32) -> vec4 { - return vec4(Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynRotX))], Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynRotY))], Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynRotZ))], Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynRotW))]); + return vec4(Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynRotX))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynRotY))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynRotZ))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynRotW))]); } fn SetDynamicRot(idx: i32,cni: i32, rot: vec4) { - Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynRotX))] = rot.x; - Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynRotY))] = rot.y; - Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynRotZ))] = rot.z; - Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynRotW))] = rot.w; + Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynRotX))] = rot.x; + Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynRotY))] = rot.y; + Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynRotZ))] = rot.z; + Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynRotW))] = rot.w; } fn DynamicDelta(idx: i32,cni: i32) -> vec3 { - return vec3(Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynDeltaX))], Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynDeltaY))], Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynDeltaZ))]); + return vec3(Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynDeltaX))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynDeltaY))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynDeltaZ))]); } fn SetDynamicDelta(idx: i32,cni: i32, delta: vec3) { - Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynDeltaX))] = delta.x; - Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynDeltaY))] = delta.y; - Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynDeltaZ))] = delta.z; + Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynDeltaX))] = delta.x; + Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynDeltaY))] = delta.y; + Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynDeltaZ))] = delta.z; } fn DynamicAngDelta(idx: i32,cni: i32) -> vec3 { - return vec3(Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynAngDeltaX))], Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynAngDeltaY))], Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynAngDeltaZ))]); + return vec3(Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynAngDeltaX))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynAngDeltaY))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynAngDeltaZ))]); } fn SetDynamicAngDelta(idx: i32,cni: i32, angDelta: vec3) { - Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynAngDeltaX))] = angDelta.x; - Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynAngDeltaY))] = angDelta.y; - Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynAngDeltaZ))] = angDelta.z; + Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynAngDeltaX))] = angDelta.x; + Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynAngDeltaY))] = angDelta.y; + Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynAngDeltaZ))] = angDelta.z; } //////// import: "enumgen.go" const BodyVarsN: BodyVars = 37; const ContactVarsN: ContactVars = 9; -const JointControlVarsN: JointControlVars = 19; +const JointControlVarsN: JointControlVars = 3; const DynamicVarsN: DynamicVars = 32; -const GPUVarsN: GPUVars = 7; -const JointVarsN: JointVars = 53; +const GPUVarsN: GPUVars = 8; const JointTypesN: JointTypes = 7; +const JointVarsN: JointVars = 49; +const JointDoFVarsN: JointDoFVars = 7; const ShapesN: Shapes = 4; //////// import: "joint.go" +const JointLimitUnlimited = 1e10; +alias JointTypes = i32; //enums:enum +const Prismatic: JointTypes = 0; +const Revolute: JointTypes = 1; +const Ball: JointTypes = 2; +const Fixed: JointTypes = 3; +const Free: JointTypes = 4; +const Distance: JointTypes = 5; +const D6: JointTypes = 6; alias JointVars = i32; //enums:enum const JointType: JointVars = 0; const JointEnabled: JointVars = 1; @@ -221,49 +215,45 @@ const JointCRotX: JointVars = 14; const JointCRotY: JointVars = 15; const JointCRotZ: JointVars = 16; const JointCRotW: JointVars = 17; -const JointAxisX: JointVars = 18; -const JointAxisY: JointVars = 19; -const JointAxisZ: JointVars = 20; -const JointLimitLower: JointVars = 21; -const JointLimitUpper: JointVars = 22; -const JointStiffX: JointVars = 23; -const JointStiffY: JointVars = 24; -const JointStiffZ: JointVars = 25; -const JointDampX: JointVars = 26; -const JointDampY: JointVars = 27; -const JointDampZ: JointVars = 28; -const JointPForceX: JointVars = 29; -const JointPForceY: JointVars = 30; -const JointPForceZ: JointVars = 31; -const JointPTorqueX: JointVars = 32; -const JointPTorqueY: JointVars = 33; -const JointPTorqueZ: JointVars = 34; -const JointCForceX: JointVars = 35; -const JointCForceY: JointVars = 36; -const JointCForceZ: JointVars = 37; -const JointCTorqueX: JointVars = 38; -const JointCTorqueY: JointVars = 39; -const JointCTorqueZ: JointVars = 40; -const JointPDeltaX: JointVars = 41; -const JointPDeltaY: JointVars = 42; -const JointPDeltaZ: JointVars = 43; -const JointPAngDeltaX: JointVars = 44; -const JointPAngDeltaY: JointVars = 45; -const JointPAngDeltaZ: JointVars = 46; -const JointCDeltaX: JointVars = 47; -const JointCDeltaY: JointVars = 48; -const JointCDeltaZ: JointVars = 49; -const JointCAngDeltaX: JointVars = 50; -const JointCAngDeltaY: JointVars = 51; -const JointCAngDeltaZ: JointVars = 52; -alias JointTypes = i32; //enums:enum -const Prismatic: JointTypes = 0; -const Revolute: JointTypes = 1; -const Ball: JointTypes = 2; -const Fixed: JointTypes = 3; -const Free: JointTypes = 4; -const Distance: JointTypes = 5; -const D6: JointTypes = 6; +const JointDoFN: JointVars = 18; +const JointDoF1: JointVars = 19; +const JointDoF2: JointVars = 20; +const JointDoF3: JointVars = 21; +const JointDoF4: JointVars = 22; +const JointDoF5: JointVars = 23; +const JointDoF6: JointVars = 24; +const JointPForceX: JointVars = 25; +const JointPForceY: JointVars = 26; +const JointPForceZ: JointVars = 27; +const JointPTorqueX: JointVars = 28; +const JointPTorqueY: JointVars = 29; +const JointPTorqueZ: JointVars = 30; +const JointCForceX: JointVars = 31; +const JointCForceY: JointVars = 32; +const JointCForceZ: JointVars = 33; +const JointCTorqueX: JointVars = 34; +const JointCTorqueY: JointVars = 35; +const JointCTorqueZ: JointVars = 36; +const JointPDeltaX: JointVars = 37; +const JointPDeltaY: JointVars = 38; +const JointPDeltaZ: JointVars = 39; +const JointPAngDeltaX: JointVars = 40; +const JointPAngDeltaY: JointVars = 41; +const JointPAngDeltaZ: JointVars = 42; +const JointCDeltaX: JointVars = 43; +const JointCDeltaY: JointVars = 44; +const JointCDeltaZ: JointVars = 45; +const JointCAngDeltaX: JointVars = 46; +const JointCAngDeltaY: JointVars = 47; +const JointCAngDeltaZ: JointVars = 48; +alias JointDoFVars = i32; //enums:enum +const JointAxisX: JointDoFVars = 0; +const JointAxisY: JointDoFVars = 1; +const JointAxisZ: JointDoFVars = 2; +const JointLimitLower: JointDoFVars = 3; +const JointLimitUpper: JointDoFVars = 4; +const JointStiff: JointDoFVars = 5; +const JointDamp: JointDoFVars = 6; //////// import: "params.go" struct PhysParams { @@ -280,9 +270,13 @@ struct PhysParams { Restitution: i32, DynamicsN: i32, JointsN: i32, + JointDoFsN: i32, BodyJointsMax: i32, Cur: i32, Next: i32, + pad: i32, + pad1: i32, + pad2: i32, Gravity: vec4, } diff --git a/physics/shaders/StepIntegrateBodies.wgsl b/physics/shaders/StepIntegrateBodies.wgsl index cec97e27..647cab2f 100644 --- a/physics/shaders/StepIntegrateBodies.wgsl +++ b/physics/shaders/StepIntegrateBodies.wgsl @@ -12,7 +12,7 @@ var Bodies: array; // // Dynamics are the dynamic rigid body elements: these actually move. // Two alternating states are used: Params.Cur and Params.Next. // [dyn body][cur/next][DynamicVarsN] @group(2) @binding(0) var Dynamics: array; -// // JointControls are dynamic joint control inputs. // [joint][JointControlVarsN] +// // JointControls are dynamic joint control inputs, per joint DoF // (in correspondence with [JointDoFs]). This can be uploaded to the // GPU at every step. // [dof][JointControlVarsN] alias GPUVars = i32; @@ -100,25 +100,9 @@ const ContactDist: ContactVars = 8; //////// import: "control.go" alias JointControlVars = i32; //enums:enum -const JointCtrlForceX: JointControlVars = 0; -const JointCtrlForceY: JointControlVars = 1; -const JointCtrlForceZ: JointControlVars = 2; -const JointCtrlTorqueX: JointControlVars = 3; -const JointCtrlTorqueY: JointControlVars = 4; -const JointCtrlTorqueZ: JointControlVars = 5; -const JointTargetPosX: JointControlVars = 6; -const JointTargetPosY: JointControlVars = 7; -const JointTargetPosZ: JointControlVars = 8; -const JointTargetRotX: JointControlVars = 9; -const JointTargetRotY: JointControlVars = 10; -const JointTargetRotZ: JointControlVars = 11; -const JointTargetRotW: JointControlVars = 12; -const JointTargetVelX: JointControlVars = 13; -const JointTargetVelY: JointControlVars = 14; -const JointTargetVelZ: JointControlVars = 15; -const JointTargetAngVelX: JointControlVars = 16; -const JointTargetAngVelY: JointControlVars = 17; -const JointTargetAngVelZ: JointControlVars = 18; +const JointControlForce: JointControlVars = 0; +const JointTargetPos: JointControlVars = 1; +const JointTargetVel: JointControlVars = 2; //////// import: "dynamics.go" alias DynamicVars = i32; //enums:enum @@ -155,59 +139,69 @@ const DynAngDeltaX: DynamicVars = 29; const DynAngDeltaY: DynamicVars = 30; const DynAngDeltaZ: DynamicVars = 31; fn DynamicIndex(idx: i32,cni: i32) -> i32 { - return i32(bitcast(Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynIndex))])); + return i32(bitcast(Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynIndex))])); } fn DynamicPos(idx: i32,cni: i32) -> vec3 { - return vec3(Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynPosX))], Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynPosY))], Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynPosZ))]); + return vec3(Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynPosX))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynPosY))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynPosZ))]); } fn SetDynamicPos(idx: i32,cni: i32, pos: vec3) { - Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynPosX))] = pos.x; - Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynPosY))] = pos.y; - Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynPosZ))] = pos.z; + Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynPosX))] = pos.x; + Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynPosY))] = pos.y; + Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynPosZ))] = pos.z; } fn DynamicRot(idx: i32,cni: i32) -> vec4 { - return vec4(Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynRotX))], Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynRotY))], Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynRotZ))], Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynRotW))]); + return vec4(Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynRotX))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynRotY))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynRotZ))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynRotW))]); } fn SetDynamicRot(idx: i32,cni: i32, rot: vec4) { - Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynRotX))] = rot.x; - Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynRotY))] = rot.y; - Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynRotZ))] = rot.z; - Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynRotW))] = rot.w; + Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynRotX))] = rot.x; + Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynRotY))] = rot.y; + Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynRotZ))] = rot.z; + Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynRotW))] = rot.w; } fn DynamicForce(idx: i32,cni: i32) -> vec3 { - return vec3(Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynForceX))], Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynForceY))], Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynForceZ))]); + return vec3(Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynForceX))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynForceY))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynForceZ))]); } fn DynamicTorque(idx: i32,cni: i32) -> vec3 { - return vec3(Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynTorqueX))], Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynTorqueY))], Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynTorqueZ))]); + return vec3(Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynTorqueX))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynTorqueY))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynTorqueZ))]); } fn DynamicDelta(idx: i32,cni: i32) -> vec3 { - return vec3(Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynDeltaX))], Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynDeltaY))], Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynDeltaZ))]); + return vec3(Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynDeltaX))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynDeltaY))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynDeltaZ))]); } fn SetDynamicDelta(idx: i32,cni: i32, delta: vec3) { - Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynDeltaX))] = delta.x; - Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynDeltaY))] = delta.y; - Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynDeltaZ))] = delta.z; + Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynDeltaX))] = delta.x; + Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynDeltaY))] = delta.y; + Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynDeltaZ))] = delta.z; } fn DynamicAngDelta(idx: i32,cni: i32) -> vec3 { - return vec3(Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynAngDeltaX))], Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynAngDeltaY))], Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynAngDeltaZ))]); + return vec3(Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynAngDeltaX))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynAngDeltaY))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynAngDeltaZ))]); } fn SetDynamicAngDelta(idx: i32,cni: i32, angDelta: vec3) { - Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynAngDeltaX))] = angDelta.x; - Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynAngDeltaY))] = angDelta.y; - Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynAngDeltaZ))] = angDelta.z; + Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynAngDeltaX))] = angDelta.x; + Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynAngDeltaY))] = angDelta.y; + Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynAngDeltaZ))] = angDelta.z; } //////// import: "enumgen.go" const BodyVarsN: BodyVars = 37; const ContactVarsN: ContactVars = 9; -const JointControlVarsN: JointControlVars = 19; +const JointControlVarsN: JointControlVars = 3; const DynamicVarsN: DynamicVars = 32; -const GPUVarsN: GPUVars = 7; -const JointVarsN: JointVars = 53; +const GPUVarsN: GPUVars = 8; const JointTypesN: JointTypes = 7; +const JointVarsN: JointVars = 49; +const JointDoFVarsN: JointDoFVars = 7; const ShapesN: Shapes = 4; //////// import: "joint.go" +const JointLimitUnlimited = 1e10; +alias JointTypes = i32; //enums:enum +const Prismatic: JointTypes = 0; +const Revolute: JointTypes = 1; +const Ball: JointTypes = 2; +const Fixed: JointTypes = 3; +const Free: JointTypes = 4; +const Distance: JointTypes = 5; +const D6: JointTypes = 6; alias JointVars = i32; //enums:enum const JointType: JointVars = 0; const JointEnabled: JointVars = 1; @@ -227,49 +221,45 @@ const JointCRotX: JointVars = 14; const JointCRotY: JointVars = 15; const JointCRotZ: JointVars = 16; const JointCRotW: JointVars = 17; -const JointAxisX: JointVars = 18; -const JointAxisY: JointVars = 19; -const JointAxisZ: JointVars = 20; -const JointLimitLower: JointVars = 21; -const JointLimitUpper: JointVars = 22; -const JointStiffX: JointVars = 23; -const JointStiffY: JointVars = 24; -const JointStiffZ: JointVars = 25; -const JointDampX: JointVars = 26; -const JointDampY: JointVars = 27; -const JointDampZ: JointVars = 28; -const JointPForceX: JointVars = 29; -const JointPForceY: JointVars = 30; -const JointPForceZ: JointVars = 31; -const JointPTorqueX: JointVars = 32; -const JointPTorqueY: JointVars = 33; -const JointPTorqueZ: JointVars = 34; -const JointCForceX: JointVars = 35; -const JointCForceY: JointVars = 36; -const JointCForceZ: JointVars = 37; -const JointCTorqueX: JointVars = 38; -const JointCTorqueY: JointVars = 39; -const JointCTorqueZ: JointVars = 40; -const JointPDeltaX: JointVars = 41; -const JointPDeltaY: JointVars = 42; -const JointPDeltaZ: JointVars = 43; -const JointPAngDeltaX: JointVars = 44; -const JointPAngDeltaY: JointVars = 45; -const JointPAngDeltaZ: JointVars = 46; -const JointCDeltaX: JointVars = 47; -const JointCDeltaY: JointVars = 48; -const JointCDeltaZ: JointVars = 49; -const JointCAngDeltaX: JointVars = 50; -const JointCAngDeltaY: JointVars = 51; -const JointCAngDeltaZ: JointVars = 52; -alias JointTypes = i32; //enums:enum -const Prismatic: JointTypes = 0; -const Revolute: JointTypes = 1; -const Ball: JointTypes = 2; -const Fixed: JointTypes = 3; -const Free: JointTypes = 4; -const Distance: JointTypes = 5; -const D6: JointTypes = 6; +const JointDoFN: JointVars = 18; +const JointDoF1: JointVars = 19; +const JointDoF2: JointVars = 20; +const JointDoF3: JointVars = 21; +const JointDoF4: JointVars = 22; +const JointDoF5: JointVars = 23; +const JointDoF6: JointVars = 24; +const JointPForceX: JointVars = 25; +const JointPForceY: JointVars = 26; +const JointPForceZ: JointVars = 27; +const JointPTorqueX: JointVars = 28; +const JointPTorqueY: JointVars = 29; +const JointPTorqueZ: JointVars = 30; +const JointCForceX: JointVars = 31; +const JointCForceY: JointVars = 32; +const JointCForceZ: JointVars = 33; +const JointCTorqueX: JointVars = 34; +const JointCTorqueY: JointVars = 35; +const JointCTorqueZ: JointVars = 36; +const JointPDeltaX: JointVars = 37; +const JointPDeltaY: JointVars = 38; +const JointPDeltaZ: JointVars = 39; +const JointPAngDeltaX: JointVars = 40; +const JointPAngDeltaY: JointVars = 41; +const JointPAngDeltaZ: JointVars = 42; +const JointCDeltaX: JointVars = 43; +const JointCDeltaY: JointVars = 44; +const JointCDeltaZ: JointVars = 45; +const JointCAngDeltaX: JointVars = 46; +const JointCAngDeltaY: JointVars = 47; +const JointCAngDeltaZ: JointVars = 48; +alias JointDoFVars = i32; //enums:enum +const JointAxisX: JointDoFVars = 0; +const JointAxisY: JointDoFVars = 1; +const JointAxisZ: JointDoFVars = 2; +const JointLimitLower: JointDoFVars = 3; +const JointLimitUpper: JointDoFVars = 4; +const JointStiff: JointDoFVars = 5; +const JointDamp: JointDoFVars = 6; //////// import: "params.go" struct PhysParams { @@ -286,9 +276,13 @@ struct PhysParams { Restitution: i32, DynamicsN: i32, JointsN: i32, + JointDoFsN: i32, BodyJointsMax: i32, Cur: i32, Next: i32, + pad: i32, + pad1: i32, + pad2: i32, Gravity: vec4, } diff --git a/physics/shaders/StepJointForces.wgsl b/physics/shaders/StepJointForces.wgsl index 3af34451..c6564f02 100644 --- a/physics/shaders/StepJointForces.wgsl +++ b/physics/shaders/StepJointForces.wgsl @@ -11,10 +11,12 @@ var Params: array; var Bodies: array; @group(1) @binding(1) var Joints: array; +@group(1) @binding(2) +var JointDoFs: array; // // Dynamics are the dynamic rigid body elements: these actually move. // Two alternating states are used: Params.Cur and Params.Next. // [dyn body][cur/next][DynamicVarsN] @group(2) @binding(0) var Dynamics: array; -// // JointControls are dynamic joint control inputs. // [joint][JointControlVarsN] +// // JointControls are dynamic joint control inputs, per joint DoF // (in correspondence with [JointDoFs]). This can be uploaded to the // GPU at every step. // [dof][JointControlVarsN] @group(3) @binding(0) var JointControls: array; @@ -94,30 +96,11 @@ const ContactDist: ContactVars = 8; //////// import: "control.go" alias JointControlVars = i32; //enums:enum -const JointCtrlForceX: JointControlVars = 0; -const JointCtrlForceY: JointControlVars = 1; -const JointCtrlForceZ: JointControlVars = 2; -const JointCtrlTorqueX: JointControlVars = 3; -const JointCtrlTorqueY: JointControlVars = 4; -const JointCtrlTorqueZ: JointControlVars = 5; -const JointTargetPosX: JointControlVars = 6; -const JointTargetPosY: JointControlVars = 7; -const JointTargetPosZ: JointControlVars = 8; -const JointTargetRotX: JointControlVars = 9; -const JointTargetRotY: JointControlVars = 10; -const JointTargetRotZ: JointControlVars = 11; -const JointTargetRotW: JointControlVars = 12; -const JointTargetVelX: JointControlVars = 13; -const JointTargetVelY: JointControlVars = 14; -const JointTargetVelZ: JointControlVars = 15; -const JointTargetAngVelX: JointControlVars = 16; -const JointTargetAngVelY: JointControlVars = 17; -const JointTargetAngVelZ: JointControlVars = 18; -fn JointControlForce(idx: i32) -> vec3 { - return vec3(JointControls[Index2D(TensorStrides[50], TensorStrides[51], u32(idx), u32(JointCtrlForceX))], JointControls[Index2D(TensorStrides[50], TensorStrides[51], u32(idx), u32(JointCtrlForceY))], JointControls[Index2D(TensorStrides[50], TensorStrides[51], u32(idx), u32(JointCtrlForceZ))]); -} -fn JointControlTorque(idx: i32) -> vec3 { - return vec3(JointControls[Index2D(TensorStrides[50], TensorStrides[51], u32(idx), u32(JointCtrlTorqueX))], JointControls[Index2D(TensorStrides[50], TensorStrides[51], u32(idx), u32(JointCtrlTorqueY))], JointControls[Index2D(TensorStrides[50], TensorStrides[51], u32(idx), u32(JointCtrlTorqueZ))]); +const JointControlForce: JointControlVars = 0; +const JointTargetPos: JointControlVars = 1; +const JointTargetVel: JointControlVars = 2; +fn GetJointControlForce(idx: i32,dof: i32) -> f32 { + return JointControls[Index2D(TensorStrides[60], TensorStrides[61], u32(JointDoFIndex(idx, dof)), u32(JointControlForce))]; } //////// import: "dynamics.go" @@ -155,26 +138,36 @@ const DynAngDeltaX: DynamicVars = 29; const DynAngDeltaY: DynamicVars = 30; const DynAngDeltaZ: DynamicVars = 31; fn DynamicIndex(idx: i32,cni: i32) -> i32 { - return i32(bitcast(Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynIndex))])); + return i32(bitcast(Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynIndex))])); } fn DynamicPos(idx: i32,cni: i32) -> vec3 { - return vec3(Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynPosX))], Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynPosY))], Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynPosZ))]); + return vec3(Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynPosX))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynPosY))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynPosZ))]); } fn DynamicRot(idx: i32,cni: i32) -> vec4 { - return vec4(Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynRotX))], Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynRotY))], Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynRotZ))], Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynRotW))]); + return vec4(Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynRotX))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynRotY))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynRotZ))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynRotW))]); } //////// import: "enumgen.go" const BodyVarsN: BodyVars = 37; const ContactVarsN: ContactVars = 9; -const JointControlVarsN: JointControlVars = 19; +const JointControlVarsN: JointControlVars = 3; const DynamicVarsN: DynamicVars = 32; -const GPUVarsN: GPUVars = 7; -const JointVarsN: JointVars = 53; +const GPUVarsN: GPUVars = 8; const JointTypesN: JointTypes = 7; +const JointVarsN: JointVars = 49; +const JointDoFVarsN: JointDoFVars = 7; const ShapesN: Shapes = 4; //////// import: "joint.go" +const JointLimitUnlimited = 1e10; +alias JointTypes = i32; //enums:enum +const Prismatic: JointTypes = 0; +const Revolute: JointTypes = 1; +const Ball: JointTypes = 2; +const Fixed: JointTypes = 3; +const Free: JointTypes = 4; +const Distance: JointTypes = 5; +const D6: JointTypes = 6; alias JointVars = i32; //enums:enum const JointType: JointVars = 0; const JointEnabled: JointVars = 1; @@ -194,41 +187,37 @@ const JointCRotX: JointVars = 14; const JointCRotY: JointVars = 15; const JointCRotZ: JointVars = 16; const JointCRotW: JointVars = 17; -const JointAxisX: JointVars = 18; -const JointAxisY: JointVars = 19; -const JointAxisZ: JointVars = 20; -const JointLimitLower: JointVars = 21; -const JointLimitUpper: JointVars = 22; -const JointStiffX: JointVars = 23; -const JointStiffY: JointVars = 24; -const JointStiffZ: JointVars = 25; -const JointDampX: JointVars = 26; -const JointDampY: JointVars = 27; -const JointDampZ: JointVars = 28; -const JointPForceX: JointVars = 29; -const JointPForceY: JointVars = 30; -const JointPForceZ: JointVars = 31; -const JointPTorqueX: JointVars = 32; -const JointPTorqueY: JointVars = 33; -const JointPTorqueZ: JointVars = 34; -const JointCForceX: JointVars = 35; -const JointCForceY: JointVars = 36; -const JointCForceZ: JointVars = 37; -const JointCTorqueX: JointVars = 38; -const JointCTorqueY: JointVars = 39; -const JointCTorqueZ: JointVars = 40; -const JointPDeltaX: JointVars = 41; -const JointPDeltaY: JointVars = 42; -const JointPDeltaZ: JointVars = 43; -const JointPAngDeltaX: JointVars = 44; -const JointPAngDeltaY: JointVars = 45; -const JointPAngDeltaZ: JointVars = 46; -const JointCDeltaX: JointVars = 47; -const JointCDeltaY: JointVars = 48; -const JointCDeltaZ: JointVars = 49; -const JointCAngDeltaX: JointVars = 50; -const JointCAngDeltaY: JointVars = 51; -const JointCAngDeltaZ: JointVars = 52; +const JointDoFN: JointVars = 18; +const JointDoF1: JointVars = 19; +const JointDoF2: JointVars = 20; +const JointDoF3: JointVars = 21; +const JointDoF4: JointVars = 22; +const JointDoF5: JointVars = 23; +const JointDoF6: JointVars = 24; +const JointPForceX: JointVars = 25; +const JointPForceY: JointVars = 26; +const JointPForceZ: JointVars = 27; +const JointPTorqueX: JointVars = 28; +const JointPTorqueY: JointVars = 29; +const JointPTorqueZ: JointVars = 30; +const JointCForceX: JointVars = 31; +const JointCForceY: JointVars = 32; +const JointCForceZ: JointVars = 33; +const JointCTorqueX: JointVars = 34; +const JointCTorqueY: JointVars = 35; +const JointCTorqueZ: JointVars = 36; +const JointPDeltaX: JointVars = 37; +const JointPDeltaY: JointVars = 38; +const JointPDeltaZ: JointVars = 39; +const JointPAngDeltaX: JointVars = 40; +const JointPAngDeltaY: JointVars = 41; +const JointPAngDeltaZ: JointVars = 42; +const JointCDeltaX: JointVars = 43; +const JointCDeltaY: JointVars = 44; +const JointCDeltaZ: JointVars = 45; +const JointCAngDeltaX: JointVars = 46; +const JointCAngDeltaY: JointVars = 47; +const JointCAngDeltaZ: JointVars = 48; fn GetJointType(idx: i32) -> JointTypes { return JointTypes(bitcast(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointType))])); } @@ -238,15 +227,15 @@ fn JointParentIndex(idx: i32) -> i32 { fn JointChildIndex(idx: i32) -> i32 { return i32(bitcast(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointChild))])); } +fn JointDoFIndex(idx: i32,dof: i32) -> i32 { + return i32(bitcast(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(i32(JointDoF1) + dof))])); +} fn JointPPos(idx: i32) -> vec3 { return vec3(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPPosX))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPPosY))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPPosZ))]); } fn JointPRot(idx: i32) -> vec4 { return vec4(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPRotX))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPRotY))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPRotZ))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPRotW))]); } -fn JointAxis(idx: i32) -> vec3 { - return vec3(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointAxisX))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointAxisY))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointAxisZ))]); -} fn SetJointPForce(idx: i32, f: vec3) { Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPForceX))] = f.x; Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPForceY))] = f.y; @@ -267,14 +256,20 @@ fn SetJointCTorque(idx: i32, t: vec3) { Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCTorqueY))] = t.y; Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCTorqueZ))] = t.z; } -alias JointTypes = i32; //enums:enum -const Prismatic: JointTypes = 0; -const Revolute: JointTypes = 1; -const Ball: JointTypes = 2; -const Fixed: JointTypes = 3; -const Free: JointTypes = 4; -const Distance: JointTypes = 5; -const D6: JointTypes = 6; +alias JointDoFVars = i32; //enums:enum +const JointAxisX: JointDoFVars = 0; +const JointAxisY: JointDoFVars = 1; +const JointAxisZ: JointDoFVars = 2; +const JointLimitLower: JointDoFVars = 3; +const JointLimitUpper: JointDoFVars = 4; +const JointStiff: JointDoFVars = 5; +const JointDamp: JointDoFVars = 6; +fn JointAxisDoF(didx: i32) -> vec3 { + return vec3(JointDoFs[Index2D(TensorStrides[20], TensorStrides[21], u32(didx), u32(JointAxisX))], JointDoFs[Index2D(TensorStrides[20], TensorStrides[21], u32(didx), u32(JointAxisY))], JointDoFs[Index2D(TensorStrides[20], TensorStrides[21], u32(didx), u32(JointAxisZ))]); +} +fn JointAxis(idx: i32,dof: i32) -> vec3 { + return JointAxisDoF(JointDoFIndex(idx, dof)); +} //////// import: "params.go" struct PhysParams { @@ -291,9 +286,13 @@ struct PhysParams { Restitution: i32, DynamicsN: i32, JointsN: i32, + JointDoFsN: i32, BodyJointsMax: i32, Cur: i32, Next: i32, + pad: i32, + pad1: i32, + pad2: i32, Gravity: vec4, } @@ -372,22 +371,20 @@ fn StepJointForces(i: u32) { //gosl:kernel var xwcP = posecP; var comc = BodyCom(jcbi); var rc = xwcP-(MulQPPoint(posecP, posecQ, comc)); // child moment arm - var jf = JointControlForce(ji); - var jtq = JointControlTorque(ji); var f: vec3; var t: vec3; switch (jt) { case Free, Distance: { - f = jf; - t = jtq; + f = vec3(GetJointControlForce(ji, i32(i32(0))), GetJointControlForce(ji, i32(i32(1))), GetJointControlForce(ji, i32(i32(2)))); + t = vec3(GetJointControlForce(ji, i32(i32(3))), GetJointControlForce(ji, i32(i32(4))), GetJointControlForce(ji, i32(i32(5)))); } case Ball: { - t = jtq; + t = vec3(GetJointControlForce(ji, i32(i32(0))), GetJointControlForce(ji, i32(i32(1))), GetJointControlForce(ji, i32(i32(2)))); } case Revolute, Prismatic: { - var axis = JointAxis(ji); + var axis = JointAxis(ji, i32(i32(0))); var ap = MulQuatVector(xwpQ, axis); - f = f+(MulScalar3(ap, jf.x)); + f = MulScalar3(ap, GetJointControlForce(ji, i32(i32(0)))); } default: { } diff --git a/physics/shaders/StepSolveJoints.wgsl b/physics/shaders/StepSolveJoints.wgsl index d43722ab..b0d4c059 100644 --- a/physics/shaders/StepSolveJoints.wgsl +++ b/physics/shaders/StepSolveJoints.wgsl @@ -11,10 +11,12 @@ var Params: array; var Bodies: array; @group(1) @binding(1) var Joints: array; +@group(1) @binding(2) +var JointDoFs: array; // // Dynamics are the dynamic rigid body elements: these actually move. // Two alternating states are used: Params.Cur and Params.Next. // [dyn body][cur/next][DynamicVarsN] @group(2) @binding(0) var Dynamics: array; -// // JointControls are dynamic joint control inputs. // [joint][JointControlVarsN] +// // JointControls are dynamic joint control inputs, per joint DoF // (in correspondence with [JointDoFs]). This can be uploaded to the // GPU at every step. // [dof][JointControlVarsN] @group(3) @binding(0) var JointControls: array; @@ -99,30 +101,14 @@ const ContactDist: ContactVars = 8; //////// import: "control.go" alias JointControlVars = i32; //enums:enum -const JointCtrlForceX: JointControlVars = 0; -const JointCtrlForceY: JointControlVars = 1; -const JointCtrlForceZ: JointControlVars = 2; -const JointCtrlTorqueX: JointControlVars = 3; -const JointCtrlTorqueY: JointControlVars = 4; -const JointCtrlTorqueZ: JointControlVars = 5; -const JointTargetPosX: JointControlVars = 6; -const JointTargetPosY: JointControlVars = 7; -const JointTargetPosZ: JointControlVars = 8; -const JointTargetRotX: JointControlVars = 9; -const JointTargetRotY: JointControlVars = 10; -const JointTargetRotZ: JointControlVars = 11; -const JointTargetRotW: JointControlVars = 12; -const JointTargetVelX: JointControlVars = 13; -const JointTargetVelY: JointControlVars = 14; -const JointTargetVelZ: JointControlVars = 15; -const JointTargetAngVelX: JointControlVars = 16; -const JointTargetAngVelY: JointControlVars = 17; -const JointTargetAngVelZ: JointControlVars = 18; -fn JointTargetPos(idx: i32) -> vec3 { - return vec3(JointControls[Index2D(TensorStrides[50], TensorStrides[51], u32(idx), u32(JointTargetPosX))], JointControls[Index2D(TensorStrides[50], TensorStrides[51], u32(idx), u32(JointTargetPosY))], JointControls[Index2D(TensorStrides[50], TensorStrides[51], u32(idx), u32(JointTargetPosZ))]); -} -fn JointTargetVel(idx: i32) -> vec3 { - return vec3(JointControls[Index2D(TensorStrides[50], TensorStrides[51], u32(idx), u32(JointTargetVelX))], JointControls[Index2D(TensorStrides[50], TensorStrides[51], u32(idx), u32(JointTargetVelY))], JointControls[Index2D(TensorStrides[50], TensorStrides[51], u32(idx), u32(JointTargetVelZ))]); +const JointControlForce: JointControlVars = 0; +const JointTargetPos: JointControlVars = 1; +const JointTargetVel: JointControlVars = 2; +fn GetJointTargetPos(idx: i32,dof: i32) -> f32 { + return JointControls[Index2D(TensorStrides[60], TensorStrides[61], u32(JointDoFIndex(idx, dof)), u32(JointTargetPos))]; +} +fn GetJointTargetVel(idx: i32,dof: i32) -> f32 { + return JointControls[Index2D(TensorStrides[60], TensorStrides[61], u32(JointDoFIndex(idx, dof)), u32(JointTargetVel))]; } //////// import: "dynamics.go" @@ -160,32 +146,42 @@ const DynAngDeltaX: DynamicVars = 29; const DynAngDeltaY: DynamicVars = 30; const DynAngDeltaZ: DynamicVars = 31; fn DynamicIndex(idx: i32,cni: i32) -> i32 { - return i32(bitcast(Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynIndex))])); + return i32(bitcast(Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynIndex))])); } fn DynamicPos(idx: i32,cni: i32) -> vec3 { - return vec3(Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynPosX))], Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynPosY))], Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynPosZ))]); + return vec3(Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynPosX))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynPosY))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynPosZ))]); } fn DynamicRot(idx: i32,cni: i32) -> vec4 { - return vec4(Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynRotX))], Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynRotY))], Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynRotZ))], Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynRotW))]); + return vec4(Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynRotX))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynRotY))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynRotZ))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynRotW))]); } fn DynamicDelta(idx: i32,cni: i32) -> vec3 { - return vec3(Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynDeltaX))], Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynDeltaY))], Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynDeltaZ))]); + return vec3(Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynDeltaX))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynDeltaY))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynDeltaZ))]); } fn DynamicAngDelta(idx: i32,cni: i32) -> vec3 { - return vec3(Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynAngDeltaX))], Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynAngDeltaY))], Dynamics[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(idx), u32(cni), u32(DynAngDeltaZ))]); + return vec3(Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynAngDeltaX))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynAngDeltaY))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynAngDeltaZ))]); } //////// import: "enumgen.go" const BodyVarsN: BodyVars = 37; const ContactVarsN: ContactVars = 9; -const JointControlVarsN: JointControlVars = 19; +const JointControlVarsN: JointControlVars = 3; const DynamicVarsN: DynamicVars = 32; -const GPUVarsN: GPUVars = 7; -const JointVarsN: JointVars = 53; +const GPUVarsN: GPUVars = 8; const JointTypesN: JointTypes = 7; +const JointVarsN: JointVars = 49; +const JointDoFVarsN: JointDoFVars = 7; const ShapesN: Shapes = 4; //////// import: "joint.go" +const JointLimitUnlimited = 1e10; +alias JointTypes = i32; //enums:enum +const Prismatic: JointTypes = 0; +const Revolute: JointTypes = 1; +const Ball: JointTypes = 2; +const Fixed: JointTypes = 3; +const Free: JointTypes = 4; +const Distance: JointTypes = 5; +const D6: JointTypes = 6; alias JointVars = i32; //enums:enum const JointType: JointVars = 0; const JointEnabled: JointVars = 1; @@ -205,41 +201,37 @@ const JointCRotX: JointVars = 14; const JointCRotY: JointVars = 15; const JointCRotZ: JointVars = 16; const JointCRotW: JointVars = 17; -const JointAxisX: JointVars = 18; -const JointAxisY: JointVars = 19; -const JointAxisZ: JointVars = 20; -const JointLimitLower: JointVars = 21; -const JointLimitUpper: JointVars = 22; -const JointStiffX: JointVars = 23; -const JointStiffY: JointVars = 24; -const JointStiffZ: JointVars = 25; -const JointDampX: JointVars = 26; -const JointDampY: JointVars = 27; -const JointDampZ: JointVars = 28; -const JointPForceX: JointVars = 29; -const JointPForceY: JointVars = 30; -const JointPForceZ: JointVars = 31; -const JointPTorqueX: JointVars = 32; -const JointPTorqueY: JointVars = 33; -const JointPTorqueZ: JointVars = 34; -const JointCForceX: JointVars = 35; -const JointCForceY: JointVars = 36; -const JointCForceZ: JointVars = 37; -const JointCTorqueX: JointVars = 38; -const JointCTorqueY: JointVars = 39; -const JointCTorqueZ: JointVars = 40; -const JointPDeltaX: JointVars = 41; -const JointPDeltaY: JointVars = 42; -const JointPDeltaZ: JointVars = 43; -const JointPAngDeltaX: JointVars = 44; -const JointPAngDeltaY: JointVars = 45; -const JointPAngDeltaZ: JointVars = 46; -const JointCDeltaX: JointVars = 47; -const JointCDeltaY: JointVars = 48; -const JointCDeltaZ: JointVars = 49; -const JointCAngDeltaX: JointVars = 50; -const JointCAngDeltaY: JointVars = 51; -const JointCAngDeltaZ: JointVars = 52; +const JointDoFN: JointVars = 18; +const JointDoF1: JointVars = 19; +const JointDoF2: JointVars = 20; +const JointDoF3: JointVars = 21; +const JointDoF4: JointVars = 22; +const JointDoF5: JointVars = 23; +const JointDoF6: JointVars = 24; +const JointPForceX: JointVars = 25; +const JointPForceY: JointVars = 26; +const JointPForceZ: JointVars = 27; +const JointPTorqueX: JointVars = 28; +const JointPTorqueY: JointVars = 29; +const JointPTorqueZ: JointVars = 30; +const JointCForceX: JointVars = 31; +const JointCForceY: JointVars = 32; +const JointCForceZ: JointVars = 33; +const JointCTorqueX: JointVars = 34; +const JointCTorqueY: JointVars = 35; +const JointCTorqueZ: JointVars = 36; +const JointPDeltaX: JointVars = 37; +const JointPDeltaY: JointVars = 38; +const JointPDeltaZ: JointVars = 39; +const JointPAngDeltaX: JointVars = 40; +const JointPAngDeltaY: JointVars = 41; +const JointPAngDeltaZ: JointVars = 42; +const JointCDeltaX: JointVars = 43; +const JointCDeltaY: JointVars = 44; +const JointCDeltaZ: JointVars = 45; +const JointCAngDeltaX: JointVars = 46; +const JointCAngDeltaY: JointVars = 47; +const JointCAngDeltaZ: JointVars = 48; fn GetJointType(idx: i32) -> JointTypes { return JointTypes(bitcast(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointType))])); } @@ -249,6 +241,9 @@ fn JointParentIndex(idx: i32) -> i32 { fn JointChildIndex(idx: i32) -> i32 { return i32(bitcast(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointChild))])); } +fn JointDoFIndex(idx: i32,dof: i32) -> i32 { + return i32(bitcast(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(i32(JointDoF1) + dof))])); +} fn JointPPos(idx: i32) -> vec3 { return vec3(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPPosX))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPPosY))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPPosZ))]); } @@ -261,15 +256,6 @@ fn JointCPos(idx: i32) -> vec3 { fn JointCRot(idx: i32) -> vec4 { return vec4(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCRotX))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCRotY))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCRotZ))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCRotW))]); } -fn JointAxis(idx: i32) -> vec3 { - return vec3(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointAxisX))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointAxisY))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointAxisZ))]); -} -fn JointStiff(idx: i32) -> vec3 { - return vec3(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointStiffX))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointStiffY))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointStiffZ))]); -} -fn JointDamp(idx: i32) -> vec3 { - return vec3(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointDampX))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointDampY))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointDampZ))]); -} fn SetJointPDelta(idx: i32, f: vec3) { Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPDeltaX))] = f.x; Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPDeltaY))] = f.y; @@ -290,14 +276,23 @@ fn SetJointCAngDelta(idx: i32, t: vec3) { Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCAngDeltaY))] = t.y; Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCAngDeltaZ))] = t.z; } -alias JointTypes = i32; //enums:enum -const Prismatic: JointTypes = 0; -const Revolute: JointTypes = 1; -const Ball: JointTypes = 2; -const Fixed: JointTypes = 3; -const Free: JointTypes = 4; -const Distance: JointTypes = 5; -const D6: JointTypes = 6; +alias JointDoFVars = i32; //enums:enum +const JointAxisX: JointDoFVars = 0; +const JointAxisY: JointDoFVars = 1; +const JointAxisZ: JointDoFVars = 2; +const JointLimitLower: JointDoFVars = 3; +const JointLimitUpper: JointDoFVars = 4; +const JointStiff: JointDoFVars = 5; +const JointDamp: JointDoFVars = 6; +fn JointAxisDoF(didx: i32) -> vec3 { + return vec3(JointDoFs[Index2D(TensorStrides[20], TensorStrides[21], u32(didx), u32(JointAxisX))], JointDoFs[Index2D(TensorStrides[20], TensorStrides[21], u32(didx), u32(JointAxisY))], JointDoFs[Index2D(TensorStrides[20], TensorStrides[21], u32(didx), u32(JointAxisZ))]); +} +fn JointAxis(idx: i32,dof: i32) -> vec3 { + return JointAxisDoF(JointDoFIndex(idx, dof)); +} +fn JointDoF(idx: i32,dof: i32, vr: JointDoFVars) -> f32 { + return JointDoFs[Index2D(TensorStrides[20], TensorStrides[21], u32(JointDoFIndex(idx, dof)), u32(vr))]; +} //////// import: "params.go" struct PhysParams { @@ -314,9 +309,13 @@ struct PhysParams { Restitution: i32, DynamicsN: i32, JointsN: i32, + JointDoFsN: i32, BodyJointsMax: i32, Cur: i32, Next: i32, + pad: i32, + pad1: i32, + pad2: i32, Gravity: vec4, } @@ -488,20 +487,20 @@ fn StepSolveJoints(i: u32) { //gosl:kernel var axisTargetPosKeA: vec3; var axisTargetVelKdD: vec3; var axisTargetVelKdA: vec3; - var axis = JointAxis(ji); - var loTemp = axis*(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(ji), u32(JointLimitLower))]); - var upTemp = axis*(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(ji), u32(JointLimitUpper))]); + var axis = JointAxis(ji, i32(i32(0))); + var loTemp = axis*(JointDoF(ji, i32(i32(0)), JointLimitLower)); + var upTemp = axis*(JointDoF(ji, i32(i32(0)), JointLimitUpper)); axisLimitsD = Min3(loTemp, upTemp); axisLimitsA = Max3(loTemp, upTemp); - var ke = JointStiff(ji); - var kd = JointDamp(ji); - var targetPos = JointTargetPos(ji); - var targetVel = JointTargetVel(ji); - if (ke.x > 0.0) { // has position control - UpdateJointAxisWeightedTarget(axis, targetPos.x, ke.x, &axisTargetPosKeD, &axisTargetPosKeA); + var ke = JointDoF(ji, i32(i32(0)), JointStiff); + var kd = JointDoF(ji, i32(i32(0)), JointDamp); + var targetPos = GetJointTargetPos(ji, i32(i32(0))); + var targetVel = GetJointTargetVel(ji, i32(i32(0))); + if (ke > 0.0) { // has position control + UpdateJointAxisWeightedTarget(axis, targetPos, ke, &axisTargetPosKeD, &axisTargetPosKeA); } - if (kd.x > 0.0) { // has velocity control - UpdateJointAxisWeightedTarget(axis, targetVel.x, kd.x, &axisTargetVelKdD, &axisTargetVelKdA); + if (kd > 0.0) { // has velocity control + UpdateJointAxisWeightedTarget(axis, targetVel, kd, &axisTargetVelKdD, &axisTargetVelKdA); } var axisStiffness = axisTargetPosKeA; var axisDamping = axisTargetVelKdA; diff --git a/physics/step_joint.go b/physics/step_joint.go index 8834da14..4a9a33d0 100644 --- a/physics/step_joint.go +++ b/physics/step_joint.go @@ -24,6 +24,7 @@ func StepJointForces(i uint32) { //gosl:kernel if ji >= params.JointsN { return } + // ndof := JointDoFN(ji) // todo: enabled jpi := JointParentIndex(ji) jpbi := int32(-1) @@ -60,22 +61,17 @@ func StepJointForces(i uint32) { //gosl:kernel comc := BodyCom(jcbi) rc := xwcP.Sub(slmath.MulQPPoint(posecP, posecQ, comc)) // child moment arm - // from controls: - jf := JointControlForce(ji) - jtq := JointControlTorque(ji) - var f, t math32.Vector3 switch jt { case Free, Distance: - // todo: distance doesn't seem to be supported here? - f = jf - t = jtq + f = math32.Vec3(GetJointControlForce(ji, 0), GetJointControlForce(ji, 1), GetJointControlForce(ji, 2)) + t = math32.Vec3(GetJointControlForce(ji, 3), GetJointControlForce(ji, 4), GetJointControlForce(ji, 5)) case Ball: - t = jtq + t = math32.Vec3(GetJointControlForce(ji, 0), GetJointControlForce(ji, 1), GetJointControlForce(ji, 2)) case Revolute, Prismatic: - axis := JointAxis(ji) + axis := JointAxis(ji, 0) ap := slmath.MulQuatVector(xwpQ, axis) - f = f.Add(slmath.MulScalar3(ap, jf.X)) + f = slmath.MulScalar3(ap, GetJointControlForce(ji, 0)) default: // todo: D6 requires more iteration! } @@ -241,25 +237,20 @@ func StepSolveJoints(i uint32) { //gosl:kernel var axisTargetPosKeD, axisTargetPosKeA math32.Vector3 var axisTargetVelKdD, axisTargetVelKdA math32.Vector3 - // axis_target_pos_ke = wp.spatial_vector() - // axis_target_vel_kd = wp.spatial_vector() - // - // avoid a for loop here since local variables would need to be modified - // which is not yet differentiable - axis := JointAxis(ji) - loTemp := axis.MulScalar(Joints.Value(int(ji), int(JointLimitLower))) - upTemp := axis.MulScalar(Joints.Value(int(ji), int(JointLimitUpper))) + axis := JointAxis(ji, 0) + loTemp := axis.MulScalar(JointDoF(ji, 0, JointLimitLower)) + upTemp := axis.MulScalar(JointDoF(ji, 0, JointLimitUpper)) axisLimitsD = slmath.Min3(loTemp, upTemp) axisLimitsA = slmath.Max3(loTemp, upTemp) - ke := JointStiff(ji) - kd := JointDamp(ji) - targetPos := JointTargetPos(ji) - targetVel := JointTargetVel(ji) - if ke.X > 0.0 { // has position control - UpdateJointAxisWeightedTarget(axis, targetPos.X, ke.X, &axisTargetPosKeD, &axisTargetPosKeA) + ke := JointDoF(ji, 0, JointStiff) + kd := JointDoF(ji, 0, JointDamp) + targetPos := GetJointTargetPos(ji, 0) + targetVel := GetJointTargetVel(ji, 0) + if ke > 0.0 { // has position control + UpdateJointAxisWeightedTarget(axis, targetPos, ke, &axisTargetPosKeD, &axisTargetPosKeA) } - if kd.X > 0.0 { // has velocity control - UpdateJointAxisWeightedTarget(axis, targetVel.X, kd.X, &axisTargetVelKdD, &axisTargetVelKdA) + if kd > 0.0 { // has velocity control + UpdateJointAxisWeightedTarget(axis, targetVel, kd, &axisTargetVelKdD, &axisTargetVelKdA) } // if lin_axis_count > 1: // axis_idx = axis_start + 1 diff --git a/physics/step_joint.goal b/physics/step_joint.goal index 14e76841..8a11b3f2 100644 --- a/physics/step_joint.goal +++ b/physics/step_joint.goal @@ -22,6 +22,7 @@ func StepJointForces(i uint32) { //gosl:kernel if ji >= params.JointsN { return } + // ndof := JointDoFN(ji) // todo: enabled jpi := JointParentIndex(ji) jpbi := int32(-1) @@ -58,22 +59,17 @@ func StepJointForces(i uint32) { //gosl:kernel comc := BodyCom(jcbi) rc := xwcP.Sub(slmath.MulQPPoint(posecP, posecQ, comc)) // child moment arm - // from controls: - jf := JointControlForce(ji) - jtq := JointControlTorque(ji) - var f, t math32.Vector3 switch jt { case Free, Distance: - // todo: distance doesn't seem to be supported here? - f = jf - t = jtq + f = math32.Vec3(GetJointControlForce(ji, 0), GetJointControlForce(ji, 1), GetJointControlForce(ji, 2)) + t = math32.Vec3(GetJointControlForce(ji, 3), GetJointControlForce(ji, 4), GetJointControlForce(ji, 5)) case Ball: - t = jtq + t = math32.Vec3(GetJointControlForce(ji, 0), GetJointControlForce(ji, 1), GetJointControlForce(ji, 2)) case Revolute, Prismatic: - axis := JointAxis(ji) + axis := JointAxis(ji, 0) ap := slmath.MulQuatVector(xwpQ, axis) - f = f.Add(slmath.MulScalar3(ap, jf.X)) + f = slmath.MulScalar3(ap, GetJointControlForce(ji, 0)) default: // todo: D6 requires more iteration! } @@ -234,24 +230,20 @@ func StepSolveJoints(i uint32) { //gosl:kernel var axisTargetPosKeD, axisTargetPosKeA math32.Vector3 var axisTargetVelKdD, axisTargetVelKdA math32.Vector3 - // axis_target_pos_ke = wp.spatial_vector() - // axis_target_vel_kd = wp.spatial_vector() - // avoid a for loop here since local variables would need to be modified - // which is not yet differentiable - axis := JointAxis(ji) - loTemp := axis.MulScalar(Joints[ji, JointLimitLower]) - upTemp := axis.MulScalar(Joints[ji, JointLimitUpper]) + axis := JointAxis(ji, 0) + loTemp := axis.MulScalar(JointDoF(ji, 0, JointLimitLower)) + upTemp := axis.MulScalar(JointDoF(ji, 0, JointLimitUpper)) axisLimitsD = slmath.Min3(loTemp, upTemp) axisLimitsA = slmath.Max3(loTemp, upTemp) - ke := JointStiff(ji) - kd := JointDamp(ji) - targetPos := JointTargetPos(ji) - targetVel := JointTargetVel(ji) - if ke.X > 0.0 { // has position control - UpdateJointAxisWeightedTarget(axis, targetPos.X, ke.X, &axisTargetPosKeD, &axisTargetPosKeA) + ke := JointDoF(ji, 0, JointStiff) + kd := JointDoF(ji, 0, JointDamp) + targetPos := GetJointTargetPos(ji, 0) + targetVel := GetJointTargetVel(ji, 0) + if ke > 0.0 { // has position control + UpdateJointAxisWeightedTarget(axis, targetPos, ke, &axisTargetPosKeD, &axisTargetPosKeA) } - if kd.X > 0.0 { // has velocity control - UpdateJointAxisWeightedTarget(axis, targetVel.X, kd.X, &axisTargetVelKdD, &axisTargetVelKdA) + if kd > 0.0 { // has velocity control + UpdateJointAxisWeightedTarget(axis, targetVel, kd, &axisTargetVelKdD, &axisTargetVelKdA) } // if lin_axis_count > 1: // axis_idx = axis_start + 1 diff --git a/physics/typegen.go b/physics/typegen.go index 1a8e6966..a1eab1d6 100644 --- a/physics/typegen.go +++ b/physics/typegen.go @@ -12,20 +12,22 @@ var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.BodyVars", I var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.ContactVars", IDName: "contact-vars", Doc: "Contact is one pairwise point of contact between two bodies.\nContacts are represented in spherical terms relative to the\nspherical BBox of A and B."}) -var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.JointControlVars", IDName: "joint-control-vars", Doc: "JointControlVars are external joint control input variables stored in tensor.Float32.\nThese must be in one-to-one correspondence with the joints.\nThe X,Y,Z indexes are used progressively for the increasing degrees of freedom for\nthe joints."}) +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.JointControlVars", IDName: "joint-control-vars", Doc: "JointControlVars are external joint control input variables stored in tensor.Float32.\nThese must be in one-to-one correspondence with the JointDoFs."}) var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.DynamicVars", IDName: "dynamic-vars", Doc: "DynamicVars are dynamic body variables stored in tensor.Float32."}) var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.GPUVars", IDName: "gpu-vars", Doc: "GPUVars is an enum for GPU variables, for specifying what to sync."}) +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.JointTypes", IDName: "joint-types", Doc: "JointTypes are joint types that determine nature of interaction."}) + var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.JointVars", IDName: "joint-vars", Doc: "JointVars are joint state variables stored in tensor.Float32.\nThese are all static joint properties; dynamic control variables\nin [JointControlVars] and [JointControls]."}) -var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.JointTypes", IDName: "joint-types", Doc: "JointTypes are joint types that determine nature of interaction."}) +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.JointDoFVars", IDName: "joint-do-f-vars", Doc: "JointDoFVars are joint DoF state variables stored in tensor.Float32,\none for each DoF."}) -var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.PhysParams", IDName: "phys-params", Doc: "PhysParams are the physics parameters", Directives: []types.Directive{{Tool: "gosl", Directive: "start"}}, Fields: []types.Field{{Name: "Iterations", Doc: "Iterations is the number of iterations to perform."}, {Name: "Dt", Doc: "Dt is the integration stepsize."}, {Name: "SoftRelax", Doc: "SoftRelax is soft-body relaxation constant."}, {Name: "JointLinearRelax", Doc: "JointLinearRelax is joint linear relaxation constant."}, {Name: "JointAngularRelax", Doc: "JointAngularRelax is joint angular relaxation constant."}, {Name: "JointLinearComply", Doc: "JointLinearComply is joint linear compliance constant."}, {Name: "JointAngularComply", Doc: "JointAngularComply is joint angular compliance constant."}, {Name: "ContactRelax", Doc: "ContactRelax is rigid contact relaxation constant."}, {Name: "AngularDamping", Doc: "AngularDamping is damping of angular motion."}, {Name: "ContactWeighting", Doc: "Contact weighting"}, {Name: "Restitution", Doc: "Restitution"}, {Name: "DynamicsN", Doc: "DynamicsN is number of dynamics bodies."}, {Name: "JointsN", Doc: "JointsN is number of joints."}, {Name: "BodyJointsMax", Doc: "BodyJointsMax is max number of joints per body + 1 for actual n."}, {Name: "Cur", Doc: "Index for the current state (0 or 1, alternates with Next)."}, {Name: "Next", Doc: "Index for the next state (1 or 0, alternates with Cur)."}, {Name: "Gravity", Doc: "Gravity is the gravity acceleration function"}}}) +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.PhysParams", IDName: "phys-params", Doc: "PhysParams are the physics parameters", Directives: []types.Directive{{Tool: "gosl", Directive: "start"}}, Fields: []types.Field{{Name: "Iterations", Doc: "Iterations is the number of iterations to perform."}, {Name: "Dt", Doc: "Dt is the integration stepsize."}, {Name: "SoftRelax", Doc: "SoftRelax is soft-body relaxation constant."}, {Name: "JointLinearRelax", Doc: "JointLinearRelax is joint linear relaxation constant."}, {Name: "JointAngularRelax", Doc: "JointAngularRelax is joint angular relaxation constant."}, {Name: "JointLinearComply", Doc: "JointLinearComply is joint linear compliance constant."}, {Name: "JointAngularComply", Doc: "JointAngularComply is joint angular compliance constant."}, {Name: "ContactRelax", Doc: "ContactRelax is rigid contact relaxation constant."}, {Name: "AngularDamping", Doc: "AngularDamping is damping of angular motion."}, {Name: "ContactWeighting", Doc: "Contact weighting"}, {Name: "Restitution", Doc: "Restitution"}, {Name: "DynamicsN", Doc: "DynamicsN is number of dynamics bodies."}, {Name: "JointsN", Doc: "JointsN is number of joints."}, {Name: "JointDoFsN", Doc: "JointDoFsN is number of joint DoFs."}, {Name: "BodyJointsMax", Doc: "BodyJointsMax is max number of joints per body + 1 for actual n."}, {Name: "Cur", Doc: "Index for the current state (0 or 1, alternates with Next)."}, {Name: "Next", Doc: "Index for the next state (1 or 0, alternates with Cur)."}, {Name: "pad"}, {Name: "pad1"}, {Name: "pad2"}, {Name: "Gravity", Doc: "Gravity is the gravity acceleration function"}}}) var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.Shapes", IDName: "shapes", Doc: "Shapes are elemental shapes for rigid bodies."}) var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.State", IDName: "state", Doc: "State contains the basic physical state including position, orientation, velocity.\nThese are only the values that can be either relative or absolute -- other physical\nstate values such as Mass should go in Rigid.", Methods: []types.Method{{Name: "MoveOnAxis", Doc: "MoveOnAxis moves (translates) the specified distance on the specified local axis,\nrelative to the current rotation orientation.\nThe axis is normalized prior to aplying the distance factor.\nSets the LinVel to motion vector.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z", "dist"}}, {Name: "MoveOnAxisAbs", Doc: "MoveOnAxisAbs moves (translates) the specified distance on the specified local axis,\nin absolute X,Y,Z coordinates (does not apply the Quat rotation factor.\nThe axis is normalized prior to aplying the distance factor.\nSets the LinVel to motion vector.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z", "dist"}}, {Name: "SetEulerRotation", Doc: "SetEulerRotation sets the rotation in Euler angles (degrees).", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z"}}, {Name: "EulerRotation", Doc: "EulerRotation returns the current rotation in Euler angles (degrees).", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Returns: []string{"Vector3"}}, {Name: "SetAxisRotation", Doc: "SetAxisRotation sets rotation from local axis and angle in degrees.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z", "angle"}}, {Name: "RotateOnAxis", Doc: "RotateOnAxis rotates around the specified local axis the specified angle in degrees.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z", "angle"}}, {Name: "RotateEuler", Doc: "RotateEuler rotates by given Euler angles (in degrees) relative to existing rotation.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z"}}}, Fields: []types.Field{{Name: "Pos", Doc: "Pos is the position of center of mass of object."}, {Name: "Quat", Doc: "Quat is the rotation specified as a quaternion."}, {Name: "LinVel", Doc: "LinVel is the linear velocity."}, {Name: "AngVel", Doc: "AngVel is the angular velocity."}}}) -var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.World", IDName: "world", Doc: "World contains and manages all of the physics elements.", Directives: []types.Directive{{Tool: "go", Directive: "generate", Args: []string{"core", "generate", "-add-types", "-gosl"}}}, Fields: []types.Field{{Name: "GPU", Doc: "GPU determines whether to use GPU (else CPU)."}, {Name: "Params", Doc: "Params are global parameters."}, {Name: "Bodies", Doc: "Bodies are the rigid body elements (dynamic and static),\nspecifying the constant, non-dynamic properties,\nwhich is initial state for dynamics.\n[body][BodyVarsN]"}, {Name: "Joints", Doc: "Joints is a list of permanent joints connecting bodies,\nwhich do not change (no dynamic variables).\n[joint][JointVarsN]"}, {Name: "BodyJoints", Doc: "BodyJoints is a list of joint indexes for each dynamic body, for aggregating.\n[dyn body][parent, child][Params.BodyJointsMax]"}, {Name: "Dynamics", Doc: "Dynamics are the dynamic rigid body elements: these actually move.\nThe first set of variables are for initial values, and the second current.\n[body][cur/next][DynamicVarsN]"}, {Name: "Contacts", Doc: "Contacts are points of contact between bodies.\n[contact][ContactVarsN]"}, {Name: "JointControls", Doc: "JointControls are dynamic joint control inputs.\n[joint][JointControlVarsN]"}}}) +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.World", IDName: "world", Doc: "World contains and manages all of the physics elements.", Directives: []types.Directive{{Tool: "go", Directive: "generate", Args: []string{"core", "generate", "-add-types", "-gosl"}}}, Fields: []types.Field{{Name: "GPU", Doc: "GPU determines whether to use GPU (else CPU)."}, {Name: "Params", Doc: "Params are global parameters."}, {Name: "Bodies", Doc: "Bodies are the rigid body elements (dynamic and static),\nspecifying the constant, non-dynamic properties,\nwhich is initial state for dynamics.\n[body][BodyVarsN]"}, {Name: "Joints", Doc: "Joints is a list of permanent joints connecting bodies,\nwhich do not change (no dynamic variables).\n[joint][JointVarsN]"}, {Name: "JointDoFs", Doc: "JointDoFs is a list of joint DoF parameters, allocated per joint.\n[dof][JointDoFVars]"}, {Name: "BodyJoints", Doc: "BodyJoints is a list of joint indexes for each dynamic body, for aggregating.\n[dyn body][parent, child][Params.BodyJointsMax]"}, {Name: "Dynamics", Doc: "Dynamics are the dynamic rigid body elements: these actually move.\nThe first set of variables are for initial values, and the second current.\n[body][cur/next][DynamicVarsN]"}, {Name: "Contacts", Doc: "Contacts are points of contact between bodies.\n[contact][ContactVarsN]"}, {Name: "JointControls", Doc: "JointControls are dynamic joint control inputs, per joint DoF\n(in correspondence with [JointDoFs]). This can be uploaded to the\nGPU at every step.\n[dof][JointControlVarsN]"}}}) diff --git a/physics/vars.go b/physics/vars.go index 31df9788..428feba8 100644 --- a/physics/vars.go +++ b/physics/vars.go @@ -33,6 +33,11 @@ var ( //gosl:dims 2 Joints *tensor.Float32 + // JointDoFs is a list of joint DoF parameters, allocated per joint. + // [dof][JointDoFVars] + //gosl:dims 2 + JointDoFs *tensor.Float32 + // BodyJoints is a list of joint indexes for each dynamic body, for aggregating. // [dyn body][parent, child][maxjointsperbody] //gosl:dims 3 @@ -50,8 +55,10 @@ var ( //gosl:dims 2 Contacts *tensor.Float32 - // JointControls are dynamic joint control inputs. - // [joint][JointControlVarsN] + // JointControls are dynamic joint control inputs, per joint DoF + // (in correspondence with [JointDoFs]). This can be uploaded to the + // GPU at every step. + // [dof][JointControlVarsN] //gosl:group Controls //gosl:dims 2 JointControls *tensor.Float32 diff --git a/physics/world.go b/physics/world.go index 8048daa7..d049be4d 100644 --- a/physics/world.go +++ b/physics/world.go @@ -32,6 +32,10 @@ type World struct { // [joint][JointVarsN] Joints *tensor.Float32 `display:"no-inline"` + // JointDoFs is a list of joint DoF parameters, allocated per joint. + // [dof][JointDoFVars] + JointDoFs *tensor.Float32 `display:"no-inline"` + // BodyJoints is a list of joint indexes for each dynamic body, for aggregating. // [dyn body][parent, child][Params.BodyJointsMax] BodyJoints *tensor.Int32 `display:"no-inline"` @@ -45,8 +49,10 @@ type World struct { // [contact][ContactVarsN] Contacts *tensor.Float32 `display:"no-inline"` - // JointControls are dynamic joint control inputs. - // [joint][JointControlVarsN] + // JointControls are dynamic joint control inputs, per joint DoF + // (in correspondence with [JointDoFs]). This can be uploaded to the + // GPU at every step. + // [dof][JointControlVarsN] JointControls *tensor.Float32 `display:"no-inline"` } @@ -62,6 +68,7 @@ func (wl *World) Init() { wl.Params[0].Defaults() wl.Bodies = tensor.NewFloat32(0, int(BodyVarsN)) wl.Joints = tensor.NewFloat32(0, int(JointVarsN)) + wl.JointDoFs = tensor.NewFloat32(0, int(JointDoFVarsN)) wl.BodyJoints = tensor.NewInt32(0, 2, 2) wl.Dynamics = tensor.NewFloat32(0, 2, int(DynamicVarsN)) wl.Contacts = tensor.NewFloat32(0, int(ContactVarsN)) @@ -95,26 +102,42 @@ func (wl *World) NewDynamic(shape Shapes, mass float32, size, pos math32.Vector3 return } -// NewJoint adds a new joint between parent and child dynamic object indexes. +// NewJoint1D adds a new 1 DoF joint between parent and child dynamic object indexes, +// for Prismatic and Revolute joint types. // Use -1 for parent to add a world-anchored joint. // ppos, cpos are the relative positions from the parent, child. // Sets relative rotation matricies to identity by default. // axis is the axis of articulation for the joint. -func (wl *World) NewJoint(joint JointTypes, parent, child int32, ppos, cpos, axis math32.Vector3) int32 { +// Use [JointDoFIndex] to access the remaining DoF parameters. +func (wl *World) NewJoint1D(joint JointTypes, parent, child int32, ppos, cpos, axis math32.Vector3) int32 { sizes := wl.Joints.ShapeSizes() idx := int32(sizes[0]) wl.Joints.SetShapeSizes(int(idx+1), int(JointVarsN)) - wl.JointControls.SetShapeSizes(int(idx+1), int(JointControlVarsN)) SetJointParent(idx, parent) SetJointChild(idx, child) SetJointPPos(idx, ppos) SetJointCPos(idx, cpos) - SetJointAxis(idx, axis) + SetJointDoFN(idx, 1) + didx := wl.newJointDoF(0, axis) + SetJointDoFIndex(idx, 0, didx) wl.JointDefaults(idx) wl.Params[0].JointsN = idx + 1 return idx } +// newJointDoF adds new JointDoFs and JointControls entries +// initialized to detfaults. Returns index. +func (wl *World) newJointDoF(dof int32, axis math32.Vector3) int32 { + sizes := wl.JointDoFs.ShapeSizes() + idx := int32(sizes[0]) + wl.JointDoFs.SetShapeSizes(int(idx+1), int(JointDoFVarsN)) + wl.JointControls.SetShapeSizes(int(idx+1), int(JointControlVarsN)) + wl.JointDoFDefaults(idx) + SetJointAxis(idx, 0, axis) + wl.Params[0].JointDoFsN = idx + 1 + return idx +} + // SetAsCurrent sets these as the current global values that are // processed in the code (on the GPU). If this was not the setter of // the current variables, then the parameter variables are copied up @@ -134,6 +157,7 @@ func (wl *World) SetAsCurrentVars() { Bodies = wl.Bodies Joints = wl.Joints BodyJoints = wl.BodyJoints + JointDoFs = wl.JointDoFs Dynamics = wl.Dynamics Contacts = wl.Contacts JointControls = wl.JointControls @@ -151,5 +175,5 @@ func (wl *World) GPUInit() { // the GPU. This is done in GPUInit, and if current switched. func (wl *World) ToGPUInfra() { ToGPUTensorStrides() - ToGPU(ParamsVar, BodiesVar, JointsVar, BodyJointsVar) + ToGPU(ParamsVar, BodiesVar, JointsVar, BodyJointsVar, JointDoFsVar) } diff --git a/physics/world.goal b/physics/world.goal index 771f1764..d8fc6c97 100644 --- a/physics/world.goal +++ b/physics/world.goal @@ -30,6 +30,10 @@ type World struct { // [joint][JointVarsN] Joints *tensor.Float32 `display:"no-inline"` + // JointDoFs is a list of joint DoF parameters, allocated per joint. + // [dof][JointDoFVars] + JointDoFs *tensor.Float32 `display:"no-inline"` + // BodyJoints is a list of joint indexes for each dynamic body, for aggregating. // [dyn body][parent, child][Params.BodyJointsMax] BodyJoints *tensor.Int32 `display:"no-inline"` @@ -43,8 +47,10 @@ type World struct { // [contact][ContactVarsN] Contacts *tensor.Float32 `display:"no-inline"` - // JointControls are dynamic joint control inputs. - // [joint][JointControlVarsN] + // JointControls are dynamic joint control inputs, per joint DoF + // (in correspondence with [JointDoFs]). This can be uploaded to the + // GPU at every step. + // [dof][JointControlVarsN] JointControls *tensor.Float32 `display:"no-inline"` } @@ -60,6 +66,7 @@ func (wl *World) Init() { wl.Params[0].Defaults() wl.Bodies = tensor.NewFloat32(0, int(BodyVarsN)) wl.Joints = tensor.NewFloat32(0, int(JointVarsN)) + wl.JointDoFs = tensor.NewFloat32(0, int(JointDoFVarsN)) wl.BodyJoints = tensor.NewInt32(0, 2, 2) wl.Dynamics = tensor.NewFloat32(0, 2, int(DynamicVarsN)) wl.Contacts = tensor.NewFloat32(0, int(ContactVarsN)) @@ -93,26 +100,42 @@ func (wl *World) NewDynamic(shape Shapes, mass float32, size, pos math32.Vector3 return } -// NewJoint adds a new joint between parent and child dynamic object indexes. +// NewJoint1D adds a new 1 DoF joint between parent and child dynamic object indexes, +// for Prismatic and Revolute joint types. // Use -1 for parent to add a world-anchored joint. // ppos, cpos are the relative positions from the parent, child. // Sets relative rotation matricies to identity by default. // axis is the axis of articulation for the joint. -func (wl *World) NewJoint(joint JointTypes, parent, child int32, ppos, cpos, axis math32.Vector3) int32 { +// Use [JointDoFIndex] to access the remaining DoF parameters. +func (wl *World) NewJoint1D(joint JointTypes, parent, child int32, ppos, cpos, axis math32.Vector3) int32 { sizes := wl.Joints.ShapeSizes() idx := int32(sizes[0]) wl.Joints.SetShapeSizes(int(idx+1), int(JointVarsN)) - wl.JointControls.SetShapeSizes(int(idx+1), int(JointControlVarsN)) SetJointParent(idx, parent) SetJointChild(idx, child) SetJointPPos(idx, ppos) SetJointCPos(idx, cpos) - SetJointAxis(idx, axis) + SetJointDoFN(idx, 1) + didx := wl.newJointDoF(0, axis) + SetJointDoFIndex(idx, 0, didx) wl.JointDefaults(idx) wl.Params[0].JointsN = idx + 1 return idx } +// newJointDoF adds new JointDoFs and JointControls entries +// initialized to detfaults. Returns index. +func (wl *World) newJointDoF(dof int32, axis math32.Vector3) int32 { + sizes := wl.JointDoFs.ShapeSizes() + idx := int32(sizes[0]) + wl.JointDoFs.SetShapeSizes(int(idx+1), int(JointDoFVarsN)) + wl.JointControls.SetShapeSizes(int(idx+1), int(JointControlVarsN)) + wl.JointDoFDefaults(idx) + SetJointAxis(idx, 0, axis) + wl.Params[0].JointDoFsN = idx + 1 + return idx +} + // SetAsCurrent sets these as the current global values that are // processed in the code (on the GPU). If this was not the setter of // the current variables, then the parameter variables are copied up @@ -132,6 +155,7 @@ func (wl *World) SetAsCurrentVars() { Bodies = wl.Bodies Joints = wl.Joints BodyJoints = wl.BodyJoints + JointDoFs = wl.JointDoFs Dynamics = wl.Dynamics Contacts = wl.Contacts JointControls = wl.JointControls @@ -149,5 +173,5 @@ func (wl *World) GPUInit() { // the GPU. This is done in GPUInit, and if current switched. func (wl *World) ToGPUInfra() { ToGPUTensorStrides() - ToGPU(ParamsVar, BodiesVar, JointsVar, BodyJointsVar) + ToGPU(ParamsVar, BodiesVar, JointsVar, BodyJointsVar, JointDoFsVar) } From 811866702705fa09647f005c43d09c4632f66abb Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Wed, 17 Dec 2025 13:36:30 +0100 Subject: [PATCH 21/97] physics: joint code fully implemented and nominally initially working --- gosl/slmath/quaternion.go | 64 +- gosl/slmath/vector3.go | 39 ++ physics/README.md | 2 +- physics/body.go | 22 +- physics/body.goal | 22 +- physics/config.go | 4 +- physics/config.goal | 4 +- physics/control.go | 27 +- physics/control.goal | 27 +- physics/dynamics.go | 22 +- physics/dynamics.goal | 22 +- physics/enumgen.go | 18 +- physics/examples/test1/test1.go | 7 +- physics/joint.go | 195 +++++- physics/joint.goal | 195 +++++- physics/shaders/DeltasFromJoints.wgsl | 97 +-- physics/shaders/DynamicsCurToNext.wgsl | 97 +-- physics/shaders/ForcesFromJoints.wgsl | 97 +-- physics/shaders/InitDynamics.wgsl | 105 +-- physics/shaders/StepBodyDeltas.wgsl | 115 ++-- physics/shaders/StepIntegrateBodies.wgsl | 115 ++-- physics/shaders/StepJointForces.wgsl | 204 +++--- physics/shaders/StepSolveJoints.wgsl | 552 ++++++++++----- physics/step_body.go | 18 +- physics/step_body.goal | 18 +- physics/step_joint.go | 816 +++++++++++------------ physics/step_joint.goal | 808 +++++++++++----------- physics/world.go | 38 +- physics/world.goal | 38 +- physics/world/view.go | 14 +- physics/world/world.go | 2 +- 31 files changed, 2102 insertions(+), 1702 deletions(-) diff --git a/gosl/slmath/quaternion.go b/gosl/slmath/quaternion.go index fa12e1f6..c368babe 100644 --- a/gosl/slmath/quaternion.go +++ b/gosl/slmath/quaternion.go @@ -15,7 +15,7 @@ func QuatLength(q math32.Quat) float32 { // QuatNormalize normalizes the quaternion. func QuatNormalize(q math32.Quat) math32.Quat { - var nq math32.Quat + nq := q l := QuatLength(q) if l == 0 { nq.X = 0 @@ -59,21 +59,21 @@ func MulQuats(a, b math32.Quat) math32.Quat { return q } -// MulQPTransforms computes the equivalent of matrix multiplication for +// MulQRTransforms computes the equivalent of matrix multiplication for // two quat-point spatial transforms: o = a * b -func MulQPTransforms(aP math32.Vector3, aQ math32.Quat, bP math32.Vector3, bQ math32.Quat, oP *math32.Vector3, oQ *math32.Quat) { +func MulSpatialTransforms(aP math32.Vector3, aQ math32.Quat, bP math32.Vector3, bQ math32.Quat, oP *math32.Vector3, oQ *math32.Quat) { // rotate b by a and add a *oP = MulQuatVector(aQ, bP).Add(aP) *oQ = MulQuats(aQ, bQ) } -// MulQPPoint applies quat-point spatial transform to given 3D point. -func MulQPPoint(xP math32.Vector3, xQ math32.Quat, p math32.Vector3) math32.Vector3 { +// MulSpatialPoint applies quat-point spatial transform to given 3D point. +func MulSpatialPoint(xP math32.Vector3, xQ math32.Quat, p math32.Vector3) math32.Vector3 { dp := MulQuatVector(xQ, p) return dp.Add(xP) } -func QPTransformInverse(p math32.Vector3, q math32.Quat, oP *math32.Vector3, oQ *math32.Quat) { +func SpatialTransformInverse(p math32.Vector3, q math32.Quat, oP *math32.Vector3, oQ *math32.Quat) { qi := QuatInverse(q) *oP = Negate3(MulQuatVector(qi, p)) *oQ = qi @@ -87,6 +87,58 @@ func QuatInverse(q math32.Quat) math32.Quat { return QuatNormalize(nq) } +func QuatDot(q, o math32.Quat) float32 { + return q.X*o.X + q.Y*o.Y + q.Z*o.Z + q.W*o.W +} + +func QuatAdd(q math32.Quat, o math32.Quat) math32.Quat { + nq := q + nq.X += o.X + nq.Y += o.Y + nq.Z += o.Z + nq.W += o.W + return nq +} + +func QuatMulScalar(q math32.Quat, s float32) math32.Quat { + nq := q + nq.X *= s + nq.Y *= s + nq.Z *= s + nq.W *= s + return nq +} + +func QuatDim(v math32.Quat, dim int32) float32 { + if dim == 0 { + return v.X + } + if dim == 1 { + return v.Y + } + if dim == 2 { + return v.Z + } + return v.W +} + +func QuatSetDim(v math32.Quat, dim int32, val float32) math32.Quat { + nv := v + if dim == 0 { + nv.X = val + } + if dim == 1 { + nv.Y = val + } + if dim == 3 { + nv.Z = val + } + if dim == 4 { + nv.W = val + } + return nv +} + func QuatToMatrix3(q math32.Quat) math32.Matrix3 { var m math32.Matrix3 x := q.X diff --git a/gosl/slmath/vector3.go b/gosl/slmath/vector3.go index 6993575b..2a23dbdc 100644 --- a/gosl/slmath/vector3.go +++ b/gosl/slmath/vector3.go @@ -24,6 +24,21 @@ func DivScalar3(v math32.Vector3, s float32) math32.Vector3 { return math32.Vec3(v.X/s, v.Y/s, v.Z/s) } +// DivSafe3 divides v by o elementwise, only where o != 0 +func DivSafe3(v math32.Vector3, o math32.Vector3) math32.Vector3 { + nv := v + if o.X != 0 { + nv.X /= o.X + } + if o.Y != 0 { + nv.Y /= o.Y + } + if o.Z != 0 { + nv.Z /= o.Z + } + return nv +} + func Negate3(v math32.Vector3) math32.Vector3 { return math32.Vec3(-v.X, -v.Y, -v.Z) } @@ -87,4 +102,28 @@ func Cross3(v, o math32.Vector3) math32.Vector3 { return math32.Vec3(v.Y*o.Z-v.Z*o.Y, v.Z*o.X-v.X*o.Z, v.X*o.Y-v.Y*o.X) } +func Dim3(v math32.Vector3, dim int32) float32 { + if dim == 0 { + return v.X + } + if dim == 1 { + return v.Y + } + return v.Z +} + +func SetDim3(v math32.Vector3, dim int32, val float32) math32.Vector3 { + nv := v + if dim == 0 { + nv.X = val + } + if dim == 1 { + nv.Y = val + } + if dim == 3 { + nv.Z = val + } + return nv +} + //gosl:end diff --git a/physics/README.md b/physics/README.md index 55d66e2e..4ef2f78a 100644 --- a/physics/README.md +++ b/physics/README.md @@ -69,7 +69,7 @@ The `Group` has a set of `World*` methods that should be used on the top-level w ### Scripted Mode -For Scripted mode, each update step typically involves manually updating the `Rel.Pos` and `.Quat` fields on `Body` objects to update their relative positions. This field is a `State` type and has `MoveOnAxis` and `RotateOnAxis` (and a number of other rotation methods). The Move methods update the `LinVel` field to reflect any delta in movement. +For Scripted mode, each update step typically involves manually updating the `Rel.Pos` and `.Quat` fields on `Body` objects to update their relative positions. This field is a `State` type and has `MoveOnAxis` and `QuatateOnAxis` (and a number of other rotation methods). The Move methods update the `LinVel` field to reflect any delta in movement. It is also possible to manually set the `Abs.LinVel` and `Abs.AngVel` fields and call `Step` to update. diff --git a/physics/body.go b/physics/body.go index 44d003a2..3ac71393 100644 --- a/physics/body.go +++ b/physics/body.go @@ -52,10 +52,10 @@ const ( BodyPosZ // Quaternion rotation of body. - BodyRotX - BodyRotY - BodyRotZ - BodyRotW + BodyQuatX + BodyQuatY + BodyQuatZ + BodyQuatW // Relative center-of-mass offset from 3D position of body. BodyComX @@ -113,15 +113,15 @@ func SetBodyPos(idx int32, pos math32.Vector3) { Bodies.Set(pos.Z, int(idx), int(BodyPosZ)) } -func BodyRot(idx int32) math32.Quat { - return math32.NewQuat(Bodies.Value(int(idx), int(BodyRotX)), Bodies.Value(int(idx), int(BodyRotY)), Bodies.Value(int(idx), int(BodyRotZ)), Bodies.Value(int(idx), int(BodyRotW))) +func BodyQuat(idx int32) math32.Quat { + return math32.NewQuat(Bodies.Value(int(idx), int(BodyQuatX)), Bodies.Value(int(idx), int(BodyQuatY)), Bodies.Value(int(idx), int(BodyQuatZ)), Bodies.Value(int(idx), int(BodyQuatW))) } -func SetBodyRot(idx int32, rot math32.Quat) { - Bodies.Set(rot.X, int(idx), int(BodyRotX)) - Bodies.Set(rot.Y, int(idx), int(BodyRotY)) - Bodies.Set(rot.Z, int(idx), int(BodyRotZ)) - Bodies.Set(rot.W, int(idx), int(BodyRotW)) +func SetBodyQuat(idx int32, rot math32.Quat) { + Bodies.Set(rot.X, int(idx), int(BodyQuatX)) + Bodies.Set(rot.Y, int(idx), int(BodyQuatY)) + Bodies.Set(rot.Z, int(idx), int(BodyQuatZ)) + Bodies.Set(rot.W, int(idx), int(BodyQuatW)) } func BodyCom(idx int32) math32.Vector3 { diff --git a/physics/body.goal b/physics/body.goal index a99f5b5f..0c05e969 100644 --- a/physics/body.goal +++ b/physics/body.goal @@ -50,10 +50,10 @@ const ( BodyPosZ // Quaternion rotation of body. - BodyRotX - BodyRotY - BodyRotZ - BodyRotW + BodyQuatX + BodyQuatY + BodyQuatZ + BodyQuatW // Relative center-of-mass offset from 3D position of body. BodyComX @@ -111,15 +111,15 @@ func SetBodyPos(idx int32, pos math32.Vector3) { Bodies[idx, BodyPosZ] = pos.Z } -func BodyRot(idx int32) math32.Quat { - return math32.NewQuat(Bodies[idx, BodyRotX], Bodies[idx, BodyRotY], Bodies[idx, BodyRotZ], Bodies[idx, BodyRotW]) +func BodyQuat(idx int32) math32.Quat { + return math32.NewQuat(Bodies[idx, BodyQuatX], Bodies[idx, BodyQuatY], Bodies[idx, BodyQuatZ], Bodies[idx, BodyQuatW]) } -func SetBodyRot(idx int32, rot math32.Quat) { - Bodies[idx, BodyRotX] = rot.X - Bodies[idx, BodyRotY] = rot.Y - Bodies[idx, BodyRotZ] = rot.Z - Bodies[idx, BodyRotW] = rot.W +func SetBodyQuat(idx int32, rot math32.Quat) { + Bodies[idx, BodyQuatX] = rot.X + Bodies[idx, BodyQuatY] = rot.Y + Bodies[idx, BodyQuatZ] = rot.Z + Bodies[idx, BodyQuatW] = rot.W } func BodyCom(idx int32) math32.Vector3 { diff --git a/physics/config.go b/physics/config.go index f05a78cd..02e12531 100644 --- a/physics/config.go +++ b/physics/config.go @@ -12,7 +12,6 @@ package physics // after everything has been added. Does SetAsCurrent, GPUInit. func (wl *World) Config() { wl.ConfigJoints() - // todo: other things wl.SetAsCurrent() wl.GPUInit() wl.InitState() @@ -24,11 +23,10 @@ func (wl *World) ConfigJoints() { params := &wl.Params[0] nj := params.JointsN nd := params.DynamicsN + bjp := make([][]int32, nd) bjc := make([][]int32, nd) - maxi := 0 - for ji := range nj { jpi := JointParentIndex(ji) jci := JointChildIndex(ji) diff --git a/physics/config.goal b/physics/config.goal index c208e8c0..65f103a8 100644 --- a/physics/config.goal +++ b/physics/config.goal @@ -10,7 +10,6 @@ package physics // after everything has been added. Does SetAsCurrent, GPUInit. func (wl *World) Config() { wl.ConfigJoints() - // todo: other things wl.SetAsCurrent() wl.GPUInit() wl.InitState() @@ -22,11 +21,10 @@ func (wl *World) ConfigJoints() { params := &wl.Params[0] nj := params.JointsN nd := params.DynamicsN + bjp := make([][]int32, nd) bjc := make([][]int32, nd) - maxi := 0 - for ji := range nj { jpi := JointParentIndex(ji) jci := JointChildIndex(ji) diff --git a/physics/control.go b/physics/control.go index cdec59f2..66246dfd 100644 --- a/physics/control.go +++ b/physics/control.go @@ -19,30 +19,29 @@ const ( JointTargetVel ) -// SetJointControlForce sets the control force for given dof for given joint +// SetJointControl sets the control for given joint, dof and parameter // to given value. -func SetJointControlForce(idx, dof int32, value float32) { - JointControls.Set(value, int(JointDoFIndex(idx, dof)), int(JointControlForce)) +func SetJointControl(idx, dof int32, vr JointControlVars, value float32) { + JointControls.Set(value, int(JointDoFIndex(idx, dof)), int(vr)) } -func GetJointControlForce(idx, dof int32) float32 { - return JointControls.Value(int(JointDoFIndex(idx, dof)), int(JointControlForce)) +func JointControl(idx, dof int32, vr JointControlVars) float32 { + return JointControls.Value(int(JointDoFIndex(idx, dof)), int(vr)) } -func SetJointTargetPos(idx, dof int32, value float32) { - JointControls.Set(value, int(JointDoFIndex(idx, dof)), int(JointTargetPos)) +// SetJointControlForce sets the force for given joint, dof to given value. +func SetJointControlForce(idx, dof int32, value float32) { + SetJointControl(idx, dof, JointControlForce, value) } -func GetJointTargetPos(idx, dof int32) float32 { - return JointControls.Value(int(JointDoFIndex(idx, dof)), int(JointTargetPos)) +// SetJointTargetPos sets the target position for given joint, dof to given value. +func SetJointTargetPos(idx, dof int32, value float32) { + SetJointControl(idx, dof, JointTargetPos, value) } +// SetJointTargetVel sets the target velocity for given joint, dof to given value. func SetJointTargetVel(idx, dof int32, value float32) { - JointControls.Set(value, int(JointDoFIndex(idx, dof)), int(JointTargetVel)) -} - -func GetJointTargetVel(idx, dof int32) float32 { - return JointControls.Value(int(JointDoFIndex(idx, dof)), int(JointTargetVel)) + SetJointControl(idx, dof, JointTargetVel, value) } //gosl:end diff --git a/physics/control.goal b/physics/control.goal index 65999e2b..50f86c15 100644 --- a/physics/control.goal +++ b/physics/control.goal @@ -17,30 +17,29 @@ const ( JointTargetVel ) -// SetJointControlForce sets the control force for given dof for given joint +// SetJointControl sets the control for given joint, dof and parameter // to given value. -func SetJointControlForce(idx, dof int32, value float32) { - JointControls[JointDoFIndex(idx, dof), JointControlForce] = value +func SetJointControl(idx, dof int32, vr JointControlVars, value float32) { + JointControls[JointDoFIndex(idx, dof), vr] = value } -func GetJointControlForce(idx, dof int32) float32 { - return JointControls[JointDoFIndex(idx, dof), JointControlForce] +func JointControl(idx, dof int32, vr JointControlVars) float32 { + return JointControls[JointDoFIndex(idx, dof), vr] } -func SetJointTargetPos(idx, dof int32, value float32) { - JointControls[JointDoFIndex(idx, dof), JointTargetPos] = value +// SetJointControlForce sets the force for given joint, dof to given value. +func SetJointControlForce(idx, dof int32, value float32) { + SetJointControl(idx, dof, JointControlForce, value) } -func GetJointTargetPos(idx, dof int32) float32 { - return JointControls[JointDoFIndex(idx, dof), JointTargetPos] +// SetJointTargetPos sets the target position for given joint, dof to given value. +func SetJointTargetPos(idx, dof int32, value float32) { + SetJointControl(idx, dof, JointTargetPos, value) } +// SetJointTargetVel sets the target velocity for given joint, dof to given value. func SetJointTargetVel(idx, dof int32, value float32) { - JointControls[JointDoFIndex(idx, dof), JointTargetVel] = value -} - -func GetJointTargetVel(idx, dof int32) float32 { - return JointControls[JointDoFIndex(idx, dof), JointTargetVel] + SetJointControl(idx, dof, JointTargetVel, value) } //gosl:end diff --git a/physics/dynamics.go b/physics/dynamics.go index bced7d8d..23363690 100644 --- a/physics/dynamics.go +++ b/physics/dynamics.go @@ -27,10 +27,10 @@ const ( DynPosZ // Quaternion rotation. - DynRotX - DynRotY - DynRotZ - DynRotW + DynQuatX + DynQuatY + DynQuatZ + DynQuatW // Linear velocity. DynVelX @@ -91,15 +91,15 @@ func SetDynamicPos(idx, cni int32, pos math32.Vector3) { Dynamics.Set(pos.Z, int(idx), int(cni), int(DynPosZ)) } -func DynamicRot(idx, cni int32) math32.Quat { - return math32.NewQuat(Dynamics.Value(int(idx), int(cni), int(DynRotX)), Dynamics.Value(int(idx), int(cni), int(DynRotY)), Dynamics.Value(int(idx), int(cni), int(DynRotZ)), Dynamics.Value(int(idx), int(cni), int(DynRotW))) +func DynamicQuat(idx, cni int32) math32.Quat { + return math32.NewQuat(Dynamics.Value(int(idx), int(cni), int(DynQuatX)), Dynamics.Value(int(idx), int(cni), int(DynQuatY)), Dynamics.Value(int(idx), int(cni), int(DynQuatZ)), Dynamics.Value(int(idx), int(cni), int(DynQuatW))) } -func SetDynamicRot(idx, cni int32, rot math32.Quat) { - Dynamics.Set(rot.X, int(idx), int(cni), int(DynRotX)) - Dynamics.Set(rot.Y, int(idx), int(cni), int(DynRotY)) - Dynamics.Set(rot.Z, int(idx), int(cni), int(DynRotZ)) - Dynamics.Set(rot.W, int(idx), int(cni), int(DynRotW)) +func SetDynamicQuat(idx, cni int32, rot math32.Quat) { + Dynamics.Set(rot.X, int(idx), int(cni), int(DynQuatX)) + Dynamics.Set(rot.Y, int(idx), int(cni), int(DynQuatY)) + Dynamics.Set(rot.Z, int(idx), int(cni), int(DynQuatZ)) + Dynamics.Set(rot.W, int(idx), int(cni), int(DynQuatW)) } func DynamicVel(idx, cni int32) math32.Vector3 { diff --git a/physics/dynamics.goal b/physics/dynamics.goal index f361a2dc..460ff98e 100644 --- a/physics/dynamics.goal +++ b/physics/dynamics.goal @@ -25,10 +25,10 @@ const ( DynPosZ // Quaternion rotation. - DynRotX - DynRotY - DynRotZ - DynRotW + DynQuatX + DynQuatY + DynQuatZ + DynQuatW // Linear velocity. DynVelX @@ -89,15 +89,15 @@ func SetDynamicPos(idx, cni int32, pos math32.Vector3) { Dynamics[idx, cni, DynPosZ] = pos.Z } -func DynamicRot(idx, cni int32) math32.Quat { - return math32.NewQuat(Dynamics[idx, cni, DynRotX], Dynamics[idx, cni, DynRotY], Dynamics[idx, cni, DynRotZ], Dynamics[idx, cni, DynRotW]) +func DynamicQuat(idx, cni int32) math32.Quat { + return math32.NewQuat(Dynamics[idx, cni, DynQuatX], Dynamics[idx, cni, DynQuatY], Dynamics[idx, cni, DynQuatZ], Dynamics[idx, cni, DynQuatW]) } -func SetDynamicRot(idx, cni int32, rot math32.Quat) { - Dynamics[idx, cni, DynRotX] = rot.X - Dynamics[idx, cni, DynRotY] = rot.Y - Dynamics[idx, cni, DynRotZ] = rot.Z - Dynamics[idx, cni, DynRotW] = rot.W +func SetDynamicQuat(idx, cni int32, rot math32.Quat) { + Dynamics[idx, cni, DynQuatX] = rot.X + Dynamics[idx, cni, DynQuatY] = rot.Y + Dynamics[idx, cni, DynQuatZ] = rot.Z + Dynamics[idx, cni, DynQuatW] = rot.W } func DynamicVel(idx, cni int32) math32.Vector3 { diff --git a/physics/enumgen.go b/physics/enumgen.go index b44dbf62..abed223b 100644 --- a/physics/enumgen.go +++ b/physics/enumgen.go @@ -15,11 +15,11 @@ const BodyVarsN BodyVars = 37 //gosl:end -var _BodyVarsValueMap = map[string]BodyVars{`BodyShape`: 0, `BodyWorldIndex`: 1, `BodySizeX`: 2, `BodySizeY`: 3, `BodySizeZ`: 4, `BodyMass`: 5, `BodyInvMass`: 6, `BodyBounce`: 7, `BodyFriction`: 8, `BodyPosX`: 9, `BodyPosY`: 10, `BodyPosZ`: 11, `BodyRotX`: 12, `BodyRotY`: 13, `BodyRotZ`: 14, `BodyRotW`: 15, `BodyComX`: 16, `BodyComY`: 17, `BodyComZ`: 18, `BodyInertiaXX`: 19, `BodyInertiaYX`: 20, `BodyInertiaZX`: 21, `BodyInertiaXY`: 22, `BodyInertiaYY`: 23, `BodyInertiaZY`: 24, `BodyInertiaXZ`: 25, `BodyInertiaYZ`: 26, `BodyInertiaZZ`: 27, `BodyInvInertiaXX`: 28, `BodyInvInertiaYX`: 29, `BodyInvInertiaZX`: 30, `BodyInvInertiaXY`: 31, `BodyInvInertiaYY`: 32, `BodyInvInertiaZY`: 33, `BodyInvInertiaXZ`: 34, `BodyInvInertiaYZ`: 35, `BodyInvInertiaZZ`: 36} +var _BodyVarsValueMap = map[string]BodyVars{`BodyShape`: 0, `BodyWorldIndex`: 1, `BodySizeX`: 2, `BodySizeY`: 3, `BodySizeZ`: 4, `BodyMass`: 5, `BodyInvMass`: 6, `BodyBounce`: 7, `BodyFriction`: 8, `BodyPosX`: 9, `BodyPosY`: 10, `BodyPosZ`: 11, `BodyQuatX`: 12, `BodyQuatY`: 13, `BodyQuatZ`: 14, `BodyQuatW`: 15, `BodyComX`: 16, `BodyComY`: 17, `BodyComZ`: 18, `BodyInertiaXX`: 19, `BodyInertiaYX`: 20, `BodyInertiaZX`: 21, `BodyInertiaXY`: 22, `BodyInertiaYY`: 23, `BodyInertiaZY`: 24, `BodyInertiaXZ`: 25, `BodyInertiaYZ`: 26, `BodyInertiaZZ`: 27, `BodyInvInertiaXX`: 28, `BodyInvInertiaYX`: 29, `BodyInvInertiaZX`: 30, `BodyInvInertiaXY`: 31, `BodyInvInertiaYY`: 32, `BodyInvInertiaZY`: 33, `BodyInvInertiaXZ`: 34, `BodyInvInertiaYZ`: 35, `BodyInvInertiaZZ`: 36} var _BodyVarsDescMap = map[BodyVars]string{0: `BodyShape is the shape type of the object, as a Shapes type.`, 1: `BodyWorldIndex partitions body into different worlds; Global are -1`, 2: `BodySize is the size of the object (values depend on shape type).`, 3: ``, 4: ``, 5: `BodyMass is the mass of the object.`, 6: `BodyInvMass is 1/mass of the object or 0 if no mass.`, 7: `BodyBounce specifies the COR or coefficient of restitution (0..1), which determines how elastic the collision is, i.e., final velocity / initial velocity.`, 8: `BodyFriction coefficient: how much friction is generated by transverse motion. Additive across the two surfaces.`, 9: `3D position of body (structural center).`, 10: ``, 11: ``, 12: `Quaternion rotation of body.`, 13: ``, 14: ``, 15: ``, 16: `Relative center-of-mass offset from 3D position of body.`, 17: ``, 18: ``, 19: `Inertia 3x3 matrix (column matrix organization, r,c labels).`, 20: ``, 21: ``, 22: ``, 23: ``, 24: ``, 25: ``, 26: ``, 27: ``, 28: `InvInertia inverse inertia 3x3 matrix (column matrix organization, r,c labels).`, 29: ``, 30: ``, 31: ``, 32: ``, 33: ``, 34: ``, 35: ``, 36: ``} -var _BodyVarsMap = map[BodyVars]string{0: `BodyShape`, 1: `BodyWorldIndex`, 2: `BodySizeX`, 3: `BodySizeY`, 4: `BodySizeZ`, 5: `BodyMass`, 6: `BodyInvMass`, 7: `BodyBounce`, 8: `BodyFriction`, 9: `BodyPosX`, 10: `BodyPosY`, 11: `BodyPosZ`, 12: `BodyRotX`, 13: `BodyRotY`, 14: `BodyRotZ`, 15: `BodyRotW`, 16: `BodyComX`, 17: `BodyComY`, 18: `BodyComZ`, 19: `BodyInertiaXX`, 20: `BodyInertiaYX`, 21: `BodyInertiaZX`, 22: `BodyInertiaXY`, 23: `BodyInertiaYY`, 24: `BodyInertiaZY`, 25: `BodyInertiaXZ`, 26: `BodyInertiaYZ`, 27: `BodyInertiaZZ`, 28: `BodyInvInertiaXX`, 29: `BodyInvInertiaYX`, 30: `BodyInvInertiaZX`, 31: `BodyInvInertiaXY`, 32: `BodyInvInertiaYY`, 33: `BodyInvInertiaZY`, 34: `BodyInvInertiaXZ`, 35: `BodyInvInertiaYZ`, 36: `BodyInvInertiaZZ`} +var _BodyVarsMap = map[BodyVars]string{0: `BodyShape`, 1: `BodyWorldIndex`, 2: `BodySizeX`, 3: `BodySizeY`, 4: `BodySizeZ`, 5: `BodyMass`, 6: `BodyInvMass`, 7: `BodyBounce`, 8: `BodyFriction`, 9: `BodyPosX`, 10: `BodyPosY`, 11: `BodyPosZ`, 12: `BodyQuatX`, 13: `BodyQuatY`, 14: `BodyQuatZ`, 15: `BodyQuatW`, 16: `BodyComX`, 17: `BodyComY`, 18: `BodyComZ`, 19: `BodyInertiaXX`, 20: `BodyInertiaYX`, 21: `BodyInertiaZX`, 22: `BodyInertiaXY`, 23: `BodyInertiaYY`, 24: `BodyInertiaZY`, 25: `BodyInertiaXZ`, 26: `BodyInertiaYZ`, 27: `BodyInertiaZZ`, 28: `BodyInvInertiaXX`, 29: `BodyInvInertiaYX`, 30: `BodyInvInertiaZX`, 31: `BodyInvInertiaXY`, 32: `BodyInvInertiaYY`, 33: `BodyInvInertiaZY`, 34: `BodyInvInertiaXZ`, 35: `BodyInvInertiaYZ`, 36: `BodyInvInertiaZZ`} // String returns the string representation of this BodyVars value. func (i BodyVars) String() string { return enums.String(i, _BodyVarsMap) } @@ -154,11 +154,11 @@ const DynamicVarsN DynamicVars = 32 //gosl:end -var _DynamicVarsValueMap = map[string]DynamicVars{`DynIndex`: 0, `DynPosX`: 1, `DynPosY`: 2, `DynPosZ`: 3, `DynRotX`: 4, `DynRotY`: 5, `DynRotZ`: 6, `DynRotW`: 7, `DynVelX`: 8, `DynVelY`: 9, `DynVelZ`: 10, `DynAngVelX`: 11, `DynAngVelY`: 12, `DynAngVelZ`: 13, `DynAccX`: 14, `DynAccY`: 15, `DynAccZ`: 16, `DynAngAccX`: 17, `DynAngAccY`: 18, `DynAngAccZ`: 19, `DynForceX`: 20, `DynForceY`: 21, `DynForceZ`: 22, `DynTorqueX`: 23, `DynTorqueY`: 24, `DynTorqueZ`: 25, `DynDeltaX`: 26, `DynDeltaY`: 27, `DynDeltaZ`: 28, `DynAngDeltaX`: 29, `DynAngDeltaY`: 30, `DynAngDeltaZ`: 31} +var _DynamicVarsValueMap = map[string]DynamicVars{`DynIndex`: 0, `DynPosX`: 1, `DynPosY`: 2, `DynPosZ`: 3, `DynQuatX`: 4, `DynQuatY`: 5, `DynQuatZ`: 6, `DynQuatW`: 7, `DynVelX`: 8, `DynVelY`: 9, `DynVelZ`: 10, `DynAngVelX`: 11, `DynAngVelY`: 12, `DynAngVelZ`: 13, `DynAccX`: 14, `DynAccY`: 15, `DynAccZ`: 16, `DynAngAccX`: 17, `DynAngAccY`: 18, `DynAngAccZ`: 19, `DynForceX`: 20, `DynForceY`: 21, `DynForceZ`: 22, `DynTorqueX`: 23, `DynTorqueY`: 24, `DynTorqueZ`: 25, `DynDeltaX`: 26, `DynDeltaY`: 27, `DynDeltaZ`: 28, `DynAngDeltaX`: 29, `DynAngDeltaY`: 30, `DynAngDeltaZ`: 31} var _DynamicVarsDescMap = map[DynamicVars]string{0: `Index of body in list of bodies.`, 1: `3D position of center of mass.`, 2: ``, 3: ``, 4: `Quaternion rotation.`, 5: ``, 6: ``, 7: ``, 8: `Linear velocity.`, 9: ``, 10: ``, 11: `Angular velocity.`, 12: ``, 13: ``, 14: `Linear acceleration.`, 15: ``, 16: ``, 17: `Angular acceleration due to applied torques.`, 18: ``, 19: ``, 20: `Linear force driving linear acceleration (from joints, etc).`, 21: ``, 22: ``, 23: `Torque driving angular acceleration (from joints, etc).`, 24: ``, 25: ``, 26: `Linear deltas.`, 27: ``, 28: ``, 29: `Angular deltas.`, 30: ``, 31: ``} -var _DynamicVarsMap = map[DynamicVars]string{0: `DynIndex`, 1: `DynPosX`, 2: `DynPosY`, 3: `DynPosZ`, 4: `DynRotX`, 5: `DynRotY`, 6: `DynRotZ`, 7: `DynRotW`, 8: `DynVelX`, 9: `DynVelY`, 10: `DynVelZ`, 11: `DynAngVelX`, 12: `DynAngVelY`, 13: `DynAngVelZ`, 14: `DynAccX`, 15: `DynAccY`, 16: `DynAccZ`, 17: `DynAngAccX`, 18: `DynAngAccY`, 19: `DynAngAccZ`, 20: `DynForceX`, 21: `DynForceY`, 22: `DynForceZ`, 23: `DynTorqueX`, 24: `DynTorqueY`, 25: `DynTorqueZ`, 26: `DynDeltaX`, 27: `DynDeltaY`, 28: `DynDeltaZ`, 29: `DynAngDeltaX`, 30: `DynAngDeltaY`, 31: `DynAngDeltaZ`} +var _DynamicVarsMap = map[DynamicVars]string{0: `DynIndex`, 1: `DynPosX`, 2: `DynPosY`, 3: `DynPosZ`, 4: `DynQuatX`, 5: `DynQuatY`, 6: `DynQuatZ`, 7: `DynQuatW`, 8: `DynVelX`, 9: `DynVelY`, 10: `DynVelZ`, 11: `DynAngVelX`, 12: `DynAngVelY`, 13: `DynAngVelZ`, 14: `DynAccX`, 15: `DynAccY`, 16: `DynAccZ`, 17: `DynAngAccX`, 18: `DynAngAccY`, 19: `DynAngAccZ`, 20: `DynForceX`, 21: `DynForceY`, 22: `DynForceZ`, 23: `DynTorqueX`, 24: `DynTorqueY`, 25: `DynTorqueZ`, 26: `DynDeltaX`, 27: `DynDeltaY`, 28: `DynDeltaZ`, 29: `DynAngDeltaX`, 30: `DynAngDeltaY`, 31: `DynAngDeltaZ`} // String returns the string representation of this DynamicVars value. func (i DynamicVars) String() string { return enums.String(i, _DynamicVarsMap) } @@ -284,20 +284,20 @@ func (i *JointTypes) UnmarshalText(text []byte) error { return enums.UnmarshalText(i, text, "JointTypes") } -var _JointVarsValues = []JointVars{0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48} +var _JointVarsValues = []JointVars{0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48, 49} // JointVarsN is the highest valid value for type JointVars, plus one. // //gosl:start -const JointVarsN JointVars = 49 +const JointVarsN JointVars = 50 //gosl:end -var _JointVarsValueMap = map[string]JointVars{`JointType`: 0, `JointEnabled`: 1, `JointParent`: 2, `JointChild`: 3, `JointPPosX`: 4, `JointPPosY`: 5, `JointPPosZ`: 6, `JointPRotX`: 7, `JointPRotY`: 8, `JointPRotZ`: 9, `JointPRotW`: 10, `JointCPosX`: 11, `JointCPosY`: 12, `JointCPosZ`: 13, `JointCRotX`: 14, `JointCRotY`: 15, `JointCRotZ`: 16, `JointCRotW`: 17, `JointDoFN`: 18, `JointDoF1`: 19, `JointDoF2`: 20, `JointDoF3`: 21, `JointDoF4`: 22, `JointDoF5`: 23, `JointDoF6`: 24, `JointPForceX`: 25, `JointPForceY`: 26, `JointPForceZ`: 27, `JointPTorqueX`: 28, `JointPTorqueY`: 29, `JointPTorqueZ`: 30, `JointCForceX`: 31, `JointCForceY`: 32, `JointCForceZ`: 33, `JointCTorqueX`: 34, `JointCTorqueY`: 35, `JointCTorqueZ`: 36, `JointPDeltaX`: 37, `JointPDeltaY`: 38, `JointPDeltaZ`: 39, `JointPAngDeltaX`: 40, `JointPAngDeltaY`: 41, `JointPAngDeltaZ`: 42, `JointCDeltaX`: 43, `JointCDeltaY`: 44, `JointCDeltaZ`: 45, `JointCAngDeltaX`: 46, `JointCAngDeltaY`: 47, `JointCAngDeltaZ`: 48} +var _JointVarsValueMap = map[string]JointVars{`JointType`: 0, `JointEnabled`: 1, `JointParent`: 2, `JointChild`: 3, `JointPPosX`: 4, `JointPPosY`: 5, `JointPPosZ`: 6, `JointPQuatX`: 7, `JointPQuatY`: 8, `JointPQuatZ`: 9, `JointPQuatW`: 10, `JointCPosX`: 11, `JointCPosY`: 12, `JointCPosZ`: 13, `JointCQuatX`: 14, `JointCQuatY`: 15, `JointCQuatZ`: 16, `JointCQuatW`: 17, `JointLinearDoFN`: 18, `JointAngularDoFN`: 19, `JointDoF1`: 20, `JointDoF2`: 21, `JointDoF3`: 22, `JointDoF4`: 23, `JointDoF5`: 24, `JointDoF6`: 25, `JointPForceX`: 26, `JointPForceY`: 27, `JointPForceZ`: 28, `JointPTorqueX`: 29, `JointPTorqueY`: 30, `JointPTorqueZ`: 31, `JointCForceX`: 32, `JointCForceY`: 33, `JointCForceZ`: 34, `JointCTorqueX`: 35, `JointCTorqueY`: 36, `JointCTorqueZ`: 37, `JointPDeltaX`: 38, `JointPDeltaY`: 39, `JointPDeltaZ`: 40, `JointPAngDeltaX`: 41, `JointPAngDeltaY`: 42, `JointPAngDeltaZ`: 43, `JointCDeltaX`: 44, `JointCDeltaY`: 45, `JointCDeltaZ`: 46, `JointCAngDeltaX`: 47, `JointCAngDeltaY`: 48, `JointCAngDeltaZ`: 49} -var _JointVarsDescMap = map[JointVars]string{0: `JointType (as an int32 from bits).`, 1: `JointEnabled allows joints to be dynamically enabled.`, 2: `JointParent is the dynamic body index for parent body. Can be -1 for a fixed parent for absolute anchor.`, 3: `JointChild is the dynamic body index for child body.`, 4: `relative position of joint, in parent frame.`, 5: ``, 6: ``, 7: `relative orientation of joint, in parent frame.`, 8: ``, 9: ``, 10: ``, 11: `relative position of joint, in child frame.`, 12: ``, 13: ``, 14: `relative orientation of joint, in child frame.`, 15: ``, 16: ``, 17: ``, 18: `JointDoFN is the number of degrees-of-freedom for the joint.`, 19: `indexes in JointDoFs for each DoF`, 20: ``, 21: ``, 22: `angular starts here for Free, D6`, 23: ``, 24: ``, 25: `Computed parent joint force value.`, 26: ``, 27: ``, 28: `Computed parent joint torque value.`, 29: ``, 30: ``, 31: `Computed child joint force value.`, 32: ``, 33: ``, 34: `Computed child joint torque value.`, 35: ``, 36: ``, 37: `Computed parent joint delta value.`, 38: ``, 39: ``, 40: `Computed parent joint angdelta value.`, 41: ``, 42: ``, 43: `Computed child joint delta value.`, 44: ``, 45: ``, 46: `Computed child joint angdelta value.`, 47: ``, 48: ``} +var _JointVarsDescMap = map[JointVars]string{0: `JointType (as an int32 from bits).`, 1: `JointEnabled allows joints to be dynamically enabled.`, 2: `JointParent is the dynamic body index for parent body. Can be -1 for a fixed parent for absolute anchor.`, 3: `JointChild is the dynamic body index for child body.`, 4: `relative position of joint, in parent frame.`, 5: ``, 6: ``, 7: `relative orientation of joint, in parent frame.`, 8: ``, 9: ``, 10: ``, 11: `relative position of joint, in child frame.`, 12: ``, 13: ``, 14: `relative orientation of joint, in child frame.`, 15: ``, 16: ``, 17: ``, 18: `JointLinearDoFN is the number of linear degrees-of-freedom for the joint.`, 19: `JointAngularDoFN is the number of angular degrees-of-freedom for the joint.`, 20: `indexes in JointDoFs for each DoF`, 21: ``, 22: ``, 23: `angular starts here for Free, Distance, D6`, 24: ``, 25: ``, 26: `Computed parent joint force value.`, 27: ``, 28: ``, 29: `Computed parent joint torque value.`, 30: ``, 31: ``, 32: `Computed child joint force value.`, 33: ``, 34: ``, 35: `Computed child joint torque value.`, 36: ``, 37: ``, 38: `Computed parent joint delta value.`, 39: ``, 40: ``, 41: `Computed parent joint angdelta value.`, 42: ``, 43: ``, 44: `Computed child joint delta value.`, 45: ``, 46: ``, 47: `Computed child joint angdelta value.`, 48: ``, 49: ``} -var _JointVarsMap = map[JointVars]string{0: `JointType`, 1: `JointEnabled`, 2: `JointParent`, 3: `JointChild`, 4: `JointPPosX`, 5: `JointPPosY`, 6: `JointPPosZ`, 7: `JointPRotX`, 8: `JointPRotY`, 9: `JointPRotZ`, 10: `JointPRotW`, 11: `JointCPosX`, 12: `JointCPosY`, 13: `JointCPosZ`, 14: `JointCRotX`, 15: `JointCRotY`, 16: `JointCRotZ`, 17: `JointCRotW`, 18: `JointDoFN`, 19: `JointDoF1`, 20: `JointDoF2`, 21: `JointDoF3`, 22: `JointDoF4`, 23: `JointDoF5`, 24: `JointDoF6`, 25: `JointPForceX`, 26: `JointPForceY`, 27: `JointPForceZ`, 28: `JointPTorqueX`, 29: `JointPTorqueY`, 30: `JointPTorqueZ`, 31: `JointCForceX`, 32: `JointCForceY`, 33: `JointCForceZ`, 34: `JointCTorqueX`, 35: `JointCTorqueY`, 36: `JointCTorqueZ`, 37: `JointPDeltaX`, 38: `JointPDeltaY`, 39: `JointPDeltaZ`, 40: `JointPAngDeltaX`, 41: `JointPAngDeltaY`, 42: `JointPAngDeltaZ`, 43: `JointCDeltaX`, 44: `JointCDeltaY`, 45: `JointCDeltaZ`, 46: `JointCAngDeltaX`, 47: `JointCAngDeltaY`, 48: `JointCAngDeltaZ`} +var _JointVarsMap = map[JointVars]string{0: `JointType`, 1: `JointEnabled`, 2: `JointParent`, 3: `JointChild`, 4: `JointPPosX`, 5: `JointPPosY`, 6: `JointPPosZ`, 7: `JointPQuatX`, 8: `JointPQuatY`, 9: `JointPQuatZ`, 10: `JointPQuatW`, 11: `JointCPosX`, 12: `JointCPosY`, 13: `JointCPosZ`, 14: `JointCQuatX`, 15: `JointCQuatY`, 16: `JointCQuatZ`, 17: `JointCQuatW`, 18: `JointLinearDoFN`, 19: `JointAngularDoFN`, 20: `JointDoF1`, 21: `JointDoF2`, 22: `JointDoF3`, 23: `JointDoF4`, 24: `JointDoF5`, 25: `JointDoF6`, 26: `JointPForceX`, 27: `JointPForceY`, 28: `JointPForceZ`, 29: `JointPTorqueX`, 30: `JointPTorqueY`, 31: `JointPTorqueZ`, 32: `JointCForceX`, 33: `JointCForceY`, 34: `JointCForceZ`, 35: `JointCTorqueX`, 36: `JointCTorqueY`, 37: `JointCTorqueZ`, 38: `JointPDeltaX`, 39: `JointPDeltaY`, 40: `JointPDeltaZ`, 41: `JointPAngDeltaX`, 42: `JointPAngDeltaY`, 43: `JointPAngDeltaZ`, 44: `JointCDeltaX`, 45: `JointCDeltaY`, 46: `JointCDeltaZ`, 47: `JointCAngDeltaX`, 48: `JointCAngDeltaY`, 49: `JointCAngDeltaZ`} // String returns the string representation of this JointVars value. func (i JointVars) String() string { return enums.String(i, _JointVarsMap) } diff --git a/physics/examples/test1/test1.go b/physics/examples/test1/test1.go index 500f2417..e6e1175f 100644 --- a/physics/examples/test1/test1.go +++ b/physics/examples/test1/test1.go @@ -57,9 +57,10 @@ func main() { depth := height * .15 b1 := wr.NewDynamic(wl, "body", physics.Box, "purple", 1.0, math32.Vec3(width, height, depth), math32.Vec3(0, height/2, 0), rot) - bj := wl.NewJoint1D(physics.Revolute, -1, b1.DynamicIndex, math32.Vec3(-width, 0, 0), math32.Vec3(0, 0, 0), math32.Vec3(1, 0, 0)) - _ = bj - physics.SetJointControlForce(bj, 0, 5) + + bj := wl.NewJointRevolute(-1, b1.DynamicIndex, math32.Vec3(0, 0, 0), math32.Vec3(0, -height/2, 0), math32.Vec3(0, 1, 0)) + physics.SetJointControlForce(bj, 0, 2) + // physics.SetJointDoF(bj, 0, physics.JointDamp, 0.01) wl.Config() params := physics.GetParams(0) diff --git a/physics/joint.go b/physics/joint.go index 2e68e901..eb5d8709 100644 --- a/physics/joint.go +++ b/physics/joint.go @@ -68,10 +68,10 @@ const ( JointPPosZ // relative orientation of joint, in parent frame. - JointPRotX - JointPRotY - JointPRotZ - JointPRotW + JointPQuatX + JointPQuatY + JointPQuatZ + JointPQuatW // relative position of joint, in child frame. JointCPosX @@ -79,19 +79,21 @@ const ( JointCPosZ // relative orientation of joint, in child frame. - JointCRotX - JointCRotY - JointCRotZ - JointCRotW + JointCQuatX + JointCQuatY + JointCQuatZ + JointCQuatW - // JointDoFN is the number of degrees-of-freedom for the joint. - JointDoFN + // JointLinearDoFN is the number of linear degrees-of-freedom for the joint. + JointLinearDoFN + // JointAngularDoFN is the number of angular degrees-of-freedom for the joint. + JointAngularDoFN // indexes in JointDoFs for each DoF JointDoF1 JointDoF2 JointDoF3 - // angular starts here for Free, D6 + // angular starts here for Free, Distance, D6 JointDoF4 JointDoF5 JointDoF6 @@ -147,6 +149,19 @@ func SetJointType(idx int32, typ JointTypes) { Joints.Set(math.Float32frombits(uint32(typ)), int(idx), int(JointType)) } +func GetJointEnabled(idx int32) bool { + je := math.Float32bits(Joints.Value(int(idx), int(JointEnabled))) + return je != 0 +} + +func SetJointEnabled(idx int32, enabled bool) { + je := uint32(0) + if enabled { + je = 1 + } + Joints.Set(math.Float32frombits(je), int(idx), int(JointEnabled)) +} + func SetJointParent(idx, bodyIdx int32) { Joints.Set(math.Float32frombits(uint32(bodyIdx)), int(idx), int(JointParent)) } @@ -163,12 +178,20 @@ func JointChildIndex(idx int32) int32 { return int32(math.Float32bits(Joints.Value(int(idx), int(JointChild)))) } -func SetJointDoFN(idx, dofN int32) { - Joints.Set(math.Float32frombits(uint32(dofN)), int(idx), int(JointDoFN)) +func SetJointLinearDoFN(idx, dofN int32) { + Joints.Set(math.Float32frombits(uint32(dofN)), int(idx), int(JointLinearDoFN)) +} + +func GetJointLinearDoFN(idx int32) int32 { + return int32(math.Float32bits(Joints.Value(int(idx), int(JointLinearDoFN)))) +} + +func SetJointAngularDoFN(idx, dofN int32) { + Joints.Set(math.Float32frombits(uint32(dofN)), int(idx), int(JointAngularDoFN)) } -func GetJointDoFN(idx int32) int32 { - return int32(math.Float32bits(Joints.Value(int(idx), int(JointDoFN)))) +func GetJointAngularDoFN(idx int32) int32 { + return int32(math.Float32bits(Joints.Value(int(idx), int(JointAngularDoFN)))) } func SetJointDoFIndex(idx, dof, dofIdx int32) { @@ -189,15 +212,15 @@ func SetJointPPos(idx int32, pos math32.Vector3) { Joints.Set(pos.Z, int(idx), int(JointPPosZ)) } -func JointPRot(idx int32) math32.Quat { - return math32.NewQuat(Joints.Value(int(idx), int(JointPRotX)), Joints.Value(int(idx), int(JointPRotY)), Joints.Value(int(idx), int(JointPRotZ)), Joints.Value(int(idx), int(JointPRotW))) +func JointPQuat(idx int32) math32.Quat { + return math32.NewQuat(Joints.Value(int(idx), int(JointPQuatX)), Joints.Value(int(idx), int(JointPQuatY)), Joints.Value(int(idx), int(JointPQuatZ)), Joints.Value(int(idx), int(JointPQuatW))) } -func SetJointPRot(idx int32, rot math32.Quat) { - Joints.Set(rot.X, int(idx), int(JointPRotX)) - Joints.Set(rot.Y, int(idx), int(JointPRotY)) - Joints.Set(rot.Z, int(idx), int(JointPRotZ)) - Joints.Set(rot.W, int(idx), int(JointPRotW)) +func SetJointPQuat(idx int32, rot math32.Quat) { + Joints.Set(rot.X, int(idx), int(JointPQuatX)) + Joints.Set(rot.Y, int(idx), int(JointPQuatY)) + Joints.Set(rot.Z, int(idx), int(JointPQuatZ)) + Joints.Set(rot.W, int(idx), int(JointPQuatW)) } func JointCPos(idx int32) math32.Vector3 { @@ -210,15 +233,15 @@ func SetJointCPos(idx int32, pos math32.Vector3) { Joints.Set(pos.Z, int(idx), int(JointCPosZ)) } -func JointCRot(idx int32) math32.Quat { - return math32.NewQuat(Joints.Value(int(idx), int(JointCRotX)), Joints.Value(int(idx), int(JointCRotY)), Joints.Value(int(idx), int(JointCRotZ)), Joints.Value(int(idx), int(JointCRotW))) +func JointCQuat(idx int32) math32.Quat { + return math32.NewQuat(Joints.Value(int(idx), int(JointCQuatX)), Joints.Value(int(idx), int(JointCQuatY)), Joints.Value(int(idx), int(JointCQuatZ)), Joints.Value(int(idx), int(JointCQuatW))) } -func SetJointCRot(idx int32, rot math32.Quat) { - Joints.Set(rot.X, int(idx), int(JointCRotX)) - Joints.Set(rot.Y, int(idx), int(JointCRotY)) - Joints.Set(rot.Z, int(idx), int(JointCRotZ)) - Joints.Set(rot.W, int(idx), int(JointCRotW)) +func SetJointCQuat(idx int32, rot math32.Quat) { + Joints.Set(rot.X, int(idx), int(JointCQuatX)) + Joints.Set(rot.Y, int(idx), int(JointCQuatY)) + Joints.Set(rot.Z, int(idx), int(JointCQuatZ)) + Joints.Set(rot.W, int(idx), int(JointCQuatW)) } func JointPForce(idx int32) math32.Vector3 { @@ -352,8 +375,8 @@ func SetJointDoF(idx, dof int32, vr JointDoFVars, value float32) { func (wl *World) JointDefaults(idx int32) { rot := math32.NewQuat(0, 0, 0, 1) - SetJointPRot(idx, rot) - SetJointCRot(idx, rot) + SetJointPQuat(idx, rot) + SetJointCQuat(idx, rot) } func (wl *World) JointDoFDefaults(didx int32) { @@ -362,3 +385,113 @@ func (wl *World) JointDoFDefaults(didx int32) { JointDoFs.Set(1.0e4, int(didx), int(JointStiff)) JointDoFs.Set(1.0e1, int(didx), int(JointDamp)) } + +// NewJointPrismatic adds a new Prismatic (slider) joint +// between parent and child dynamic object indexes. +// Use -1 for parent to add a world-anchored joint. +// ppos, cpos are the relative positions from the parent, child. +// Sets relative rotation matricies to identity by default. +// axis is the axis of articulation for the joint. +// Use [SetJointDoF] to set the remaining DoF parameters. +func (wl *World) NewJointPrismatic(parent, child int32, ppos, cpos, axis math32.Vector3) int32 { + idx := wl.newJoint(Prismatic, parent, child, ppos, cpos) + SetJointLinearDoFN(idx, 1) + didx := wl.newJointDoF(0, axis) + SetJointDoFIndex(idx, 0, didx) + return idx +} + +// NewJointRevolute adds a new Revolute (hinge, axel) joint +// between parent and child dynamic object indexes. +// Use -1 for parent to add a world-anchored joint. +// ppos, cpos are the relative positions from the parent, child. +// Sets relative rotation matricies to identity by default. +// axis is the axis of articulation for the joint. +// Use [SetJointDoF] to set the remaining DoF parameters. +func (wl *World) NewJointRevolute(parent, child int32, ppos, cpos, axis math32.Vector3) int32 { + idx := wl.newJoint(Revolute, parent, child, ppos, cpos) + SetJointAngularDoFN(idx, 1) + didx := wl.newJointDoF(0, axis) + SetJointDoFIndex(idx, 0, didx) + return idx +} + +// NewJointBall adds a new Ball joint (3 angular DoF) +// between parent and child dynamic object indexes. +// Use -1 for parent to add a world-anchored joint. +// ppos, cpos are the relative positions from the parent, child. +// Sets relative rotation matricies to identity by default. +// Use [SetJointDoF] to set the remaining DoF parameters. +func (wl *World) NewJointBall(parent, child int32, ppos, cpos math32.Vector3) int32 { + idx := wl.newJoint(Ball, parent, child, ppos, cpos) + SetJointAngularDoFN(idx, 3) + for d := range math32.W { + axis := math32.Vector3{} + axis.SetDim(d, 1) + didx := wl.newJointDoF(int32(d), axis) + SetJointDoFIndex(idx, int32(d), didx) + } + return idx +} + +// NewJointDistance adds a new Distance joint (6 DoF) +// between parent and child dynamic object indexes, +// with distance constrained only on the first linear X axis. +// Use -1 for parent to add a world-anchored joint. +// ppos, cpos are the relative positions from the parent, child. +// Sets relative rotation matricies to identity by default. +// Use [SetJointDoF] to set the remaining DoF parameters. +func (wl *World) NewJointDistance(parent, child int32, ppos, cpos math32.Vector3, minDist, maxDist float32) int32 { + idx := wl.newJoint(Distance, parent, child, ppos, cpos) + SetJointLinearDoFN(idx, 3) + SetJointAngularDoFN(idx, 3) + for d := range math32.W { + axis := math32.Vector3{} + axis.SetDim(d, 1) + didx := wl.newJointDoF(int32(d), axis) + SetJointDoFIndex(idx, int32(d), didx) + } + for d := range math32.W { + axis := math32.Vector3{} + axis.SetDim(d, 1) + didx := wl.newJointDoF(int32(d), axis) + SetJointDoFIndex(idx, int32(d), didx) + } + // only on the X linear axis + SetJointDoF(idx, 0, JointLimitLower, minDist) + SetJointDoF(idx, 0, JointLimitUpper, maxDist) + return idx +} + +// newJoint adds a new joint between parent and child +// dynamic object indexes. +// Use -1 for parent to add a world-anchored joint. +// ppos, cpos are the relative positions from the parent, child. +// Sets relative rotation matricies to identity by default. +func (wl *World) newJoint(joint JointTypes, parent, child int32, ppos, cpos math32.Vector3) int32 { + sizes := wl.Joints.ShapeSizes() + idx := int32(sizes[0]) + wl.Params[0].JointsN = idx + 1 + wl.Joints.SetShapeSizes(int(idx+1), int(JointVarsN)) + wl.JointDefaults(idx) + SetJointType(idx, joint) + SetJointEnabled(idx, true) + SetJointParent(idx, parent) + SetJointChild(idx, child) + SetJointPPos(idx, ppos) + SetJointCPos(idx, cpos) + return idx +} + +// newJointDoF adds new JointDoFs and JointControls entries +// initialized to detfaults. Returns index. +func (wl *World) newJointDoF(dof int32, axis math32.Vector3) int32 { + sizes := wl.JointDoFs.ShapeSizes() + idx := int32(sizes[0]) + wl.JointDoFs.SetShapeSizes(int(idx+1), int(JointDoFVarsN)) + wl.JointControls.SetShapeSizes(int(idx+1), int(JointControlVarsN)) + wl.JointDoFDefaults(idx) + SetJointAxis(idx, 0, axis) + wl.Params[0].JointDoFsN = idx + 1 + return idx +} diff --git a/physics/joint.goal b/physics/joint.goal index 55463d2d..9e62ce3b 100644 --- a/physics/joint.goal +++ b/physics/joint.goal @@ -66,10 +66,10 @@ const ( JointPPosZ // relative orientation of joint, in parent frame. - JointPRotX - JointPRotY - JointPRotZ - JointPRotW + JointPQuatX + JointPQuatY + JointPQuatZ + JointPQuatW // relative position of joint, in child frame. JointCPosX @@ -77,19 +77,21 @@ const ( JointCPosZ // relative orientation of joint, in child frame. - JointCRotX - JointCRotY - JointCRotZ - JointCRotW + JointCQuatX + JointCQuatY + JointCQuatZ + JointCQuatW - // JointDoFN is the number of degrees-of-freedom for the joint. - JointDoFN + // JointLinearDoFN is the number of linear degrees-of-freedom for the joint. + JointLinearDoFN + // JointAngularDoFN is the number of angular degrees-of-freedom for the joint. + JointAngularDoFN // indexes in JointDoFs for each DoF JointDoF1 JointDoF2 JointDoF3 - // angular starts here for Free, D6 + // angular starts here for Free, Distance, D6 JointDoF4 JointDoF5 JointDoF6 @@ -145,6 +147,19 @@ func SetJointType(idx int32, typ JointTypes) { Joints[idx, JointType] = math.Float32frombits(uint32(typ)) } +func GetJointEnabled(idx int32) bool { + je := math.Float32bits(Joints[idx, JointEnabled]) + return je != 0 +} + +func SetJointEnabled(idx int32, enabled bool) { + je := uint32(0) + if enabled { + je = 1 + } + Joints[idx, JointEnabled] = math.Float32frombits(je) +} + func SetJointParent(idx, bodyIdx int32) { Joints[idx, JointParent] = math.Float32frombits(uint32(bodyIdx)) } @@ -161,12 +176,20 @@ func JointChildIndex(idx int32) int32 { return int32(math.Float32bits(Joints[idx, JointChild])) } -func SetJointDoFN(idx, dofN int32) { - Joints[idx, JointDoFN] = math.Float32frombits(uint32(dofN)) +func SetJointLinearDoFN(idx, dofN int32) { + Joints[idx, JointLinearDoFN] = math.Float32frombits(uint32(dofN)) +} + +func GetJointLinearDoFN(idx int32) int32 { + return int32(math.Float32bits(Joints[idx, JointLinearDoFN])) +} + +func SetJointAngularDoFN(idx, dofN int32) { + Joints[idx, JointAngularDoFN] = math.Float32frombits(uint32(dofN)) } -func GetJointDoFN(idx int32) int32 { - return int32(math.Float32bits(Joints[idx, JointDoFN])) +func GetJointAngularDoFN(idx int32) int32 { + return int32(math.Float32bits(Joints[idx, JointAngularDoFN])) } func SetJointDoFIndex(idx, dof, dofIdx int32) { @@ -187,15 +210,15 @@ func SetJointPPos(idx int32, pos math32.Vector3) { Joints[idx, JointPPosZ] = pos.Z } -func JointPRot(idx int32) math32.Quat { - return math32.NewQuat(Joints[idx, JointPRotX], Joints[idx, JointPRotY], Joints[idx, JointPRotZ], Joints[idx, JointPRotW]) +func JointPQuat(idx int32) math32.Quat { + return math32.NewQuat(Joints[idx, JointPQuatX], Joints[idx, JointPQuatY], Joints[idx, JointPQuatZ], Joints[idx, JointPQuatW]) } -func SetJointPRot(idx int32, rot math32.Quat) { - Joints[idx, JointPRotX] = rot.X - Joints[idx, JointPRotY] = rot.Y - Joints[idx, JointPRotZ] = rot.Z - Joints[idx, JointPRotW] = rot.W +func SetJointPQuat(idx int32, rot math32.Quat) { + Joints[idx, JointPQuatX] = rot.X + Joints[idx, JointPQuatY] = rot.Y + Joints[idx, JointPQuatZ] = rot.Z + Joints[idx, JointPQuatW] = rot.W } func JointCPos(idx int32) math32.Vector3 { @@ -208,15 +231,15 @@ func SetJointCPos(idx int32, pos math32.Vector3) { Joints[idx, JointCPosZ] = pos.Z } -func JointCRot(idx int32) math32.Quat { - return math32.NewQuat(Joints[idx, JointCRotX], Joints[idx, JointCRotY], Joints[idx, JointCRotZ], Joints[idx, JointCRotW]) +func JointCQuat(idx int32) math32.Quat { + return math32.NewQuat(Joints[idx, JointCQuatX], Joints[idx, JointCQuatY], Joints[idx, JointCQuatZ], Joints[idx, JointCQuatW]) } -func SetJointCRot(idx int32, rot math32.Quat) { - Joints[idx, JointCRotX] = rot.X - Joints[idx, JointCRotY] = rot.Y - Joints[idx, JointCRotZ] = rot.Z - Joints[idx, JointCRotW] = rot.W +func SetJointCQuat(idx int32, rot math32.Quat) { + Joints[idx, JointCQuatX] = rot.X + Joints[idx, JointCQuatY] = rot.Y + Joints[idx, JointCQuatZ] = rot.Z + Joints[idx, JointCQuatW] = rot.W } func JointPForce(idx int32) math32.Vector3 { @@ -352,8 +375,8 @@ func SetJointDoF(idx, dof int32, vr JointDoFVars, value float32) { func (wl *World) JointDefaults(idx int32) { rot := math32.NewQuat(0, 0, 0, 1) - SetJointPRot(idx, rot) - SetJointCRot(idx, rot) + SetJointPQuat(idx, rot) + SetJointCQuat(idx, rot) } func (wl *World) JointDoFDefaults(didx int32) { @@ -363,3 +386,113 @@ func (wl *World) JointDoFDefaults(didx int32) { JointDoFs[didx, JointDamp] = 1.0e1 } +// NewJointPrismatic adds a new Prismatic (slider) joint +// between parent and child dynamic object indexes. +// Use -1 for parent to add a world-anchored joint. +// ppos, cpos are the relative positions from the parent, child. +// Sets relative rotation matricies to identity by default. +// axis is the axis of articulation for the joint. +// Use [SetJointDoF] to set the remaining DoF parameters. +func (wl *World) NewJointPrismatic(parent, child int32, ppos, cpos, axis math32.Vector3) int32 { + idx := wl.newJoint(Prismatic, parent, child, ppos, cpos) + SetJointLinearDoFN(idx, 1) + didx := wl.newJointDoF(0, axis) + SetJointDoFIndex(idx, 0, didx) + return idx +} + +// NewJointRevolute adds a new Revolute (hinge, axel) joint +// between parent and child dynamic object indexes. +// Use -1 for parent to add a world-anchored joint. +// ppos, cpos are the relative positions from the parent, child. +// Sets relative rotation matricies to identity by default. +// axis is the axis of articulation for the joint. +// Use [SetJointDoF] to set the remaining DoF parameters. +func (wl *World) NewJointRevolute(parent, child int32, ppos, cpos, axis math32.Vector3) int32 { + idx := wl.newJoint(Revolute, parent, child, ppos, cpos) + SetJointAngularDoFN(idx, 1) + didx := wl.newJointDoF(0, axis) + SetJointDoFIndex(idx, 0, didx) + return idx +} + +// NewJointBall adds a new Ball joint (3 angular DoF) +// between parent and child dynamic object indexes. +// Use -1 for parent to add a world-anchored joint. +// ppos, cpos are the relative positions from the parent, child. +// Sets relative rotation matricies to identity by default. +// Use [SetJointDoF] to set the remaining DoF parameters. +func (wl *World) NewJointBall(parent, child int32, ppos, cpos math32.Vector3) int32 { + idx := wl.newJoint(Ball, parent, child, ppos, cpos) + SetJointAngularDoFN(idx, 3) + for d := range math32.W { + axis := math32.Vector3{} + axis.SetDim(d, 1) + didx := wl.newJointDoF(int32(d), axis) + SetJointDoFIndex(idx, int32(d), didx) + } + return idx +} + +// NewJointDistance adds a new Distance joint (6 DoF) +// between parent and child dynamic object indexes, +// with distance constrained only on the first linear X axis. +// Use -1 for parent to add a world-anchored joint. +// ppos, cpos are the relative positions from the parent, child. +// Sets relative rotation matricies to identity by default. +// Use [SetJointDoF] to set the remaining DoF parameters. +func (wl *World) NewJointDistance(parent, child int32, ppos, cpos math32.Vector3, minDist, maxDist float32) int32 { + idx := wl.newJoint(Distance, parent, child, ppos, cpos) + SetJointLinearDoFN(idx, 3) + SetJointAngularDoFN(idx, 3) + for d := range math32.W { + axis := math32.Vector3{} + axis.SetDim(d, 1) + didx := wl.newJointDoF(int32(d), axis) + SetJointDoFIndex(idx, int32(d), didx) + } + for d := range math32.W { + axis := math32.Vector3{} + axis.SetDim(d, 1) + didx := wl.newJointDoF(int32(d), axis) + SetJointDoFIndex(idx, int32(d), didx) + } + // only on the X linear axis + SetJointDoF(idx, 0, JointLimitLower, minDist) + SetJointDoF(idx, 0, JointLimitUpper, maxDist) + return idx +} + +// newJoint adds a new joint between parent and child +// dynamic object indexes. +// Use -1 for parent to add a world-anchored joint. +// ppos, cpos are the relative positions from the parent, child. +// Sets relative rotation matricies to identity by default. +func (wl *World) newJoint(joint JointTypes, parent, child int32, ppos, cpos math32.Vector3) int32 { + sizes := wl.Joints.ShapeSizes() + idx := int32(sizes[0]) + wl.Params[0].JointsN = idx + 1 + wl.Joints.SetShapeSizes(int(idx+1), int(JointVarsN)) + wl.JointDefaults(idx) + SetJointType(idx, joint) + SetJointEnabled(idx, true) + SetJointParent(idx, parent) + SetJointChild(idx, child) + SetJointPPos(idx, ppos) + SetJointCPos(idx, cpos) + return idx +} + +// newJointDoF adds new JointDoFs and JointControls entries +// initialized to detfaults. Returns index. +func (wl *World) newJointDoF(dof int32, axis math32.Vector3) int32 { + sizes := wl.JointDoFs.ShapeSizes() + idx := int32(sizes[0]) + wl.JointDoFs.SetShapeSizes(int(idx+1), int(JointDoFVarsN)) + wl.JointControls.SetShapeSizes(int(idx+1), int(JointControlVarsN)) + wl.JointDoFDefaults(idx) + SetJointAxis(idx, 0, axis) + wl.Params[0].JointDoFsN = idx + 1 + return idx +} + diff --git a/physics/shaders/DeltasFromJoints.wgsl b/physics/shaders/DeltasFromJoints.wgsl index e725f94d..dc0fb5d8 100644 --- a/physics/shaders/DeltasFromJoints.wgsl +++ b/physics/shaders/DeltasFromJoints.wgsl @@ -49,10 +49,10 @@ const BodyFriction: BodyVars = 8; const BodyPosX: BodyVars = 9; const BodyPosY: BodyVars = 10; const BodyPosZ: BodyVars = 11; -const BodyRotX: BodyVars = 12; -const BodyRotY: BodyVars = 13; -const BodyRotZ: BodyVars = 14; -const BodyRotW: BodyVars = 15; +const BodyQuatX: BodyVars = 12; +const BodyQuatY: BodyVars = 13; +const BodyQuatZ: BodyVars = 14; +const BodyQuatW: BodyVars = 15; const BodyComX: BodyVars = 16; const BodyComY: BodyVars = 17; const BodyComZ: BodyVars = 18; @@ -99,10 +99,10 @@ const DynIndex: DynamicVars = 0; const DynPosX: DynamicVars = 1; const DynPosY: DynamicVars = 2; const DynPosZ: DynamicVars = 3; -const DynRotX: DynamicVars = 4; -const DynRotY: DynamicVars = 5; -const DynRotZ: DynamicVars = 6; -const DynRotW: DynamicVars = 7; +const DynQuatX: DynamicVars = 4; +const DynQuatY: DynamicVars = 5; +const DynQuatZ: DynamicVars = 6; +const DynQuatW: DynamicVars = 7; const DynVelX: DynamicVars = 8; const DynVelY: DynamicVars = 9; const DynVelZ: DynamicVars = 10; @@ -145,7 +145,7 @@ const JointControlVarsN: JointControlVars = 3; const DynamicVarsN: DynamicVars = 32; const GPUVarsN: GPUVars = 8; const JointTypesN: JointTypes = 7; -const JointVarsN: JointVars = 49; +const JointVarsN: JointVars = 50; const JointDoFVarsN: JointDoFVars = 7; const ShapesN: Shapes = 4; @@ -167,48 +167,49 @@ const JointChild: JointVars = 3; const JointPPosX: JointVars = 4; const JointPPosY: JointVars = 5; const JointPPosZ: JointVars = 6; -const JointPRotX: JointVars = 7; -const JointPRotY: JointVars = 8; -const JointPRotZ: JointVars = 9; -const JointPRotW: JointVars = 10; +const JointPQuatX: JointVars = 7; +const JointPQuatY: JointVars = 8; +const JointPQuatZ: JointVars = 9; +const JointPQuatW: JointVars = 10; const JointCPosX: JointVars = 11; const JointCPosY: JointVars = 12; const JointCPosZ: JointVars = 13; -const JointCRotX: JointVars = 14; -const JointCRotY: JointVars = 15; -const JointCRotZ: JointVars = 16; -const JointCRotW: JointVars = 17; -const JointDoFN: JointVars = 18; -const JointDoF1: JointVars = 19; -const JointDoF2: JointVars = 20; -const JointDoF3: JointVars = 21; -const JointDoF4: JointVars = 22; -const JointDoF5: JointVars = 23; -const JointDoF6: JointVars = 24; -const JointPForceX: JointVars = 25; -const JointPForceY: JointVars = 26; -const JointPForceZ: JointVars = 27; -const JointPTorqueX: JointVars = 28; -const JointPTorqueY: JointVars = 29; -const JointPTorqueZ: JointVars = 30; -const JointCForceX: JointVars = 31; -const JointCForceY: JointVars = 32; -const JointCForceZ: JointVars = 33; -const JointCTorqueX: JointVars = 34; -const JointCTorqueY: JointVars = 35; -const JointCTorqueZ: JointVars = 36; -const JointPDeltaX: JointVars = 37; -const JointPDeltaY: JointVars = 38; -const JointPDeltaZ: JointVars = 39; -const JointPAngDeltaX: JointVars = 40; -const JointPAngDeltaY: JointVars = 41; -const JointPAngDeltaZ: JointVars = 42; -const JointCDeltaX: JointVars = 43; -const JointCDeltaY: JointVars = 44; -const JointCDeltaZ: JointVars = 45; -const JointCAngDeltaX: JointVars = 46; -const JointCAngDeltaY: JointVars = 47; -const JointCAngDeltaZ: JointVars = 48; +const JointCQuatX: JointVars = 14; +const JointCQuatY: JointVars = 15; +const JointCQuatZ: JointVars = 16; +const JointCQuatW: JointVars = 17; +const JointLinearDoFN: JointVars = 18; +const JointAngularDoFN: JointVars = 19; +const JointDoF1: JointVars = 20; +const JointDoF2: JointVars = 21; +const JointDoF3: JointVars = 22; +const JointDoF4: JointVars = 23; +const JointDoF5: JointVars = 24; +const JointDoF6: JointVars = 25; +const JointPForceX: JointVars = 26; +const JointPForceY: JointVars = 27; +const JointPForceZ: JointVars = 28; +const JointPTorqueX: JointVars = 29; +const JointPTorqueY: JointVars = 30; +const JointPTorqueZ: JointVars = 31; +const JointCForceX: JointVars = 32; +const JointCForceY: JointVars = 33; +const JointCForceZ: JointVars = 34; +const JointCTorqueX: JointVars = 35; +const JointCTorqueY: JointVars = 36; +const JointCTorqueZ: JointVars = 37; +const JointPDeltaX: JointVars = 38; +const JointPDeltaY: JointVars = 39; +const JointPDeltaZ: JointVars = 40; +const JointPAngDeltaX: JointVars = 41; +const JointPAngDeltaY: JointVars = 42; +const JointPAngDeltaZ: JointVars = 43; +const JointCDeltaX: JointVars = 44; +const JointCDeltaY: JointVars = 45; +const JointCDeltaZ: JointVars = 46; +const JointCAngDeltaX: JointVars = 47; +const JointCAngDeltaY: JointVars = 48; +const JointCAngDeltaZ: JointVars = 49; fn JointPDelta(idx: i32) -> vec3 { return vec3(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPDeltaX))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPDeltaY))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPDeltaZ))]); } diff --git a/physics/shaders/DynamicsCurToNext.wgsl b/physics/shaders/DynamicsCurToNext.wgsl index c86b1309..8f209879 100644 --- a/physics/shaders/DynamicsCurToNext.wgsl +++ b/physics/shaders/DynamicsCurToNext.wgsl @@ -41,10 +41,10 @@ const BodyFriction: BodyVars = 8; const BodyPosX: BodyVars = 9; const BodyPosY: BodyVars = 10; const BodyPosZ: BodyVars = 11; -const BodyRotX: BodyVars = 12; -const BodyRotY: BodyVars = 13; -const BodyRotZ: BodyVars = 14; -const BodyRotW: BodyVars = 15; +const BodyQuatX: BodyVars = 12; +const BodyQuatY: BodyVars = 13; +const BodyQuatZ: BodyVars = 14; +const BodyQuatW: BodyVars = 15; const BodyComX: BodyVars = 16; const BodyComY: BodyVars = 17; const BodyComZ: BodyVars = 18; @@ -91,10 +91,10 @@ const DynIndex: DynamicVars = 0; const DynPosX: DynamicVars = 1; const DynPosY: DynamicVars = 2; const DynPosZ: DynamicVars = 3; -const DynRotX: DynamicVars = 4; -const DynRotY: DynamicVars = 5; -const DynRotZ: DynamicVars = 6; -const DynRotW: DynamicVars = 7; +const DynQuatX: DynamicVars = 4; +const DynQuatY: DynamicVars = 5; +const DynQuatZ: DynamicVars = 6; +const DynQuatW: DynamicVars = 7; const DynVelX: DynamicVars = 8; const DynVelY: DynamicVars = 9; const DynVelZ: DynamicVars = 10; @@ -127,7 +127,7 @@ const JointControlVarsN: JointControlVars = 3; const DynamicVarsN: DynamicVars = 32; const GPUVarsN: GPUVars = 8; const JointTypesN: JointTypes = 7; -const JointVarsN: JointVars = 49; +const JointVarsN: JointVars = 50; const JointDoFVarsN: JointDoFVars = 7; const ShapesN: Shapes = 4; @@ -149,48 +149,49 @@ const JointChild: JointVars = 3; const JointPPosX: JointVars = 4; const JointPPosY: JointVars = 5; const JointPPosZ: JointVars = 6; -const JointPRotX: JointVars = 7; -const JointPRotY: JointVars = 8; -const JointPRotZ: JointVars = 9; -const JointPRotW: JointVars = 10; +const JointPQuatX: JointVars = 7; +const JointPQuatY: JointVars = 8; +const JointPQuatZ: JointVars = 9; +const JointPQuatW: JointVars = 10; const JointCPosX: JointVars = 11; const JointCPosY: JointVars = 12; const JointCPosZ: JointVars = 13; -const JointCRotX: JointVars = 14; -const JointCRotY: JointVars = 15; -const JointCRotZ: JointVars = 16; -const JointCRotW: JointVars = 17; -const JointDoFN: JointVars = 18; -const JointDoF1: JointVars = 19; -const JointDoF2: JointVars = 20; -const JointDoF3: JointVars = 21; -const JointDoF4: JointVars = 22; -const JointDoF5: JointVars = 23; -const JointDoF6: JointVars = 24; -const JointPForceX: JointVars = 25; -const JointPForceY: JointVars = 26; -const JointPForceZ: JointVars = 27; -const JointPTorqueX: JointVars = 28; -const JointPTorqueY: JointVars = 29; -const JointPTorqueZ: JointVars = 30; -const JointCForceX: JointVars = 31; -const JointCForceY: JointVars = 32; -const JointCForceZ: JointVars = 33; -const JointCTorqueX: JointVars = 34; -const JointCTorqueY: JointVars = 35; -const JointCTorqueZ: JointVars = 36; -const JointPDeltaX: JointVars = 37; -const JointPDeltaY: JointVars = 38; -const JointPDeltaZ: JointVars = 39; -const JointPAngDeltaX: JointVars = 40; -const JointPAngDeltaY: JointVars = 41; -const JointPAngDeltaZ: JointVars = 42; -const JointCDeltaX: JointVars = 43; -const JointCDeltaY: JointVars = 44; -const JointCDeltaZ: JointVars = 45; -const JointCAngDeltaX: JointVars = 46; -const JointCAngDeltaY: JointVars = 47; -const JointCAngDeltaZ: JointVars = 48; +const JointCQuatX: JointVars = 14; +const JointCQuatY: JointVars = 15; +const JointCQuatZ: JointVars = 16; +const JointCQuatW: JointVars = 17; +const JointLinearDoFN: JointVars = 18; +const JointAngularDoFN: JointVars = 19; +const JointDoF1: JointVars = 20; +const JointDoF2: JointVars = 21; +const JointDoF3: JointVars = 22; +const JointDoF4: JointVars = 23; +const JointDoF5: JointVars = 24; +const JointDoF6: JointVars = 25; +const JointPForceX: JointVars = 26; +const JointPForceY: JointVars = 27; +const JointPForceZ: JointVars = 28; +const JointPTorqueX: JointVars = 29; +const JointPTorqueY: JointVars = 30; +const JointPTorqueZ: JointVars = 31; +const JointCForceX: JointVars = 32; +const JointCForceY: JointVars = 33; +const JointCForceZ: JointVars = 34; +const JointCTorqueX: JointVars = 35; +const JointCTorqueY: JointVars = 36; +const JointCTorqueZ: JointVars = 37; +const JointPDeltaX: JointVars = 38; +const JointPDeltaY: JointVars = 39; +const JointPDeltaZ: JointVars = 40; +const JointPAngDeltaX: JointVars = 41; +const JointPAngDeltaY: JointVars = 42; +const JointPAngDeltaZ: JointVars = 43; +const JointCDeltaX: JointVars = 44; +const JointCDeltaY: JointVars = 45; +const JointCDeltaZ: JointVars = 46; +const JointCAngDeltaX: JointVars = 47; +const JointCAngDeltaY: JointVars = 48; +const JointCAngDeltaZ: JointVars = 49; alias JointDoFVars = i32; //enums:enum const JointAxisX: JointDoFVars = 0; const JointAxisY: JointDoFVars = 1; diff --git a/physics/shaders/ForcesFromJoints.wgsl b/physics/shaders/ForcesFromJoints.wgsl index cbfb8e7a..815170d2 100644 --- a/physics/shaders/ForcesFromJoints.wgsl +++ b/physics/shaders/ForcesFromJoints.wgsl @@ -49,10 +49,10 @@ const BodyFriction: BodyVars = 8; const BodyPosX: BodyVars = 9; const BodyPosY: BodyVars = 10; const BodyPosZ: BodyVars = 11; -const BodyRotX: BodyVars = 12; -const BodyRotY: BodyVars = 13; -const BodyRotZ: BodyVars = 14; -const BodyRotW: BodyVars = 15; +const BodyQuatX: BodyVars = 12; +const BodyQuatY: BodyVars = 13; +const BodyQuatZ: BodyVars = 14; +const BodyQuatW: BodyVars = 15; const BodyComX: BodyVars = 16; const BodyComY: BodyVars = 17; const BodyComZ: BodyVars = 18; @@ -99,10 +99,10 @@ const DynIndex: DynamicVars = 0; const DynPosX: DynamicVars = 1; const DynPosY: DynamicVars = 2; const DynPosZ: DynamicVars = 3; -const DynRotX: DynamicVars = 4; -const DynRotY: DynamicVars = 5; -const DynRotZ: DynamicVars = 6; -const DynRotW: DynamicVars = 7; +const DynQuatX: DynamicVars = 4; +const DynQuatY: DynamicVars = 5; +const DynQuatZ: DynamicVars = 6; +const DynQuatW: DynamicVars = 7; const DynVelX: DynamicVars = 8; const DynVelY: DynamicVars = 9; const DynVelZ: DynamicVars = 10; @@ -145,7 +145,7 @@ const JointControlVarsN: JointControlVars = 3; const DynamicVarsN: DynamicVars = 32; const GPUVarsN: GPUVars = 8; const JointTypesN: JointTypes = 7; -const JointVarsN: JointVars = 49; +const JointVarsN: JointVars = 50; const JointDoFVarsN: JointDoFVars = 7; const ShapesN: Shapes = 4; @@ -167,48 +167,49 @@ const JointChild: JointVars = 3; const JointPPosX: JointVars = 4; const JointPPosY: JointVars = 5; const JointPPosZ: JointVars = 6; -const JointPRotX: JointVars = 7; -const JointPRotY: JointVars = 8; -const JointPRotZ: JointVars = 9; -const JointPRotW: JointVars = 10; +const JointPQuatX: JointVars = 7; +const JointPQuatY: JointVars = 8; +const JointPQuatZ: JointVars = 9; +const JointPQuatW: JointVars = 10; const JointCPosX: JointVars = 11; const JointCPosY: JointVars = 12; const JointCPosZ: JointVars = 13; -const JointCRotX: JointVars = 14; -const JointCRotY: JointVars = 15; -const JointCRotZ: JointVars = 16; -const JointCRotW: JointVars = 17; -const JointDoFN: JointVars = 18; -const JointDoF1: JointVars = 19; -const JointDoF2: JointVars = 20; -const JointDoF3: JointVars = 21; -const JointDoF4: JointVars = 22; -const JointDoF5: JointVars = 23; -const JointDoF6: JointVars = 24; -const JointPForceX: JointVars = 25; -const JointPForceY: JointVars = 26; -const JointPForceZ: JointVars = 27; -const JointPTorqueX: JointVars = 28; -const JointPTorqueY: JointVars = 29; -const JointPTorqueZ: JointVars = 30; -const JointCForceX: JointVars = 31; -const JointCForceY: JointVars = 32; -const JointCForceZ: JointVars = 33; -const JointCTorqueX: JointVars = 34; -const JointCTorqueY: JointVars = 35; -const JointCTorqueZ: JointVars = 36; -const JointPDeltaX: JointVars = 37; -const JointPDeltaY: JointVars = 38; -const JointPDeltaZ: JointVars = 39; -const JointPAngDeltaX: JointVars = 40; -const JointPAngDeltaY: JointVars = 41; -const JointPAngDeltaZ: JointVars = 42; -const JointCDeltaX: JointVars = 43; -const JointCDeltaY: JointVars = 44; -const JointCDeltaZ: JointVars = 45; -const JointCAngDeltaX: JointVars = 46; -const JointCAngDeltaY: JointVars = 47; -const JointCAngDeltaZ: JointVars = 48; +const JointCQuatX: JointVars = 14; +const JointCQuatY: JointVars = 15; +const JointCQuatZ: JointVars = 16; +const JointCQuatW: JointVars = 17; +const JointLinearDoFN: JointVars = 18; +const JointAngularDoFN: JointVars = 19; +const JointDoF1: JointVars = 20; +const JointDoF2: JointVars = 21; +const JointDoF3: JointVars = 22; +const JointDoF4: JointVars = 23; +const JointDoF5: JointVars = 24; +const JointDoF6: JointVars = 25; +const JointPForceX: JointVars = 26; +const JointPForceY: JointVars = 27; +const JointPForceZ: JointVars = 28; +const JointPTorqueX: JointVars = 29; +const JointPTorqueY: JointVars = 30; +const JointPTorqueZ: JointVars = 31; +const JointCForceX: JointVars = 32; +const JointCForceY: JointVars = 33; +const JointCForceZ: JointVars = 34; +const JointCTorqueX: JointVars = 35; +const JointCTorqueY: JointVars = 36; +const JointCTorqueZ: JointVars = 37; +const JointPDeltaX: JointVars = 38; +const JointPDeltaY: JointVars = 39; +const JointPDeltaZ: JointVars = 40; +const JointPAngDeltaX: JointVars = 41; +const JointPAngDeltaY: JointVars = 42; +const JointPAngDeltaZ: JointVars = 43; +const JointCDeltaX: JointVars = 44; +const JointCDeltaY: JointVars = 45; +const JointCDeltaZ: JointVars = 46; +const JointCAngDeltaX: JointVars = 47; +const JointCAngDeltaY: JointVars = 48; +const JointCAngDeltaZ: JointVars = 49; fn JointPForce(idx: i32) -> vec3 { return vec3(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPForceX))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPForceY))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPForceZ))]); } diff --git a/physics/shaders/InitDynamics.wgsl b/physics/shaders/InitDynamics.wgsl index 65d875a8..605ffbed 100644 --- a/physics/shaders/InitDynamics.wgsl +++ b/physics/shaders/InitDynamics.wgsl @@ -47,10 +47,10 @@ const BodyFriction: BodyVars = 8; const BodyPosX: BodyVars = 9; const BodyPosY: BodyVars = 10; const BodyPosZ: BodyVars = 11; -const BodyRotX: BodyVars = 12; -const BodyRotY: BodyVars = 13; -const BodyRotZ: BodyVars = 14; -const BodyRotW: BodyVars = 15; +const BodyQuatX: BodyVars = 12; +const BodyQuatY: BodyVars = 13; +const BodyQuatZ: BodyVars = 14; +const BodyQuatW: BodyVars = 15; const BodyComX: BodyVars = 16; const BodyComY: BodyVars = 17; const BodyComZ: BodyVars = 18; @@ -97,10 +97,10 @@ const DynIndex: DynamicVars = 0; const DynPosX: DynamicVars = 1; const DynPosY: DynamicVars = 2; const DynPosZ: DynamicVars = 3; -const DynRotX: DynamicVars = 4; -const DynRotY: DynamicVars = 5; -const DynRotZ: DynamicVars = 6; -const DynRotW: DynamicVars = 7; +const DynQuatX: DynamicVars = 4; +const DynQuatY: DynamicVars = 5; +const DynQuatZ: DynamicVars = 6; +const DynQuatW: DynamicVars = 7; const DynVelX: DynamicVars = 8; const DynVelY: DynamicVars = 9; const DynVelZ: DynamicVars = 10; @@ -136,7 +136,7 @@ const JointControlVarsN: JointControlVars = 3; const DynamicVarsN: DynamicVars = 32; const GPUVarsN: GPUVars = 8; const JointTypesN: JointTypes = 7; -const JointVarsN: JointVars = 49; +const JointVarsN: JointVars = 50; const JointDoFVarsN: JointDoFVars = 7; const ShapesN: Shapes = 4; @@ -158,48 +158,49 @@ const JointChild: JointVars = 3; const JointPPosX: JointVars = 4; const JointPPosY: JointVars = 5; const JointPPosZ: JointVars = 6; -const JointPRotX: JointVars = 7; -const JointPRotY: JointVars = 8; -const JointPRotZ: JointVars = 9; -const JointPRotW: JointVars = 10; +const JointPQuatX: JointVars = 7; +const JointPQuatY: JointVars = 8; +const JointPQuatZ: JointVars = 9; +const JointPQuatW: JointVars = 10; const JointCPosX: JointVars = 11; const JointCPosY: JointVars = 12; const JointCPosZ: JointVars = 13; -const JointCRotX: JointVars = 14; -const JointCRotY: JointVars = 15; -const JointCRotZ: JointVars = 16; -const JointCRotW: JointVars = 17; -const JointDoFN: JointVars = 18; -const JointDoF1: JointVars = 19; -const JointDoF2: JointVars = 20; -const JointDoF3: JointVars = 21; -const JointDoF4: JointVars = 22; -const JointDoF5: JointVars = 23; -const JointDoF6: JointVars = 24; -const JointPForceX: JointVars = 25; -const JointPForceY: JointVars = 26; -const JointPForceZ: JointVars = 27; -const JointPTorqueX: JointVars = 28; -const JointPTorqueY: JointVars = 29; -const JointPTorqueZ: JointVars = 30; -const JointCForceX: JointVars = 31; -const JointCForceY: JointVars = 32; -const JointCForceZ: JointVars = 33; -const JointCTorqueX: JointVars = 34; -const JointCTorqueY: JointVars = 35; -const JointCTorqueZ: JointVars = 36; -const JointPDeltaX: JointVars = 37; -const JointPDeltaY: JointVars = 38; -const JointPDeltaZ: JointVars = 39; -const JointPAngDeltaX: JointVars = 40; -const JointPAngDeltaY: JointVars = 41; -const JointPAngDeltaZ: JointVars = 42; -const JointCDeltaX: JointVars = 43; -const JointCDeltaY: JointVars = 44; -const JointCDeltaZ: JointVars = 45; -const JointCAngDeltaX: JointVars = 46; -const JointCAngDeltaY: JointVars = 47; -const JointCAngDeltaZ: JointVars = 48; +const JointCQuatX: JointVars = 14; +const JointCQuatY: JointVars = 15; +const JointCQuatZ: JointVars = 16; +const JointCQuatW: JointVars = 17; +const JointLinearDoFN: JointVars = 18; +const JointAngularDoFN: JointVars = 19; +const JointDoF1: JointVars = 20; +const JointDoF2: JointVars = 21; +const JointDoF3: JointVars = 22; +const JointDoF4: JointVars = 23; +const JointDoF5: JointVars = 24; +const JointDoF6: JointVars = 25; +const JointPForceX: JointVars = 26; +const JointPForceY: JointVars = 27; +const JointPForceZ: JointVars = 28; +const JointPTorqueX: JointVars = 29; +const JointPTorqueY: JointVars = 30; +const JointPTorqueZ: JointVars = 31; +const JointCForceX: JointVars = 32; +const JointCForceY: JointVars = 33; +const JointCForceZ: JointVars = 34; +const JointCTorqueX: JointVars = 35; +const JointCTorqueY: JointVars = 36; +const JointCTorqueZ: JointVars = 37; +const JointPDeltaX: JointVars = 38; +const JointPDeltaY: JointVars = 39; +const JointPDeltaZ: JointVars = 40; +const JointPAngDeltaX: JointVars = 41; +const JointPAngDeltaY: JointVars = 42; +const JointPAngDeltaZ: JointVars = 43; +const JointCDeltaX: JointVars = 44; +const JointCDeltaY: JointVars = 45; +const JointCDeltaZ: JointVars = 46; +const JointCAngDeltaX: JointVars = 47; +const JointCAngDeltaY: JointVars = 48; +const JointCAngDeltaZ: JointVars = 49; alias JointDoFVars = i32; //enums:enum const JointAxisX: JointDoFVars = 0; const JointAxisY: JointDoFVars = 1; @@ -261,10 +262,10 @@ fn InitDynamics(i: u32) { //gosl:kernel Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(ii), u32(cni), u32(DynPosX))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyPosX))]; Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(ii), u32(cni), u32(DynPosY))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyPosY))]; Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(ii), u32(cni), u32(DynPosZ))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyPosZ))]; - Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(ii), u32(cni), u32(DynRotX))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyRotX))]; - Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(ii), u32(cni), u32(DynRotY))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyRotY))]; - Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(ii), u32(cni), u32(DynRotZ))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyRotZ))]; - Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(ii), u32(cni), u32(DynRotW))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyRotW))]; + Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(ii), u32(cni), u32(DynQuatX))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyQuatX))]; + Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(ii), u32(cni), u32(DynQuatY))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyQuatY))]; + Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(ii), u32(cni), u32(DynQuatZ))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyQuatZ))]; + Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(ii), u32(cni), u32(DynQuatW))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyQuatW))]; for (var v = DynVelX; v < DynamicVarsN; v++) { Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(ii), u32(cni), u32(v))] = 0.0; diff --git a/physics/shaders/StepBodyDeltas.wgsl b/physics/shaders/StepBodyDeltas.wgsl index 438da3af..1790ee45 100644 --- a/physics/shaders/StepBodyDeltas.wgsl +++ b/physics/shaders/StepBodyDeltas.wgsl @@ -47,10 +47,10 @@ const BodyFriction: BodyVars = 8; const BodyPosX: BodyVars = 9; const BodyPosY: BodyVars = 10; const BodyPosZ: BodyVars = 11; -const BodyRotX: BodyVars = 12; -const BodyRotY: BodyVars = 13; -const BodyRotZ: BodyVars = 14; -const BodyRotW: BodyVars = 15; +const BodyQuatX: BodyVars = 12; +const BodyQuatY: BodyVars = 13; +const BodyQuatZ: BodyVars = 14; +const BodyQuatW: BodyVars = 15; const BodyComX: BodyVars = 16; const BodyComY: BodyVars = 17; const BodyComZ: BodyVars = 18; @@ -110,10 +110,10 @@ const DynIndex: DynamicVars = 0; const DynPosX: DynamicVars = 1; const DynPosY: DynamicVars = 2; const DynPosZ: DynamicVars = 3; -const DynRotX: DynamicVars = 4; -const DynRotY: DynamicVars = 5; -const DynRotZ: DynamicVars = 6; -const DynRotW: DynamicVars = 7; +const DynQuatX: DynamicVars = 4; +const DynQuatY: DynamicVars = 5; +const DynQuatZ: DynamicVars = 6; +const DynQuatW: DynamicVars = 7; const DynVelX: DynamicVars = 8; const DynVelY: DynamicVars = 9; const DynVelZ: DynamicVars = 10; @@ -149,14 +149,14 @@ fn SetDynamicPos(idx: i32,cni: i32, pos: vec3) { Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynPosY))] = pos.y; Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynPosZ))] = pos.z; } -fn DynamicRot(idx: i32,cni: i32) -> vec4 { - return vec4(Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynRotX))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynRotY))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynRotZ))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynRotW))]); +fn DynamicQuat(idx: i32,cni: i32) -> vec4 { + return vec4(Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynQuatX))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynQuatY))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynQuatZ))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynQuatW))]); } -fn SetDynamicRot(idx: i32,cni: i32, rot: vec4) { - Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynRotX))] = rot.x; - Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynRotY))] = rot.y; - Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynRotZ))] = rot.z; - Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynRotW))] = rot.w; +fn SetDynamicQuat(idx: i32,cni: i32, rot: vec4) { + Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynQuatX))] = rot.x; + Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynQuatY))] = rot.y; + Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynQuatZ))] = rot.z; + Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynQuatW))] = rot.w; } fn DynamicDelta(idx: i32,cni: i32) -> vec3 { return vec3(Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynDeltaX))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynDeltaY))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynDeltaZ))]); @@ -182,7 +182,7 @@ const JointControlVarsN: JointControlVars = 3; const DynamicVarsN: DynamicVars = 32; const GPUVarsN: GPUVars = 8; const JointTypesN: JointTypes = 7; -const JointVarsN: JointVars = 49; +const JointVarsN: JointVars = 50; const JointDoFVarsN: JointDoFVars = 7; const ShapesN: Shapes = 4; @@ -204,48 +204,49 @@ const JointChild: JointVars = 3; const JointPPosX: JointVars = 4; const JointPPosY: JointVars = 5; const JointPPosZ: JointVars = 6; -const JointPRotX: JointVars = 7; -const JointPRotY: JointVars = 8; -const JointPRotZ: JointVars = 9; -const JointPRotW: JointVars = 10; +const JointPQuatX: JointVars = 7; +const JointPQuatY: JointVars = 8; +const JointPQuatZ: JointVars = 9; +const JointPQuatW: JointVars = 10; const JointCPosX: JointVars = 11; const JointCPosY: JointVars = 12; const JointCPosZ: JointVars = 13; -const JointCRotX: JointVars = 14; -const JointCRotY: JointVars = 15; -const JointCRotZ: JointVars = 16; -const JointCRotW: JointVars = 17; -const JointDoFN: JointVars = 18; -const JointDoF1: JointVars = 19; -const JointDoF2: JointVars = 20; -const JointDoF3: JointVars = 21; -const JointDoF4: JointVars = 22; -const JointDoF5: JointVars = 23; -const JointDoF6: JointVars = 24; -const JointPForceX: JointVars = 25; -const JointPForceY: JointVars = 26; -const JointPForceZ: JointVars = 27; -const JointPTorqueX: JointVars = 28; -const JointPTorqueY: JointVars = 29; -const JointPTorqueZ: JointVars = 30; -const JointCForceX: JointVars = 31; -const JointCForceY: JointVars = 32; -const JointCForceZ: JointVars = 33; -const JointCTorqueX: JointVars = 34; -const JointCTorqueY: JointVars = 35; -const JointCTorqueZ: JointVars = 36; -const JointPDeltaX: JointVars = 37; -const JointPDeltaY: JointVars = 38; -const JointPDeltaZ: JointVars = 39; -const JointPAngDeltaX: JointVars = 40; -const JointPAngDeltaY: JointVars = 41; -const JointPAngDeltaZ: JointVars = 42; -const JointCDeltaX: JointVars = 43; -const JointCDeltaY: JointVars = 44; -const JointCDeltaZ: JointVars = 45; -const JointCAngDeltaX: JointVars = 46; -const JointCAngDeltaY: JointVars = 47; -const JointCAngDeltaZ: JointVars = 48; +const JointCQuatX: JointVars = 14; +const JointCQuatY: JointVars = 15; +const JointCQuatZ: JointVars = 16; +const JointCQuatW: JointVars = 17; +const JointLinearDoFN: JointVars = 18; +const JointAngularDoFN: JointVars = 19; +const JointDoF1: JointVars = 20; +const JointDoF2: JointVars = 21; +const JointDoF3: JointVars = 22; +const JointDoF4: JointVars = 23; +const JointDoF5: JointVars = 24; +const JointDoF6: JointVars = 25; +const JointPForceX: JointVars = 26; +const JointPForceY: JointVars = 27; +const JointPForceZ: JointVars = 28; +const JointPTorqueX: JointVars = 29; +const JointPTorqueY: JointVars = 30; +const JointPTorqueZ: JointVars = 31; +const JointCForceX: JointVars = 32; +const JointCForceY: JointVars = 33; +const JointCForceZ: JointVars = 34; +const JointCTorqueX: JointVars = 35; +const JointCTorqueY: JointVars = 36; +const JointCTorqueZ: JointVars = 37; +const JointPDeltaX: JointVars = 38; +const JointPDeltaY: JointVars = 39; +const JointPDeltaZ: JointVars = 40; +const JointPAngDeltaX: JointVars = 41; +const JointPAngDeltaY: JointVars = 42; +const JointPAngDeltaZ: JointVars = 43; +const JointCDeltaX: JointVars = 44; +const JointCDeltaY: JointVars = 45; +const JointCDeltaZ: JointVars = 46; +const JointCAngDeltaX: JointVars = 47; +const JointCAngDeltaY: JointVars = 48; +const JointCAngDeltaZ: JointVars = 49; alias JointDoFVars = i32; //enums:enum const JointAxisX: JointDoFVars = 0; const JointAxisY: JointDoFVars = 1; @@ -353,7 +354,7 @@ fn StepBodyDeltas(i: u32) { //gosl:kernel var inertia = BodyInertia(bi); var invInertia = BodyInvInertia(bi); var p0 = DynamicPos(di, params.Next); - var q0 = DynamicRot(di, params.Next); + var q0 = DynamicQuat(di, params.Next); var v0 = DynamicDelta(di, params.Next); var w0 = DynamicAngDelta(di, params.Next); var weight = f32(1.0); @@ -378,7 +379,7 @@ fn StepBodyDeltas(i: u32) { //gosl:kernel w1 = vec3(0, 0, 0); } SetDynamicPos(di, params.Next, p1); - SetDynamicRot(di, params.Next, q1); + SetDynamicQuat(di, params.Next, q1); SetDynamicDelta(di, params.Next, v1); SetDynamicAngDelta(di, params.Next, w1); } diff --git a/physics/shaders/StepIntegrateBodies.wgsl b/physics/shaders/StepIntegrateBodies.wgsl index 647cab2f..280220b1 100644 --- a/physics/shaders/StepIntegrateBodies.wgsl +++ b/physics/shaders/StepIntegrateBodies.wgsl @@ -47,10 +47,10 @@ const BodyFriction: BodyVars = 8; const BodyPosX: BodyVars = 9; const BodyPosY: BodyVars = 10; const BodyPosZ: BodyVars = 11; -const BodyRotX: BodyVars = 12; -const BodyRotY: BodyVars = 13; -const BodyRotZ: BodyVars = 14; -const BodyRotW: BodyVars = 15; +const BodyQuatX: BodyVars = 12; +const BodyQuatY: BodyVars = 13; +const BodyQuatZ: BodyVars = 14; +const BodyQuatW: BodyVars = 15; const BodyComX: BodyVars = 16; const BodyComY: BodyVars = 17; const BodyComZ: BodyVars = 18; @@ -110,10 +110,10 @@ const DynIndex: DynamicVars = 0; const DynPosX: DynamicVars = 1; const DynPosY: DynamicVars = 2; const DynPosZ: DynamicVars = 3; -const DynRotX: DynamicVars = 4; -const DynRotY: DynamicVars = 5; -const DynRotZ: DynamicVars = 6; -const DynRotW: DynamicVars = 7; +const DynQuatX: DynamicVars = 4; +const DynQuatY: DynamicVars = 5; +const DynQuatZ: DynamicVars = 6; +const DynQuatW: DynamicVars = 7; const DynVelX: DynamicVars = 8; const DynVelY: DynamicVars = 9; const DynVelZ: DynamicVars = 10; @@ -149,14 +149,14 @@ fn SetDynamicPos(idx: i32,cni: i32, pos: vec3) { Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynPosY))] = pos.y; Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynPosZ))] = pos.z; } -fn DynamicRot(idx: i32,cni: i32) -> vec4 { - return vec4(Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynRotX))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynRotY))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynRotZ))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynRotW))]); +fn DynamicQuat(idx: i32,cni: i32) -> vec4 { + return vec4(Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynQuatX))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynQuatY))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynQuatZ))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynQuatW))]); } -fn SetDynamicRot(idx: i32,cni: i32, rot: vec4) { - Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynRotX))] = rot.x; - Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynRotY))] = rot.y; - Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynRotZ))] = rot.z; - Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynRotW))] = rot.w; +fn SetDynamicQuat(idx: i32,cni: i32, rot: vec4) { + Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynQuatX))] = rot.x; + Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynQuatY))] = rot.y; + Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynQuatZ))] = rot.z; + Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynQuatW))] = rot.w; } fn DynamicForce(idx: i32,cni: i32) -> vec3 { return vec3(Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynForceX))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynForceY))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynForceZ))]); @@ -188,7 +188,7 @@ const JointControlVarsN: JointControlVars = 3; const DynamicVarsN: DynamicVars = 32; const GPUVarsN: GPUVars = 8; const JointTypesN: JointTypes = 7; -const JointVarsN: JointVars = 49; +const JointVarsN: JointVars = 50; const JointDoFVarsN: JointDoFVars = 7; const ShapesN: Shapes = 4; @@ -210,48 +210,49 @@ const JointChild: JointVars = 3; const JointPPosX: JointVars = 4; const JointPPosY: JointVars = 5; const JointPPosZ: JointVars = 6; -const JointPRotX: JointVars = 7; -const JointPRotY: JointVars = 8; -const JointPRotZ: JointVars = 9; -const JointPRotW: JointVars = 10; +const JointPQuatX: JointVars = 7; +const JointPQuatY: JointVars = 8; +const JointPQuatZ: JointVars = 9; +const JointPQuatW: JointVars = 10; const JointCPosX: JointVars = 11; const JointCPosY: JointVars = 12; const JointCPosZ: JointVars = 13; -const JointCRotX: JointVars = 14; -const JointCRotY: JointVars = 15; -const JointCRotZ: JointVars = 16; -const JointCRotW: JointVars = 17; -const JointDoFN: JointVars = 18; -const JointDoF1: JointVars = 19; -const JointDoF2: JointVars = 20; -const JointDoF3: JointVars = 21; -const JointDoF4: JointVars = 22; -const JointDoF5: JointVars = 23; -const JointDoF6: JointVars = 24; -const JointPForceX: JointVars = 25; -const JointPForceY: JointVars = 26; -const JointPForceZ: JointVars = 27; -const JointPTorqueX: JointVars = 28; -const JointPTorqueY: JointVars = 29; -const JointPTorqueZ: JointVars = 30; -const JointCForceX: JointVars = 31; -const JointCForceY: JointVars = 32; -const JointCForceZ: JointVars = 33; -const JointCTorqueX: JointVars = 34; -const JointCTorqueY: JointVars = 35; -const JointCTorqueZ: JointVars = 36; -const JointPDeltaX: JointVars = 37; -const JointPDeltaY: JointVars = 38; -const JointPDeltaZ: JointVars = 39; -const JointPAngDeltaX: JointVars = 40; -const JointPAngDeltaY: JointVars = 41; -const JointPAngDeltaZ: JointVars = 42; -const JointCDeltaX: JointVars = 43; -const JointCDeltaY: JointVars = 44; -const JointCDeltaZ: JointVars = 45; -const JointCAngDeltaX: JointVars = 46; -const JointCAngDeltaY: JointVars = 47; -const JointCAngDeltaZ: JointVars = 48; +const JointCQuatX: JointVars = 14; +const JointCQuatY: JointVars = 15; +const JointCQuatZ: JointVars = 16; +const JointCQuatW: JointVars = 17; +const JointLinearDoFN: JointVars = 18; +const JointAngularDoFN: JointVars = 19; +const JointDoF1: JointVars = 20; +const JointDoF2: JointVars = 21; +const JointDoF3: JointVars = 22; +const JointDoF4: JointVars = 23; +const JointDoF5: JointVars = 24; +const JointDoF6: JointVars = 25; +const JointPForceX: JointVars = 26; +const JointPForceY: JointVars = 27; +const JointPForceZ: JointVars = 28; +const JointPTorqueX: JointVars = 29; +const JointPTorqueY: JointVars = 30; +const JointPTorqueZ: JointVars = 31; +const JointCForceX: JointVars = 32; +const JointCForceY: JointVars = 33; +const JointCForceZ: JointVars = 34; +const JointCTorqueX: JointVars = 35; +const JointCTorqueY: JointVars = 36; +const JointCTorqueZ: JointVars = 37; +const JointPDeltaX: JointVars = 38; +const JointPDeltaY: JointVars = 39; +const JointPDeltaZ: JointVars = 40; +const JointPAngDeltaX: JointVars = 41; +const JointPAngDeltaY: JointVars = 42; +const JointPAngDeltaZ: JointVars = 43; +const JointCDeltaX: JointVars = 44; +const JointCDeltaY: JointVars = 45; +const JointCDeltaZ: JointVars = 46; +const JointCAngDeltaX: JointVars = 47; +const JointCAngDeltaY: JointVars = 48; +const JointCAngDeltaZ: JointVars = 49; alias JointDoFVars = i32; //enums:enum const JointAxisX: JointDoFVars = 0; const JointAxisY: JointDoFVars = 1; @@ -359,7 +360,7 @@ fn StepIntegrateBodies(i: u32) { //gosl:kernel var invInertia = BodyInvInertia(bi); var com = BodyCom(bi); var p0 = DynamicPos(di, params.Cur); - var q0 = DynamicRot(di, params.Cur); + var q0 = DynamicQuat(di, params.Cur); var v0 = DynamicDelta(di, params.Cur); var w0 = DynamicAngDelta(di, params.Cur); var f0 = DynamicForce(di, params.Next); @@ -375,7 +376,7 @@ fn StepIntegrateBodies(i: u32) { //gosl:kernel w1 = w1*(1.0 - params.AngularDamping*params.Dt); var p1a = p1-(MulQuatVector(q1, com)); // pos corrected to nominal center. SetDynamicPos(di, params.Next, p1a); - SetDynamicRot(di, params.Next, q1); + SetDynamicQuat(di, params.Next, q1); SetDynamicDelta(di, params.Next, v1); SetDynamicAngDelta(di, params.Next, w1); } diff --git a/physics/shaders/StepJointForces.wgsl b/physics/shaders/StepJointForces.wgsl index c6564f02..41f148ee 100644 --- a/physics/shaders/StepJointForces.wgsl +++ b/physics/shaders/StepJointForces.wgsl @@ -53,10 +53,10 @@ const BodyFriction: BodyVars = 8; const BodyPosX: BodyVars = 9; const BodyPosY: BodyVars = 10; const BodyPosZ: BodyVars = 11; -const BodyRotX: BodyVars = 12; -const BodyRotY: BodyVars = 13; -const BodyRotZ: BodyVars = 14; -const BodyRotW: BodyVars = 15; +const BodyQuatX: BodyVars = 12; +const BodyQuatY: BodyVars = 13; +const BodyQuatZ: BodyVars = 14; +const BodyQuatW: BodyVars = 15; const BodyComX: BodyVars = 16; const BodyComY: BodyVars = 17; const BodyComZ: BodyVars = 18; @@ -99,8 +99,8 @@ alias JointControlVars = i32; //enums:enum const JointControlForce: JointControlVars = 0; const JointTargetPos: JointControlVars = 1; const JointTargetVel: JointControlVars = 2; -fn GetJointControlForce(idx: i32,dof: i32) -> f32 { - return JointControls[Index2D(TensorStrides[60], TensorStrides[61], u32(JointDoFIndex(idx, dof)), u32(JointControlForce))]; +fn JointControl(idx: i32,dof: i32, vr: JointControlVars) -> f32 { + return JointControls[Index2D(TensorStrides[60], TensorStrides[61], u32(JointDoFIndex(idx, dof)), u32(vr))]; } //////// import: "dynamics.go" @@ -109,10 +109,10 @@ const DynIndex: DynamicVars = 0; const DynPosX: DynamicVars = 1; const DynPosY: DynamicVars = 2; const DynPosZ: DynamicVars = 3; -const DynRotX: DynamicVars = 4; -const DynRotY: DynamicVars = 5; -const DynRotZ: DynamicVars = 6; -const DynRotW: DynamicVars = 7; +const DynQuatX: DynamicVars = 4; +const DynQuatY: DynamicVars = 5; +const DynQuatZ: DynamicVars = 6; +const DynQuatW: DynamicVars = 7; const DynVelX: DynamicVars = 8; const DynVelY: DynamicVars = 9; const DynVelZ: DynamicVars = 10; @@ -143,8 +143,8 @@ fn DynamicIndex(idx: i32,cni: i32) -> i32 { fn DynamicPos(idx: i32,cni: i32) -> vec3 { return vec3(Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynPosX))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynPosY))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynPosZ))]); } -fn DynamicRot(idx: i32,cni: i32) -> vec4 { - return vec4(Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynRotX))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynRotY))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynRotZ))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynRotW))]); +fn DynamicQuat(idx: i32,cni: i32) -> vec4 { + return vec4(Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynQuatX))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynQuatY))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynQuatZ))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynQuatW))]); } //////// import: "enumgen.go" @@ -154,7 +154,7 @@ const JointControlVarsN: JointControlVars = 3; const DynamicVarsN: DynamicVars = 32; const GPUVarsN: GPUVars = 8; const JointTypesN: JointTypes = 7; -const JointVarsN: JointVars = 49; +const JointVarsN: JointVars = 50; const JointDoFVarsN: JointDoFVars = 7; const ShapesN: Shapes = 4; @@ -176,65 +176,75 @@ const JointChild: JointVars = 3; const JointPPosX: JointVars = 4; const JointPPosY: JointVars = 5; const JointPPosZ: JointVars = 6; -const JointPRotX: JointVars = 7; -const JointPRotY: JointVars = 8; -const JointPRotZ: JointVars = 9; -const JointPRotW: JointVars = 10; +const JointPQuatX: JointVars = 7; +const JointPQuatY: JointVars = 8; +const JointPQuatZ: JointVars = 9; +const JointPQuatW: JointVars = 10; const JointCPosX: JointVars = 11; const JointCPosY: JointVars = 12; const JointCPosZ: JointVars = 13; -const JointCRotX: JointVars = 14; -const JointCRotY: JointVars = 15; -const JointCRotZ: JointVars = 16; -const JointCRotW: JointVars = 17; -const JointDoFN: JointVars = 18; -const JointDoF1: JointVars = 19; -const JointDoF2: JointVars = 20; -const JointDoF3: JointVars = 21; -const JointDoF4: JointVars = 22; -const JointDoF5: JointVars = 23; -const JointDoF6: JointVars = 24; -const JointPForceX: JointVars = 25; -const JointPForceY: JointVars = 26; -const JointPForceZ: JointVars = 27; -const JointPTorqueX: JointVars = 28; -const JointPTorqueY: JointVars = 29; -const JointPTorqueZ: JointVars = 30; -const JointCForceX: JointVars = 31; -const JointCForceY: JointVars = 32; -const JointCForceZ: JointVars = 33; -const JointCTorqueX: JointVars = 34; -const JointCTorqueY: JointVars = 35; -const JointCTorqueZ: JointVars = 36; -const JointPDeltaX: JointVars = 37; -const JointPDeltaY: JointVars = 38; -const JointPDeltaZ: JointVars = 39; -const JointPAngDeltaX: JointVars = 40; -const JointPAngDeltaY: JointVars = 41; -const JointPAngDeltaZ: JointVars = 42; -const JointCDeltaX: JointVars = 43; -const JointCDeltaY: JointVars = 44; -const JointCDeltaZ: JointVars = 45; -const JointCAngDeltaX: JointVars = 46; -const JointCAngDeltaY: JointVars = 47; -const JointCAngDeltaZ: JointVars = 48; +const JointCQuatX: JointVars = 14; +const JointCQuatY: JointVars = 15; +const JointCQuatZ: JointVars = 16; +const JointCQuatW: JointVars = 17; +const JointLinearDoFN: JointVars = 18; +const JointAngularDoFN: JointVars = 19; +const JointDoF1: JointVars = 20; +const JointDoF2: JointVars = 21; +const JointDoF3: JointVars = 22; +const JointDoF4: JointVars = 23; +const JointDoF5: JointVars = 24; +const JointDoF6: JointVars = 25; +const JointPForceX: JointVars = 26; +const JointPForceY: JointVars = 27; +const JointPForceZ: JointVars = 28; +const JointPTorqueX: JointVars = 29; +const JointPTorqueY: JointVars = 30; +const JointPTorqueZ: JointVars = 31; +const JointCForceX: JointVars = 32; +const JointCForceY: JointVars = 33; +const JointCForceZ: JointVars = 34; +const JointCTorqueX: JointVars = 35; +const JointCTorqueY: JointVars = 36; +const JointCTorqueZ: JointVars = 37; +const JointPDeltaX: JointVars = 38; +const JointPDeltaY: JointVars = 39; +const JointPDeltaZ: JointVars = 40; +const JointPAngDeltaX: JointVars = 41; +const JointPAngDeltaY: JointVars = 42; +const JointPAngDeltaZ: JointVars = 43; +const JointCDeltaX: JointVars = 44; +const JointCDeltaY: JointVars = 45; +const JointCDeltaZ: JointVars = 46; +const JointCAngDeltaX: JointVars = 47; +const JointCAngDeltaY: JointVars = 48; +const JointCAngDeltaZ: JointVars = 49; fn GetJointType(idx: i32) -> JointTypes { return JointTypes(bitcast(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointType))])); } +fn GetJointEnabled(idx: i32) -> bool { + var je = bitcast(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointEnabled))]);return je != 0; +} fn JointParentIndex(idx: i32) -> i32 { return i32(bitcast(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointParent))])); } fn JointChildIndex(idx: i32) -> i32 { return i32(bitcast(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointChild))])); } +fn GetJointLinearDoFN(idx: i32) -> i32 { + return i32(bitcast(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointLinearDoFN))])); +} +fn GetJointAngularDoFN(idx: i32) -> i32 { + return i32(bitcast(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointAngularDoFN))])); +} fn JointDoFIndex(idx: i32,dof: i32) -> i32 { return i32(bitcast(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(i32(JointDoF1) + dof))])); } fn JointPPos(idx: i32) -> vec3 { return vec3(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPPosX))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPPosY))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPPosZ))]); } -fn JointPRot(idx: i32) -> vec4 { - return vec4(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPRotX))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPRotY))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPRotZ))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPRotW))]); +fn JointPQuat(idx: i32) -> vec4 { + return vec4(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPQuatX))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPQuatY))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPQuatZ))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPQuatW))]); } fn SetJointPForce(idx: i32, f: vec3) { Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPForceX))] = f.x; @@ -317,11 +327,11 @@ fn MulQuats(a: vec4,b: vec4) -> vec4 { q.z = a.z*b.w + a.w*b.z + a.x*b.y - a.y*b.x; q.w = a.w*b.w - a.x*b.x - a.y*b.y - a.z*b.z;return q; } -fn MulQPTransforms(aP: vec3, aQ: vec4, bP: vec3, bQ: vec4, oP: ptr>, oQ: ptr>) { +fn MulSpatialTransforms(aP: vec3, aQ: vec4, bP: vec3, bQ: vec4, oP: ptr>, oQ: ptr>) { *oP = MulQuatVector(aQ, bP)+(aP); *oQ = MulQuats(aQ, bQ); } -fn MulQPPoint(xP: vec3, xQ: vec4, p: vec3) -> vec3 { +fn MulSpatialPoint(xP: vec3, xQ: vec4, p: vec3) -> vec3 { var dp = MulQuatVector(xQ, p);return dp+(xP); } @@ -344,53 +354,69 @@ fn StepJointForces(i: u32) { //gosl:kernel if (ji >= params.JointsN) { return; } - var jpi = JointParentIndex(ji); - var jpbi = i32(-1); - if (jpi >= 0) { - jpbi = DynamicIndex(jpi, params.Cur); - } - var jci = JointChildIndex(ji); - var jcbi = DynamicIndex(jci, params.Cur); var jt = GetJointType(ji); - var jpP = JointPPos(ji); - var jpQ = JointPRot(ji); - var xwpP = jpP; - var xwpQ = jpQ; - var posepP = jpP; - var posepQ = jpQ; - var comp: vec3; - if (jpi >= 0) { // can be fixed - posepP = DynamicPos(jpi, params.Cur); - posepQ = DynamicRot(jpi, params.Cur); - MulQPTransforms(posepP, posepQ, jpP, jpQ, &xwpP, &xwpQ); - comp = BodyCom(jpbi); + if (!GetJointEnabled(ji)) { + return; } - var rp = xwpP-(MulQPPoint(posepP, posepQ, comp)); // parent moment arm - var posecP = DynamicPos(jci, params.Cur); - var posecQ = DynamicRot(jci, params.Cur); - var xwcP = posecP; - var comc = BodyCom(jcbi); - var rc = xwcP-(MulQPPoint(posecP, posecQ, comc)); // child moment arm + var jPi = JointParentIndex(ji); + var jPbi = i32(-1); + if (jPi >= 0) { + jPbi = DynamicIndex(jPi, params.Cur); + } + var jCi = JointChildIndex(ji); + var jCbi = DynamicIndex(jCi, params.Cur); + var jLinearN = GetJointLinearDoFN(ji); + var jAngularN = GetJointAngularDoFN(ji); + var jPR = JointPPos(ji); + var jPQ = JointPQuat(ji); + var xwPR = jPR; + var xwPQ = jPQ; + var posePR = jPR; + var posePQ = jPQ; + var comP = vec3(0, 0, 0); + if (jPi >= 0) { // can be fixed + posePR = DynamicPos(jPi, params.Cur); + posePQ = DynamicQuat(jPi, params.Cur); + MulSpatialTransforms(posePR, posePQ, jPR, jPQ, &xwPR, &xwPQ); + comP = BodyCom(jPbi); + } + var dP = xwPR-(MulSpatialPoint(posePR, posePQ, comP)); // parent moment arm + var poseCR = DynamicPos(jCi, params.Cur); + var poseCQ = DynamicQuat(jCi, params.Cur); + var comC = BodyCom(jCbi); + var dC = poseCR-(MulSpatialPoint(poseCR, poseCQ, comC)); // child moment arm var f: vec3; var t: vec3; switch (jt) { case Free, Distance: { - f = vec3(GetJointControlForce(ji, i32(i32(0))), GetJointControlForce(ji, i32(i32(1))), GetJointControlForce(ji, i32(i32(2)))); - t = vec3(GetJointControlForce(ji, i32(i32(3))), GetJointControlForce(ji, i32(i32(4))), GetJointControlForce(ji, i32(i32(5)))); + f = vec3(JointControl(ji, i32(i32(0)), JointControlForce), JointControl(ji, i32(i32(1)), JointControlForce), JointControl(ji, i32(i32(2)), JointControlForce)); + t = vec3(JointControl(ji, i32(i32(3)), JointControlForce), JointControl(ji, i32(i32(4)), JointControlForce), JointControl(ji, i32(i32(5)), JointControlForce)); } case Ball: { - t = vec3(GetJointControlForce(ji, i32(i32(0))), GetJointControlForce(ji, i32(i32(1))), GetJointControlForce(ji, i32(i32(2)))); + t = vec3(JointControl(ji, i32(i32(0)), JointControlForce), JointControl(ji, i32(i32(1)), JointControlForce), JointControl(ji, i32(i32(2)), JointControlForce)); + } + case Revolute: { + var axis = JointAxis(ji, i32(i32(0))); + t = MulScalar3(MulQuatVector(xwPQ, axis), JointControl(ji, i32(i32(0)), JointControlForce)); } - case Revolute, Prismatic: { + case Prismatic: { var axis = JointAxis(ji, i32(i32(0))); - var ap = MulQuatVector(xwpQ, axis); - f = MulScalar3(ap, GetJointControlForce(ji, i32(i32(0)))); + f = MulScalar3(MulQuatVector(xwPQ, axis), JointControl(ji, i32(i32(0)), JointControlForce)); } default: { + for (var dof=0; dof f32 { - return JointControls[Index2D(TensorStrides[60], TensorStrides[61], u32(JointDoFIndex(idx, dof)), u32(JointTargetPos))]; -} -fn GetJointTargetVel(idx: i32,dof: i32) -> f32 { - return JointControls[Index2D(TensorStrides[60], TensorStrides[61], u32(JointDoFIndex(idx, dof)), u32(JointTargetVel))]; +fn JointControl(idx: i32,dof: i32, vr: JointControlVars) -> f32 { + return JointControls[Index2D(TensorStrides[60], TensorStrides[61], u32(JointDoFIndex(idx, dof)), u32(vr))]; } //////// import: "dynamics.go" @@ -117,10 +114,10 @@ const DynIndex: DynamicVars = 0; const DynPosX: DynamicVars = 1; const DynPosY: DynamicVars = 2; const DynPosZ: DynamicVars = 3; -const DynRotX: DynamicVars = 4; -const DynRotY: DynamicVars = 5; -const DynRotZ: DynamicVars = 6; -const DynRotW: DynamicVars = 7; +const DynQuatX: DynamicVars = 4; +const DynQuatY: DynamicVars = 5; +const DynQuatZ: DynamicVars = 6; +const DynQuatW: DynamicVars = 7; const DynVelX: DynamicVars = 8; const DynVelY: DynamicVars = 9; const DynVelZ: DynamicVars = 10; @@ -151,8 +148,8 @@ fn DynamicIndex(idx: i32,cni: i32) -> i32 { fn DynamicPos(idx: i32,cni: i32) -> vec3 { return vec3(Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynPosX))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynPosY))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynPosZ))]); } -fn DynamicRot(idx: i32,cni: i32) -> vec4 { - return vec4(Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynRotX))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynRotY))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynRotZ))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynRotW))]); +fn DynamicQuat(idx: i32,cni: i32) -> vec4 { + return vec4(Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynQuatX))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynQuatY))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynQuatZ))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynQuatW))]); } fn DynamicDelta(idx: i32,cni: i32) -> vec3 { return vec3(Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynDeltaX))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynDeltaY))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynDeltaZ))]); @@ -168,7 +165,7 @@ const JointControlVarsN: JointControlVars = 3; const DynamicVarsN: DynamicVars = 32; const GPUVarsN: GPUVars = 8; const JointTypesN: JointTypes = 7; -const JointVarsN: JointVars = 49; +const JointVarsN: JointVars = 50; const JointDoFVarsN: JointDoFVars = 7; const ShapesN: Shapes = 4; @@ -190,71 +187,81 @@ const JointChild: JointVars = 3; const JointPPosX: JointVars = 4; const JointPPosY: JointVars = 5; const JointPPosZ: JointVars = 6; -const JointPRotX: JointVars = 7; -const JointPRotY: JointVars = 8; -const JointPRotZ: JointVars = 9; -const JointPRotW: JointVars = 10; +const JointPQuatX: JointVars = 7; +const JointPQuatY: JointVars = 8; +const JointPQuatZ: JointVars = 9; +const JointPQuatW: JointVars = 10; const JointCPosX: JointVars = 11; const JointCPosY: JointVars = 12; const JointCPosZ: JointVars = 13; -const JointCRotX: JointVars = 14; -const JointCRotY: JointVars = 15; -const JointCRotZ: JointVars = 16; -const JointCRotW: JointVars = 17; -const JointDoFN: JointVars = 18; -const JointDoF1: JointVars = 19; -const JointDoF2: JointVars = 20; -const JointDoF3: JointVars = 21; -const JointDoF4: JointVars = 22; -const JointDoF5: JointVars = 23; -const JointDoF6: JointVars = 24; -const JointPForceX: JointVars = 25; -const JointPForceY: JointVars = 26; -const JointPForceZ: JointVars = 27; -const JointPTorqueX: JointVars = 28; -const JointPTorqueY: JointVars = 29; -const JointPTorqueZ: JointVars = 30; -const JointCForceX: JointVars = 31; -const JointCForceY: JointVars = 32; -const JointCForceZ: JointVars = 33; -const JointCTorqueX: JointVars = 34; -const JointCTorqueY: JointVars = 35; -const JointCTorqueZ: JointVars = 36; -const JointPDeltaX: JointVars = 37; -const JointPDeltaY: JointVars = 38; -const JointPDeltaZ: JointVars = 39; -const JointPAngDeltaX: JointVars = 40; -const JointPAngDeltaY: JointVars = 41; -const JointPAngDeltaZ: JointVars = 42; -const JointCDeltaX: JointVars = 43; -const JointCDeltaY: JointVars = 44; -const JointCDeltaZ: JointVars = 45; -const JointCAngDeltaX: JointVars = 46; -const JointCAngDeltaY: JointVars = 47; -const JointCAngDeltaZ: JointVars = 48; +const JointCQuatX: JointVars = 14; +const JointCQuatY: JointVars = 15; +const JointCQuatZ: JointVars = 16; +const JointCQuatW: JointVars = 17; +const JointLinearDoFN: JointVars = 18; +const JointAngularDoFN: JointVars = 19; +const JointDoF1: JointVars = 20; +const JointDoF2: JointVars = 21; +const JointDoF3: JointVars = 22; +const JointDoF4: JointVars = 23; +const JointDoF5: JointVars = 24; +const JointDoF6: JointVars = 25; +const JointPForceX: JointVars = 26; +const JointPForceY: JointVars = 27; +const JointPForceZ: JointVars = 28; +const JointPTorqueX: JointVars = 29; +const JointPTorqueY: JointVars = 30; +const JointPTorqueZ: JointVars = 31; +const JointCForceX: JointVars = 32; +const JointCForceY: JointVars = 33; +const JointCForceZ: JointVars = 34; +const JointCTorqueX: JointVars = 35; +const JointCTorqueY: JointVars = 36; +const JointCTorqueZ: JointVars = 37; +const JointPDeltaX: JointVars = 38; +const JointPDeltaY: JointVars = 39; +const JointPDeltaZ: JointVars = 40; +const JointPAngDeltaX: JointVars = 41; +const JointPAngDeltaY: JointVars = 42; +const JointPAngDeltaZ: JointVars = 43; +const JointCDeltaX: JointVars = 44; +const JointCDeltaY: JointVars = 45; +const JointCDeltaZ: JointVars = 46; +const JointCAngDeltaX: JointVars = 47; +const JointCAngDeltaY: JointVars = 48; +const JointCAngDeltaZ: JointVars = 49; fn GetJointType(idx: i32) -> JointTypes { return JointTypes(bitcast(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointType))])); } +fn GetJointEnabled(idx: i32) -> bool { + var je = bitcast(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointEnabled))]);return je != 0; +} fn JointParentIndex(idx: i32) -> i32 { return i32(bitcast(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointParent))])); } fn JointChildIndex(idx: i32) -> i32 { return i32(bitcast(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointChild))])); } +fn GetJointLinearDoFN(idx: i32) -> i32 { + return i32(bitcast(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointLinearDoFN))])); +} +fn GetJointAngularDoFN(idx: i32) -> i32 { + return i32(bitcast(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointAngularDoFN))])); +} fn JointDoFIndex(idx: i32,dof: i32) -> i32 { return i32(bitcast(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(i32(JointDoF1) + dof))])); } fn JointPPos(idx: i32) -> vec3 { return vec3(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPPosX))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPPosY))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPPosZ))]); } -fn JointPRot(idx: i32) -> vec4 { - return vec4(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPRotX))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPRotY))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPRotZ))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPRotW))]); +fn JointPQuat(idx: i32) -> vec4 { + return vec4(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPQuatX))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPQuatY))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPQuatZ))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPQuatW))]); } fn JointCPos(idx: i32) -> vec3 { return vec3(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCPosX))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCPosY))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCPosZ))]); } -fn JointCRot(idx: i32) -> vec4 { - return vec4(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCRotX))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCRotY))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCRotZ))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCRotW))]); +fn JointCQuat(idx: i32) -> vec4 { + return vec4(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCQuatX))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCQuatY))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCQuatZ))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCQuatW))]); } fn SetJointPDelta(idx: i32, f: vec3) { Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPDeltaX))] = f.x; @@ -363,14 +370,14 @@ fn MulQuats(a: vec4,b: vec4) -> vec4 { q.z = a.z*b.w + a.w*b.z + a.x*b.y - a.y*b.x; q.w = a.w*b.w - a.x*b.x - a.y*b.y - a.z*b.z;return q; } -fn MulQPTransforms(aP: vec3, aQ: vec4, bP: vec3, bQ: vec4, oP: ptr>, oQ: ptr>) { +fn MulSpatialTransforms(aP: vec3, aQ: vec4, bP: vec3, bQ: vec4, oP: ptr>, oQ: ptr>) { *oP = MulQuatVector(aQ, bP)+(aP); *oQ = MulQuats(aQ, bQ); } -fn MulQPPoint(xP: vec3, xQ: vec4, p: vec3) -> vec3 { +fn MulSpatialPoint(xP: vec3, xQ: vec4, p: vec3) -> vec3 { var dp = MulQuatVector(xQ, p);return dp+(xP); } -fn QPTransformInverse(p: vec3, q: vec4, oP: ptr>, oQ: ptr>) { +fn SpatialTransformInverse(p: vec3, q: vec4, oP: ptr>, oQ: ptr>) { var qi = QuatInverse(q); *oP = Negate3(MulQuatVector(qi, p)); *oQ = qi; @@ -381,14 +388,42 @@ fn QuatInverse(q: vec4) -> vec4 { nq.y *= f32(-1); nq.z *= f32(-1);return QuatNormalize(nq); } +fn QuatDot(q: vec4,o: vec4) -> f32 { + return q.x*o.x + q.y*o.y + q.z*o.z + q.w*o.w; +} +fn QuatMulScalar(q: vec4, s: f32) -> vec4 { + var nq = q; + nq.x *= s; + nq.y *= s; + nq.z *= s; + nq.w *= s;return nq; +} //////// import: "slmath-vector3.go" fn MulScalar3(v: vec3, s: f32) -> vec3 { return vec3(v.x*s, v.y*s, v.z*s); } +fn DivScalar3(v: vec3, s: f32) -> vec3 { + return vec3(v.x/s, v.y/s, v.z/s); +} +fn DivSafe3(v: vec3, o: vec3) -> vec3 { + var nv = v; + if (o.x != 0) { + nv.x /= o.x; + } + if (o.y != 0) { + nv.y /= o.y; + } + if (o.z != 0) { + nv.z /= o.z; + }return nv; +} fn Negate3(v: vec3) -> vec3 { return vec3(-v.x, -v.y, -v.z); } +fn Length3(v: vec3) -> f32 { + return sqrt(v.x*v.x + v.y*v.y + v.z*v.z); +} fn LengthSquared3(v: vec3) -> f32 { return v.x*v.x + v.y*v.y + v.z*v.z; } @@ -404,9 +439,32 @@ fn Min3(v: vec3,o: vec3) -> vec3 { fn Abs3(v: vec3) -> vec3 { return vec3(abs(v.x), abs(v.y), abs(v.z)); } +fn Normal3(v: vec3) -> vec3 { + return DivScalar3(v, Length3(v)); +} fn Cross3(v: vec3,o: vec3) -> vec3 { return vec3(v.y*o.z-v.z*o.y, v.z*o.x-v.x*o.z, v.x*o.y-v.y*o.x); } +fn Dim3(v: vec3, dim: i32) -> f32 { + if (dim == 0) { + return v.x; + } + if (dim == 1) { + return v.y; + }return v.z; +} +fn SetDim3(v: vec3, dim: i32, val: f32) -> vec3 { + var nv = v; + if (dim == 0) { + nv.x = val; + } + if (dim == 1) { + nv.y = val; + } + if (dim == 3) { + nv.z = val; + }return nv; +} //////// import: "step.go" @@ -419,67 +477,103 @@ fn StepSolveJoints(i: u32) { //gosl:kernel if (ji >= params.JointsN) { return; } - var jpi = JointParentIndex(ji); - var jpbi = i32(-1); - if (jpi >= 0) { - jpbi = DynamicIndex(jpi, params.Cur); - } - var jci = JointChildIndex(ji); - var jcbi = DynamicIndex(jci, params.Cur); var jt = GetJointType(ji); - if (jt == Free) { + if (jt == Free || !GetJointEnabled(ji)) { return; } - var jpP = JointPPos(ji); - var jpQ = JointPRot(ji); - var xwpP = jpP; // world xform, parent, pos - var xwpQ = jpQ; // quat - var mInvp = f32(0.0); - var iInvp = mat3x3f(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0); - var posepP = jpP; - var posepQ = jpQ; - var comp: vec3; + var jPi = JointParentIndex(ji); + var jPbi = i32(-1); + if (jPi >= 0) { + jPbi = DynamicIndex(jPi, params.Cur); + } + var jCi = JointChildIndex(ji); + var jCbi = DynamicIndex(jCi, params.Cur); + var jLinearN = GetJointLinearDoFN(ji); + var jAngularN = GetJointAngularDoFN(ji); + var jPR = JointPPos(ji); + var jPQ = JointPQuat(ji); + var xwPR = jPR; // world xform, parent, pos + var xwPQ = jPQ; // quat + var mInvP = f32(0.0); + var iInvP = mat3x3f(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0); + var posePR = jPR; + var posePQ = jPQ; + var comP: vec3; var - velp: vec3; - var omegap: vec3; - if (jpi >= 0) { - posepP = DynamicPos(jpi, params.Next); // now using next - posepQ = DynamicRot(jpi, params.Next); - MulQPTransforms(posepP, posepQ, jpP, jpQ, &xwpP, &xwpQ); - comp = BodyCom(jpbi); - mInvp = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(jpbi), u32(BodyInvMass))]; - iInvp = BodyInvInertia(jpbi); - velp = DynamicDelta(jpi, params.Next); - omegap = DynamicAngDelta(jpi, params.Next); + vP: vec3; + var wP: vec3; + if (jPi >= 0) { + posePR = DynamicPos(jPi, params.Next); // now using next + posePQ = DynamicQuat(jPi, params.Next); + MulSpatialTransforms(posePR, posePQ, jPR, jPQ, &xwPR, &xwPQ); + comP = BodyCom(jPbi); + mInvP = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(jPbi), u32(BodyInvMass))]; + iInvP = BodyInvInertia(jPbi); + vP = DynamicDelta(jPi, params.Next); + wP = DynamicAngDelta(jPi, params.Next); } - var posecP = DynamicPos(jci, params.Next); - var posecQ = DynamicRot(jci, params.Next); - var jcP = JointCPos(ji); - var jcQ = JointCRot(ji); - var xwcP = jcP; - var xwcQ = jcQ; - MulQPTransforms(posecP, posecQ, jcP, jcQ, &xwcP, &xwcQ); - var comc = BodyCom(jcbi); - var mInvc = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(jcbi), u32(BodyInvMass))]; - var iInvc = BodyInvInertia(jcbi); - var velc = DynamicDelta(jci, params.Next); - var omegac = DynamicAngDelta(jci, params.Next); - if (mInvp == 0.0 && mInvc == 0.0) { // connection between two immovable bodies + var poseCR = DynamicPos(jCi, params.Next); + var poseCQ = DynamicQuat(jCi, params.Next); + var jCR = JointCPos(ji); + var jCQ = JointCQuat(ji); + var xwCR = jCR; + var xwCQ = jCQ; + MulSpatialTransforms(poseCR, poseCQ, jCR, jCQ, &xwCR, &xwCQ); + var comC = BodyCom(jCbi); + var mInvC = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(jCbi), u32(BodyInvMass))]; + var iInvC = BodyInvInertia(jCbi); + var vC = DynamicDelta(jCi, params.Next); + var wC = DynamicAngDelta(jCi, params.Next); + if (mInvP == 0.0 && mInvC == 0.0) { // connection between two immovable bodies return; } var linDeltaP: vec3; var angDeltaP: vec3; var linDeltaC: vec3; var angDeltaC: vec3; - var relPoseP = xwpP; - var relPoseQ = xwpQ; - QPTransformInverse(xwpP, xwpQ, &relPoseP, &relPoseQ); - MulQPTransforms(relPoseP, relPoseQ, xwcP, xwcQ, &relPoseP, &relPoseQ); - var xc = xwcP; - var worldComp = MulQPPoint(posepP, posepQ, comp); - var worldComc = MulQPPoint(posecP, posecQ, comc); - _ = worldComc; + var relPoseR = xwPR; + var relPoseQ = xwPQ; + SpatialTransformInverse(xwPR, xwPQ, &relPoseR, &relPoseQ); + MulSpatialTransforms(relPoseR, relPoseQ, xwCR, xwCQ, &relPoseR, &relPoseQ); + var wComP = MulSpatialPoint(posePR, posePQ, comP); + var wComC = MulSpatialPoint(poseCR, poseCQ, comC); if (jt == Distance) { + var dP = xwPR-(wComP); + var dC = xwCR-(wComC); + var lo = JointDoF(ji, i32(i32(0)), JointLimitLower); // only first one has constraint + var up = JointDoF(ji, i32(i32(0)), JointLimitUpper); + if (lo < 0 && up < 0) { // not limited + return; + } + var d = Length3(relPoseR); + var err = f32(0.0); + if (lo >= 0.0 && d < lo) { + err = d - lo; + relPoseR = Normal3(wComC-(wComP))*(err); + } else if (up >= 0.0 && d > up) { + err = d - up; + } + if (abs(err) > 1e-9) { + var linearC = relPoseR; + var linearP = Negate3(linearC); + dC = xwCR-(wComC); + var angularP = Negate3(Cross3(dP, linearC)); + var angularC = Cross3(dC, linearC); + var derr = Dot3(linearP, vP) + Dot3(linearC, vC) + Dot3(angularP, wP) + Dot3(angularC, wC); + var lambdaIn = f32(0.0); + var compliance = params.JointLinearComply; + var ke = JointDoF(ji, i32(i32(0)), JointStiff); + var kd = JointDoF(ji, i32(i32(0)), JointDamp); + if (ke > 0.0) { + compliance = 1.0 / ke; + } + var dLambda = PositionalCorrection(err, derr, posePQ, poseCQ, mInvP, mInvC, + iInvP, iInvC, linearP, linearC, angularP, angularC, lambdaIn, compliance, kd, params.Dt); + linDeltaP = linDeltaP+(linearP*(dLambda * params.JointLinearRelax)); + angDeltaP = angDeltaP+(angularP*(dLambda * params.JointAngularRelax)); + linDeltaC = linDeltaC+(linearC*(dLambda * params.JointLinearRelax)); + angDeltaC = angDeltaC+(angularC*(dLambda * params.JointAngularRelax)); + } } else { // compute joint target, stiffness, damping var axisLimitsD: vec3; var axisLimitsA: vec3; @@ -487,98 +581,228 @@ fn StepSolveJoints(i: u32) { //gosl:kernel var axisTargetPosKeA: vec3; var axisTargetVelKdD: vec3; var axisTargetVelKdA: vec3; - var axis = JointAxis(ji, i32(i32(0))); - var loTemp = axis*(JointDoF(ji, i32(i32(0)), JointLimitLower)); - var upTemp = axis*(JointDoF(ji, i32(i32(0)), JointLimitUpper)); - axisLimitsD = Min3(loTemp, upTemp); - axisLimitsA = Max3(loTemp, upTemp); - var ke = JointDoF(ji, i32(i32(0)), JointStiff); - var kd = JointDoF(ji, i32(i32(0)), JointDamp); - var targetPos = GetJointTargetPos(ji, i32(i32(0))); - var targetVel = GetJointTargetVel(ji, i32(i32(0))); - if (ke > 0.0) { // has position control - UpdateJointAxisWeightedTarget(axis, targetPos, ke, &axisTargetPosKeD, &axisTargetPosKeA); - } - if (kd > 0.0) { // has velocity control - UpdateJointAxisWeightedTarget(axis, targetVel, kd, &axisTargetVelKdD, &axisTargetVelKdA); + for (var dof=0; dof 0.0) { // has position control + JointAxisTarget(axis, targetPos, ke, &axisTargetPosKeD, &axisTargetPosKeA); + } + if (kd > 0.0) { // has velocity control + JointAxisTarget(axis, targetVel, kd, &axisTargetVelKdD, &axisTargetVelKdA); + } } var axisStiffness = axisTargetPosKeA; var axisDamping = axisTargetVelKdA; - if (axisStiffness.x > 0.0) { // todo: Dim(i) access - axisTargetPosKeD.x /= axisStiffness.x; - } - if (axisDamping.x > 0.0) { // todo Dim - axisTargetVelKdD.x /= axisDamping.x; - } + axisTargetPosKeD = DivSafe3(axisTargetPosKeD, axisStiffness); + axisTargetVelKdD = DivSafe3(axisTargetVelKdD, axisDamping); var axisLimitsLower = axisLimitsD; var axisLimitsUpper = axisLimitsA; - var rp = xc-(worldComp); - var rc = xc-(MulQPPoint(posecP, posecQ, comc)); - { - var e = relPoseP.x; // rel_p[dim] - var linearc = vec3(0, 0, 0); - var linearp = Negate3(linearc); - var angularp = Cross3(rp, linearc); - var angularc = Cross3(rc, linearc); - var derr = Dot3(linearp, velp) + Dot3(linearc, velc) + Dot3(angularp, omegap) + Dot3(angularc, omegac); + var dP = xwCR-(wComP); + var dC = xwCR-(MulSpatialPoint(poseCR, poseCQ, comC)); + for (var dim=0; dim(0, 0, 0), dim, f32(f32(1))); // axis for dim + var linearC = MulQuatVector(xwPQ, dima); + var linearP = Negate3(linearC); + var angularP = Negate3(Cross3(dP, linearC)); + var angularC = Cross3(dC, linearC); + var derr = Dot3(linearP, vP) + Dot3(linearC, vC) + Dot3(angularP, wP) + Dot3(angularC, wC); var err = f32(0.0); var compliance = params.JointLinearComply; var damping = f32(0.0); - var targetVel = axisTargetVelKdD.x; // [dim] + var targetVel = Dim3(axisTargetVelKdD, dim); var derrRel = derr - targetVel; - var lower = axisLimitsLower.x; // [dim] - var upper = axisLimitsUpper.x; // [dim] + var lower = Dim3(axisLimitsLower, dim); + var upper = Dim3(axisLimitsUpper, dim); if (e < lower) { err = e - lower; } else if (e > upper) { err = e - upper; } else { - var targetPos = axisTargetPosKeD.x; // [dim] + var targetPos = Dim3(axisTargetPosKeD, dim); targetPos = clamp(targetPos, lower, upper); - if (axisStiffness.x > 0.0) { + var ke = Dim3(axisStiffness, dim); + var kd = Dim3(axisDamping, dim); + if (ke > 0.0) { err = e - targetPos; - compliance = 1.0 / axisStiffness.x; // [dim] - damping = axisDamping.x; // [dim] - } else if (axisDamping.x > 0.0) { - compliance = 1.0 / axisDamping.x; // [dim] - damping = axisDamping.x; // [dim] + compliance = 1.0 / ke; + damping = Dim3(axisDamping, dim); + } else if (kd > 0.0) { + compliance = 1.0 / kd; + damping = kd; } } if (abs(err) > 1e-9 || abs(derrRel) > 1e-9) { var lambdaIn = f32(0.0); - var dLambda = PositionalCorrection(err, derrRel, posepQ, posecQ, mInvp, mInvc, - iInvp, iInvc, linearp, linearc, angularp, angularc, lambdaIn, compliance, damping, params.Dt); - linDeltaP = linDeltaP+(linearp*(dLambda * params.JointLinearRelax)); - angDeltaP = angDeltaP+(angularp*(dLambda * params.JointAngularRelax)); - linDeltaC = linDeltaC+(linearc*(dLambda * params.JointLinearRelax)); - angDeltaC = angDeltaC+(angularc*(dLambda * params.JointAngularRelax)); + var dLambda = PositionalCorrection(err, derrRel, posePQ, poseCQ, mInvP, mInvC, + iInvP, iInvC, linearP, linearC, angularP, angularC, lambdaIn, compliance, damping, params.Dt); + linDeltaP = linDeltaP+(linearP*(dLambda * params.JointLinearRelax)); + angDeltaP = angDeltaP+(angularP*(dLambda * params.JointAngularRelax)); + linDeltaC = linDeltaC+(linearC*(dLambda * params.JointLinearRelax)); + angDeltaC = angDeltaC+(angularC*(dLambda * params.JointAngularRelax)); + } + } + } + if (jt == Fixed || jt == Prismatic || jt == Revolute || jt == D6) { // angular + var qP = xwPQ; + var qC = xwCQ; + if (QuatDot(qP, qC) < 0) { + qC = QuatMulScalar(qC, -1.0); + } + var relQ = MulQuats(QuatInverse(qP), qC); + var qtwist = QuatNormalize(vec4(relQ.x, 0.0, 0.0, relQ.w)); + var qswing = MulQuats(relQ, QuatInverse(qtwist)); + var s = sqrt(relQ.x*relQ.x + relQ.w*relQ.w); + var invs = 1.0 / s; + var invscube = invs * invs * invs; + var err0 = 2.0 * asin(clamp(qtwist.x, -1.0, 1.0)); + var err1 = qswing.y; + var err2 = qswing.z; + var grad0 = vec4(invs-relQ.x*relQ.x*invscube, 0.0, 0.0, -(relQ.w*relQ.x)*invscube); + var grad1 = vec4( + -relQ.w*(relQ.w*relQ.z+relQ.x*relQ.y)*invscube, + relQ.w*invs, + -relQ.x*invs, + relQ.x*(relQ.w*relQ.z+relQ.x*relQ.y)*invscube); + var grad2 = vec4( + relQ.w*(relQ.w*relQ.y-relQ.x*relQ.z)*invscube, + relQ.x*invs, + relQ.w*invs, + relQ.x*(relQ.z*relQ.x-relQ.w*relQ.y)*invscube); + grad0 = QuatMulScalar(grad0, 2.0/abs(qtwist.w)); + var swing_sq = qswing.w * qswing.w; + var angularEps = f32(1.0e-4); + if (swing_sq+angularEps < 1.0) { + var d = sqrt(1.0 - qswing.w*qswing.w); + var theta = 2.0 * acos(clamp(qswing.w, -1.0, 1.0)); + var scale = theta / d; + err1 *= scale; + err2 *= scale; + grad1 = QuatMulScalar(grad1, scale); + grad2 = QuatMulScalar(grad2, scale); + } + var errs = vec3(err0, err1, err2); + var gradX = vec3(grad0.x, grad1.x, grad2.x); + var gradY = vec3(grad0.y, grad1.y, grad2.y); + var gradZ = vec3(grad0.z, grad1.z, grad2.z); + var gradW = vec3(grad0.w, grad1.w, grad2.w); + var axisLimitsD: vec3; + var axisLimitsA: vec3; + var axisTargetPosKeD: vec3; + var axisTargetPosKeA: vec3; + var axisTargetVelKdD: vec3; + var axisTargetVelKdA: vec3; + for (var dof=0; dof 0.0) { // has position control + JointAxisTarget(axis, targetPos, ke, &axisTargetPosKeD, &axisTargetPosKeA); + } + if (kd > 0.0) { // has velocity control + JointAxisTarget(axis, targetVel, kd, &axisTargetVelKdD, &axisTargetVelKdA); } } + var axisStiffness = axisTargetPosKeA; + var axisDamping = axisTargetVelKdA; + axisTargetPosKeD = DivSafe3(axisTargetPosKeD, axisStiffness); + axisTargetVelKdD = DivSafe3(axisTargetVelKdD, axisDamping); + var axisLimitsLower = axisLimitsD; + var axisLimitsUpper = axisLimitsA; + for (var dim=0; dim(Dim3(gradX, dim), Dim3(gradY, dim), Dim3(gradZ, dim), Dim3(gradW, dim)); + var quatC = MulQuats(MulQuats(QuatMulScalar(qP, f32(0.5)), grad), QuatInverse(qC)); + var angularC = vec3(quatC.x, quatC.y, quatC.z); + var angularP = Negate3(angularC); + var derr = Dot3(angularP, wP) + Dot3(angularC, wC); + var err = f32(0.0); + var compliance = params.JointLinearComply; + var damping = f32(0.0); + var targetVel = Dim3(axisTargetVelKdD, dim); + var derrRel = derr - targetVel; + var lower = Dim3(axisLimitsLower, dim); + var upper = Dim3(axisLimitsUpper, dim); + if (e < lower) { + err = e - lower; + } else if (e > upper) { + err = e - upper; + } else { + var targetPos = Dim3(axisTargetPosKeD, dim); + targetPos = clamp(targetPos, lower, upper); + var ke = Dim3(axisStiffness, dim); + var kd = Dim3(axisDamping, dim); + if (ke > 0.0) { + err = e - targetPos; + compliance = 1.0 / ke; + damping = Dim3(axisDamping, dim); + } else if (kd > 0.0) { + compliance = 1.0 / kd; + damping = kd; + } + } + var lambdaIn = f32(0); + var dLambda = AngularCorrection(err, derrRel, posePQ, poseCQ, iInvP, iInvC, angularP, angularC, lambdaIn, compliance, damping, params.Dt); + angDeltaP = angDeltaP+(angularP*(dLambda)); + angDeltaC = angDeltaC+(angularC*(dLambda)); + } } SetJointPDelta(ji, linDeltaP); SetJointPAngDelta(ji, angDeltaP); SetJointCDelta(ji, linDeltaC); SetJointCAngDelta(ji, angDeltaC); } -fn UpdateJointAxisWeightedTarget(axis: vec3, targ: f32,weight: f32, axisTargets: ptr>,axisWeights: ptr>) { +fn JointAxisTarget(axis: vec3, targ: f32,weight: f32, axisTargets: ptr>,axisWeights: ptr>) { var weightedAxis = axis*(weight); *axisTargets = (*axisTargets)+(weightedAxis*(targ)); // weighted target (to be normalized later by sum of weights) *axisWeights = (*axisWeights)+(Abs3(weightedAxis)); } -fn PositionalCorrection(err: f32,derr: f32, tfaQ: vec4,tfbQ: vec4, mInva: f32,mInvb: f32, Iinva: mat3x3f,Iinvb: mat3x3f, lineara: vec3,linearb: vec3,angulara: vec3,angularb: vec3, lambdaIn: f32,compliance: f32,damping: f32,dt: f32) -> f32 { +fn PositionalCorrection(err: f32,derr: f32, tfaQ: vec4,tfbQ: vec4, mInva: f32,mInvb: f32, iInva: mat3x3f,iInvb: mat3x3f, lineara: vec3,linearb: vec3,angulara: vec3,angularb: vec3, lambdaIn: f32,compliance: f32,damping: f32,dt: f32) -> f32 { var denom = f32(0.0); denom += LengthSquared3(lineara) * mInva; denom += LengthSquared3(linearb) * mInvb; - var q1 = tfaQ; - var q2 = tfbQ; - var rotAngulara = MulQuatVectorInverse(q1, angulara); - var rotAngularb = MulQuatVectorInverse(q2, angularb); - denom += Dot3(rotAngulara, Iinva*(rotAngulara)); - denom += Dot3(rotAngularb, Iinvb*(rotAngularb)); + var rotAngulara = MulQuatVectorInverse(tfaQ, angulara); + var rotAngularb = MulQuatVectorInverse(tfbQ, angularb); + denom += Dot3(rotAngulara, iInva*(rotAngulara)); + denom += Dot3(rotAngularb, iInvb*(rotAngularb)); var alpha = compliance; var gamma = compliance * damping; var deltaLambda = -(err + alpha*lambdaIn + gamma*derr); if (denom+alpha > 0.0) { deltaLambda /= (dt+gamma)*denom + alpha/dt; }return deltaLambda; +} +fn AngularCorrection(err: f32,derr: f32, tfaQ: vec4,tfbQ: vec4, iInva: mat3x3f,iInvb: mat3x3f, angulara: vec3,angularb: vec3, lambdaIn: f32,compliance: f32,damping: f32,dt: f32) -> f32 { + var rotAngulara = MulQuatVectorInverse(tfaQ, angulara); + var rotAngularb = MulQuatVectorInverse(tfbQ, angularb); + var denom = f32(0.0); + denom += Dot3(rotAngulara, iInva*(rotAngulara)); + denom += Dot3(rotAngularb, iInvb*(rotAngularb)); + var alpha = compliance; + var gamma = compliance * damping; + var deltaLambda = -(err + alpha*lambdaIn + gamma*derr); + if (denom+alpha > 0.0) { + deltaLambda /= (dt+gamma)*denom + alpha/dt; + }return deltaLambda; +} +fn JointAxisLimitsUpdate(dof: i32, axis: vec3, lower: f32,upper: f32, axisLimitsD: ptr>,axisLimitsA: ptr>) { + var loTemp = axis*(lower); + var upTemp = axis*(upper); + var lo = Min3(loTemp, upTemp); + var up = Max3(loTemp, upTemp); + if (dof == 0) { + *axisLimitsD = lo; + *axisLimitsA = up; + } else { + *axisLimitsD = Min3(*axisLimitsD, lo); + *axisLimitsA = Max3(*axisLimitsD, up); + } } \ No newline at end of file diff --git a/physics/step_body.go b/physics/step_body.go index 22234fae..8084116a 100644 --- a/physics/step_body.go +++ b/physics/step_body.go @@ -31,10 +31,10 @@ func InitDynamics(i uint32) { //gosl:kernel Dynamics.Set(Bodies.Value(int(bi), int(BodyPosY)), int(ii), int(cni), int(DynPosY)) Dynamics.Set(Bodies.Value(int(bi), int(BodyPosZ)), int(ii), int(cni), int(DynPosZ)) - Dynamics.Set(Bodies.Value(int(bi), int(BodyRotX)), int(ii), int(cni), int(DynRotX)) - Dynamics.Set(Bodies.Value(int(bi), int(BodyRotY)), int(ii), int(cni), int(DynRotY)) - Dynamics.Set(Bodies.Value(int(bi), int(BodyRotZ)), int(ii), int(cni), int(DynRotZ)) - Dynamics.Set(Bodies.Value(int(bi), int(BodyRotW)), int(ii), int(cni), int(DynRotW)) + Dynamics.Set(Bodies.Value(int(bi), int(BodyQuatX)), int(ii), int(cni), int(DynQuatX)) + Dynamics.Set(Bodies.Value(int(bi), int(BodyQuatY)), int(ii), int(cni), int(DynQuatY)) + Dynamics.Set(Bodies.Value(int(bi), int(BodyQuatZ)), int(ii), int(cni), int(DynQuatZ)) + Dynamics.Set(Bodies.Value(int(bi), int(BodyQuatW)), int(ii), int(cni), int(DynQuatW)) for v := DynVelX; v < DynamicVarsN; v++ { Dynamics.Set(0.0, int(ii), int(cni), int(v)) @@ -131,7 +131,7 @@ func StepIntegrateBodies(i uint32) { //gosl:kernel // current pos p0 := DynamicPos(di, params.Cur) - q0 := DynamicRot(di, params.Cur) + q0 := DynamicQuat(di, params.Cur) // current deltas v0 := DynamicDelta(di, params.Cur) @@ -152,7 +152,7 @@ func StepIntegrateBodies(i uint32) { //gosl:kernel tb := slmath.MulQuatVectorInverse(q0, t0).Sub(slmath.Cross3(wb, inertia.MulVector3(wb))) // coriolis forces w1 := slmath.MulQuatVector(q0, wb.Add(invInertia.MulVector3(tb).MulScalar(params.Dt))) - q1 := slmath.MulQuats(math32.NewQuat(w1.X, w1.Y, w1.Z, 0), q0).MulScalar(0.5 * params.Dt) + q1 := slmath.QuatAdd(q0, slmath.MulQuats(math32.NewQuat(w1.X, w1.Y, w1.Z, 0), q0).MulScalar(0.5*params.Dt)) q1 = slmath.QuatNormalize(q1) // angular damping @@ -161,7 +161,7 @@ func StepIntegrateBodies(i uint32) { //gosl:kernel p1a := p1.Sub(slmath.MulQuatVector(q1, com)) // pos corrected to nominal center. SetDynamicPos(di, params.Next, p1a) - SetDynamicRot(di, params.Next, q1) + SetDynamicQuat(di, params.Next, q1) SetDynamicDelta(di, params.Next, v1) SetDynamicAngDelta(di, params.Next, w1) } @@ -184,7 +184,7 @@ func StepBodyDeltas(i uint32) { //gosl:kernel // starting pos (from force integration) p0 := DynamicPos(di, params.Next) - q0 := DynamicRot(di, params.Next) + q0 := DynamicQuat(di, params.Next) // starting deltas v0 := DynamicDelta(di, params.Next) @@ -231,7 +231,7 @@ func StepBodyDeltas(i uint32) { //gosl:kernel } SetDynamicPos(di, params.Next, p1) - SetDynamicRot(di, params.Next, q1) + SetDynamicQuat(di, params.Next, q1) SetDynamicDelta(di, params.Next, v1) SetDynamicAngDelta(di, params.Next, w1) } diff --git a/physics/step_body.goal b/physics/step_body.goal index 57753979..ec6f95b7 100644 --- a/physics/step_body.goal +++ b/physics/step_body.goal @@ -29,10 +29,10 @@ func InitDynamics(i uint32) { //gosl:kernel Dynamics[ii, cni, DynPosY] = Bodies[bi, BodyPosY] Dynamics[ii, cni, DynPosZ] = Bodies[bi, BodyPosZ] - Dynamics[ii, cni, DynRotX] = Bodies[bi, BodyRotX] - Dynamics[ii, cni, DynRotY] = Bodies[bi, BodyRotY] - Dynamics[ii, cni, DynRotZ] = Bodies[bi, BodyRotZ] - Dynamics[ii, cni, DynRotW] = Bodies[bi, BodyRotW] + Dynamics[ii, cni, DynQuatX] = Bodies[bi, BodyQuatX] + Dynamics[ii, cni, DynQuatY] = Bodies[bi, BodyQuatY] + Dynamics[ii, cni, DynQuatZ] = Bodies[bi, BodyQuatZ] + Dynamics[ii, cni, DynQuatW] = Bodies[bi, BodyQuatW] for v := DynVelX; v < DynamicVarsN; v++ { Dynamics[ii, cni, v] = 0.0 @@ -129,7 +129,7 @@ func StepIntegrateBodies(i uint32) { //gosl:kernel // current pos p0 := DynamicPos(di, params.Cur) - q0 := DynamicRot(di, params.Cur) + q0 := DynamicQuat(di, params.Cur) // current deltas v0 := DynamicDelta(di, params.Cur) @@ -150,7 +150,7 @@ func StepIntegrateBodies(i uint32) { //gosl:kernel tb := slmath.MulQuatVectorInverse(q0, t0).Sub(slmath.Cross3(wb, inertia.MulVector3(wb))) // coriolis forces w1 := slmath.MulQuatVector(q0, wb.Add(invInertia.MulVector3(tb).MulScalar(params.Dt))) - q1 := slmath.MulQuats(math32.NewQuat(w1.X, w1.Y, w1.Z, 0), q0).MulScalar(0.5 * params.Dt) + q1 := slmath.QuatAdd(q0, slmath.MulQuats(math32.NewQuat(w1.X, w1.Y, w1.Z, 0), q0).MulScalar(0.5 * params.Dt)) q1 = slmath.QuatNormalize(q1) // angular damping @@ -159,7 +159,7 @@ func StepIntegrateBodies(i uint32) { //gosl:kernel p1a := p1.Sub(slmath.MulQuatVector(q1, com)) // pos corrected to nominal center. SetDynamicPos(di, params.Next, p1a) - SetDynamicRot(di, params.Next, q1) + SetDynamicQuat(di, params.Next, q1) SetDynamicDelta(di, params.Next, v1) SetDynamicAngDelta(di, params.Next, w1) } @@ -182,7 +182,7 @@ func StepBodyDeltas(i uint32) { //gosl:kernel // starting pos (from force integration) p0 := DynamicPos(di, params.Next) - q0 := DynamicRot(di, params.Next) + q0 := DynamicQuat(di, params.Next) // starting deltas v0 := DynamicDelta(di, params.Next) @@ -229,7 +229,7 @@ func StepBodyDeltas(i uint32) { //gosl:kernel } SetDynamicPos(di, params.Next, p1) - SetDynamicRot(di, params.Next, q1) + SetDynamicQuat(di, params.Next, q1) SetDynamicDelta(di, params.Next, v1) SetDynamicAngDelta(di, params.Next, w1) } diff --git a/physics/step_joint.go b/physics/step_joint.go index 4a9a33d0..60737227 100644 --- a/physics/step_joint.go +++ b/physics/step_joint.go @@ -14,6 +14,12 @@ import ( "cogentcore.org/lab/gosl/slmath" ) +// notation convention: +// spatial transform: R = position, Q = quat rotation +// P = parent, C = child +// x = transform, w = world +// d = moment arm + //gosl:start //gosl:import "cogentcore.org/lab/gosl/slmath" @@ -24,62 +30,78 @@ func StepJointForces(i uint32) { //gosl:kernel if ji >= params.JointsN { return } - // ndof := JointDoFN(ji) - // todo: enabled - jpi := JointParentIndex(ji) - jpbi := int32(-1) - if jpi >= 0 { - jpbi = DynamicIndex(jpi, params.Cur) - } - jci := JointChildIndex(ji) - jcbi := DynamicIndex(jci, params.Cur) jt := GetJointType(ji) + if !GetJointEnabled(ji) { + return + } + + jPi := JointParentIndex(ji) + jPbi := int32(-1) + if jPi >= 0 { + jPbi = DynamicIndex(jPi, params.Cur) + } + jCi := JointChildIndex(ji) + jCbi := DynamicIndex(jCi, params.Cur) - jpP := JointPPos(ji) - jpQ := JointPRot(ji) + jLinearN := GetJointLinearDoFN(ji) + jAngularN := GetJointAngularDoFN(ji) + + jPR := JointPPos(ji) + jPQ := JointPQuat(ji) // parent world transform - xwpP := jpP - xwpQ := jpQ - posepP := jpP - posepQ := jpQ - var comp math32.Vector3 - - if jpi >= 0 { // can be fixed - posepP = DynamicPos(jpi, params.Cur) - posepQ = DynamicRot(jpi, params.Cur) - slmath.MulQPTransforms(posepP, posepQ, jpP, jpQ, &xwpP, &xwpQ) - comp = BodyCom(jpbi) + xwPR := jPR + xwPQ := jPQ + posePR := jPR + posePQ := jPQ + comP := math32.Vec3(0, 0, 0) + + if jPi >= 0 { // can be fixed + posePR = DynamicPos(jPi, params.Cur) + posePQ = DynamicQuat(jPi, params.Cur) + slmath.MulSpatialTransforms(posePR, posePQ, jPR, jPQ, &xwPR, &xwPQ) + comP = BodyCom(jPbi) } - rp := xwpP.Sub(slmath.MulQPPoint(posepP, posepQ, comp)) // parent moment arm + dP := xwPR.Sub(slmath.MulSpatialPoint(posePR, posePQ, comP)) // parent moment arm // child world transform - posecP := DynamicPos(jci, params.Cur) - posecQ := DynamicRot(jci, params.Cur) - xwcP := posecP - // xwcQ := posecQ - comc := BodyCom(jcbi) - rc := xwcP.Sub(slmath.MulQPPoint(posecP, posecQ, comc)) // child moment arm + poseCR := DynamicPos(jCi, params.Cur) + poseCQ := DynamicQuat(jCi, params.Cur) + // note: NOT doing this: slmath.MulSpatialTransforms(poseCR, poseCQ, jCR, jCQ, &xwCR, &xwCQ) + // https://github.com/newton-physics/newton/issues/1261 + comC := BodyCom(jCbi) + dC := poseCR.Sub(slmath.MulSpatialPoint(poseCR, poseCQ, comC)) // child moment arm var f, t math32.Vector3 switch jt { case Free, Distance: - f = math32.Vec3(GetJointControlForce(ji, 0), GetJointControlForce(ji, 1), GetJointControlForce(ji, 2)) - t = math32.Vec3(GetJointControlForce(ji, 3), GetJointControlForce(ji, 4), GetJointControlForce(ji, 5)) + f = math32.Vec3(JointControl(ji, 0, JointControlForce), JointControl(ji, 1, JointControlForce), JointControl(ji, 2, JointControlForce)) + t = math32.Vec3(JointControl(ji, 3, JointControlForce), JointControl(ji, 4, JointControlForce), JointControl(ji, 5, JointControlForce)) case Ball: - t = math32.Vec3(GetJointControlForce(ji, 0), GetJointControlForce(ji, 1), GetJointControlForce(ji, 2)) - case Revolute, Prismatic: + // note: assuming the axes are x, y, z + t = math32.Vec3(JointControl(ji, 0, JointControlForce), JointControl(ji, 1, JointControlForce), JointControl(ji, 2, JointControlForce)) + case Revolute: + axis := JointAxis(ji, 0) + t = slmath.MulScalar3(slmath.MulQuatVector(xwPQ, axis), JointControl(ji, 0, JointControlForce)) + case Prismatic: axis := JointAxis(ji, 0) - ap := slmath.MulQuatVector(xwpQ, axis) - f = slmath.MulScalar3(ap, GetJointControlForce(ji, 0)) + f = slmath.MulScalar3(slmath.MulQuatVector(xwPQ, axis), JointControl(ji, 0, JointControlForce)) default: - // todo: D6 requires more iteration! + for dof := range jLinearN { + axis := JointAxis(ji, int32(dof)) + f = f.Add(slmath.MulScalar3(slmath.MulQuatVector(xwPQ, axis), JointControl(ji, int32(dof), JointControlForce))) + } + for dof := range jAngularN { + di := int32(jLinearN) + int32(dof) + axis := JointAxis(ji, di) + t = t.Add(slmath.MulScalar3(slmath.MulQuatVector(xwPQ, axis), JointControl(ji, di, JointControlForce))) + } } - // These are unique to joint: aggregate into dynamics Next in separate step. + // These are unique to joint: aggregate into dynamics Next in [ForcesFromJoints] SetJointPForce(ji, f) SetJointCForce(ji, f) - SetJointPTorque(ji, t.Add(slmath.Cross3(rp, f))) - SetJointCTorque(ji, t.Add(slmath.Cross3(rc, f))) + SetJointPTorque(ji, t.Add(slmath.Cross3(dP, f))) + SetJointCTorque(ji, t.Add(slmath.Cross3(dC, f))) } // StepSolveJoints fixes joints after updating bodies. @@ -89,461 +111,334 @@ func StepSolveJoints(i uint32) { //gosl:kernel if ji >= params.JointsN { return } - - // todo: enabled - jpi := JointParentIndex(ji) - jpbi := int32(-1) - if jpi >= 0 { - jpbi = DynamicIndex(jpi, params.Cur) - } - jci := JointChildIndex(ji) - jcbi := DynamicIndex(jci, params.Cur) jt := GetJointType(ji) - - if jt == Free { + if jt == Free || !GetJointEnabled(ji) { return } + jPi := JointParentIndex(ji) + jPbi := int32(-1) + if jPi >= 0 { + jPbi = DynamicIndex(jPi, params.Cur) + } + jCi := JointChildIndex(ji) + jCbi := DynamicIndex(jCi, params.Cur) + + jLinearN := GetJointLinearDoFN(ji) + jAngularN := GetJointAngularDoFN(ji) - jpP := JointPPos(ji) - jpQ := JointPRot(ji) - xwpP := jpP // world xform, parent, pos - xwpQ := jpQ // quat - mInvp := float32(0.0) - iInvp := math32.Mat3(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0) - posepP := jpP - posepQ := jpQ + jPR := JointPPos(ji) + jPQ := JointPQuat(ji) + xwPR := jPR // world xform, parent, pos + xwPQ := jPQ // quat + mInvP := float32(0.0) + iInvP := math32.Mat3(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0) + posePR := jPR + posePQ := jPQ - var comp, velp, omegap math32.Vector3 + var comP, vP, wP math32.Vector3 // parent transform and moment arm - if jpi >= 0 { - posepP = DynamicPos(jpi, params.Next) // now using next - posepQ = DynamicRot(jpi, params.Next) - slmath.MulQPTransforms(posepP, posepQ, jpP, jpQ, &xwpP, &xwpQ) - comp = BodyCom(jpbi) - mInvp = Bodies.Value(int(jpbi), int(BodyInvMass)) - iInvp = BodyInvInertia(jpbi) - velp = DynamicDelta(jpi, params.Next) - omegap = DynamicAngDelta(jpi, params.Next) + if jPi >= 0 { + posePR = DynamicPos(jPi, params.Next) // now using next + posePQ = DynamicQuat(jPi, params.Next) + slmath.MulSpatialTransforms(posePR, posePQ, jPR, jPQ, &xwPR, &xwPQ) + comP = BodyCom(jPbi) + mInvP = Bodies.Value(int(jPbi), int(BodyInvMass)) + iInvP = BodyInvInertia(jPbi) + vP = DynamicDelta(jPi, params.Next) + wP = DynamicAngDelta(jPi, params.Next) } // child transform and moment arm - posecP := DynamicPos(jci, params.Next) - posecQ := DynamicRot(jci, params.Next) - jcP := JointCPos(ji) - jcQ := JointCRot(ji) - xwcP := jcP - xwcQ := jcQ - slmath.MulQPTransforms(posecP, posecQ, jcP, jcQ, &xwcP, &xwcQ) - comc := BodyCom(jcbi) - mInvc := Bodies.Value(int(jcbi), int(BodyInvMass)) - iInvc := BodyInvInertia(jcbi) - velc := DynamicDelta(jci, params.Next) - omegac := DynamicAngDelta(jci, params.Next) - - if mInvp == 0.0 && mInvc == 0.0 { // connection between two immovable bodies + poseCR := DynamicPos(jCi, params.Next) + poseCQ := DynamicQuat(jCi, params.Next) + jCR := JointCPos(ji) + jCQ := JointCQuat(ji) + xwCR := jCR + xwCQ := jCQ + slmath.MulSpatialTransforms(poseCR, poseCQ, jCR, jCQ, &xwCR, &xwCQ) + comC := BodyCom(jCbi) + mInvC := Bodies.Value(int(jCbi), int(BodyInvMass)) + iInvC := BodyInvInertia(jCbi) + vC := DynamicDelta(jCi, params.Next) + wC := DynamicAngDelta(jCi, params.Next) + + if mInvP == 0.0 && mInvC == 0.0 { // connection between two immovable bodies return } // accumulate constraint deltas var linDeltaP, angDeltaP, linDeltaC, angDeltaC math32.Vector3 - relPoseP := xwpP - relPoseQ := xwpQ - slmath.QPTransformInverse(xwpP, xwpQ, &relPoseP, &relPoseQ) - slmath.MulQPTransforms(relPoseP, relPoseQ, xwcP, xwcQ, &relPoseP, &relPoseQ) - - // joint connection points - xc := xwcP + relPoseR := xwPR + relPoseQ := xwPQ + slmath.SpatialTransformInverse(xwPR, xwPQ, &relPoseR, &relPoseQ) + slmath.MulSpatialTransforms(relPoseR, relPoseQ, xwCR, xwCQ, &relPoseR, &relPoseQ) - // axis_start = joint_qd_start[tid] - // lin_axis_count = joint_dof_dim[tid, 0] - // ang_axis_count = joint_dof_dim[tid, 1] - - worldComp := slmath.MulQPPoint(posepP, posepQ, comp) - worldComc := slmath.MulQPPoint(posecP, posecQ, comc) - _ = worldComc + wComP := slmath.MulSpatialPoint(posePR, posePQ, comP) + wComC := slmath.MulSpatialPoint(poseCR, poseCQ, comC) // handle positional constraints if jt == Distance { - // r_p = wp.transform_get_translation(X_wp) - world_com_p - // r_c = wp.transform_get_translation(X_wc) - world_com_c - // lower = joint_limit_lower[axis_start] - // upper = joint_limit_upper[axis_start] - // if lower < 0.0 and upper < 0.0: - // - // # no limits - // return - // - // d = wp.length(rel_p) - // err = 0.0 - // if lower >= 0.0 and d < lower: - // - // err = d - lower - // # use a more descriptive direction vector for the constraint - // # in case the joint parent and child anchors are very close - // rel_p = err * wp.normalize(world_com_c - world_com_p) - // - // elif upper >= 0.0 and d > upper: - // - // err = d - upper - // - // if wp.abs(err) > 1e-9: - // - // # compute gradients - // linear_c = rel_p - // linear_p = -linear_c - // r_c = x_c - world_com_c - // angular_p = -wp.cross(r_p, linear_c) - // angular_c = wp.cross(r_c, linear_c) - // # constraint time derivative - // derr = ( - // wp.dot(linear_p, vel_p) - // + wp.dot(linear_c, vel_c) - // + wp.dot(angular_p, omega_p) - // + wp.dot(angular_c, omega_c) - // ) - // lambda_in = 0.0 - // compliance = linear_compliance - // ke = joint_target_ke[axis_start] - // if ke > 0.0: - // compliance = 1.0 / ke - // damping = joint_target_kd[axis_start] - // d_lambda = compute_positional_correction( - // err, - // derr, - // pose_p, - // pose_c, - // m_inv_p, - // m_inv_c, - // I_inv_p, - // I_inv_c, - // linear_p, - // linear_c, - // angular_p, - // angular_c, - // lambda_in, - // compliance, - // damping, - // dt, - // ) - // - // linDelta_p += linear_p * (d_lambda * params.JointLinearRelax) - // angDelta_p += angular_p * (d_lambda * angular_relaxation) - // linDelta_c += linear_c * (d_lambda * params.JointLinearRelax) - // angDelta_c += angular_c * (d_lambda * angular_relaxation) + dP := xwPR.Sub(wComP) + dC := xwCR.Sub(wComC) + lo := JointDoF(ji, 0, JointLimitLower) // only first one has constraint + up := JointDoF(ji, 0, JointLimitUpper) + if lo < 0 && up < 0 { // not limited + return + } + d := slmath.Length3(relPoseR) + err := float32(0.0) + if lo >= 0.0 && d < lo { + err = d - lo + // use a more descriptive direction vector for the constraint + // in case the joint parent and child anchors are very close + relPoseR = slmath.Normal3(wComC.Sub(wComP)).MulScalar(err) + } else if up >= 0.0 && d > up { + err = d - up + } + if math32.Abs(err) > 1e-9 { + // compute gradients + linearC := relPoseR + linearP := slmath.Negate3(linearC) + dC = xwCR.Sub(wComC) + angularP := slmath.Negate3(slmath.Cross3(dP, linearC)) + angularC := slmath.Cross3(dC, linearC) + // constraint time derivative + derr := slmath.Dot3(linearP, vP) + slmath.Dot3(linearC, vC) + slmath.Dot3(angularP, wP) + slmath.Dot3(angularC, wC) + lambdaIn := float32(0.0) + compliance := params.JointLinearComply + ke := JointDoF(ji, 0, JointStiff) + kd := JointDoF(ji, 0, JointDamp) + if ke > 0.0 { + compliance = 1.0 / ke + } + dLambda := PositionalCorrection(err, derr, posePQ, poseCQ, mInvP, mInvC, + iInvP, iInvC, linearP, linearC, angularP, angularC, lambdaIn, compliance, kd, params.Dt) + linDeltaP = linDeltaP.Add(linearP.MulScalar(dLambda * params.JointLinearRelax)) + angDeltaP = angDeltaP.Add(angularP.MulScalar(dLambda * params.JointAngularRelax)) + linDeltaC = linDeltaC.Add(linearC.MulScalar(dLambda * params.JointLinearRelax)) + angDeltaC = angDeltaC.Add(angularC.MulScalar(dLambda * params.JointAngularRelax)) + } } else { // compute joint target, stiffness, damping var axisLimitsD, axisLimitsA math32.Vector3 var axisTargetPosKeD, axisTargetPosKeA math32.Vector3 var axisTargetVelKdD, axisTargetVelKdA math32.Vector3 - axis := JointAxis(ji, 0) - loTemp := axis.MulScalar(JointDoF(ji, 0, JointLimitLower)) - upTemp := axis.MulScalar(JointDoF(ji, 0, JointLimitUpper)) - axisLimitsD = slmath.Min3(loTemp, upTemp) - axisLimitsA = slmath.Max3(loTemp, upTemp) - ke := JointDoF(ji, 0, JointStiff) - kd := JointDoF(ji, 0, JointDamp) - targetPos := GetJointTargetPos(ji, 0) - targetVel := GetJointTargetVel(ji, 0) - if ke > 0.0 { // has position control - UpdateJointAxisWeightedTarget(axis, targetPos, ke, &axisTargetPosKeD, &axisTargetPosKeA) - } - if kd > 0.0 { // has velocity control - UpdateJointAxisWeightedTarget(axis, targetVel, kd, &axisTargetVelKdD, &axisTargetVelKdA) + for dof := range jLinearN { + axis := JointAxis(ji, dof) + JointAxisLimitsUpdate(dof, axis, JointDoF(ji, dof, JointLimitLower), JointDoF(ji, dof, JointLimitUpper), &axisLimitsD, &axisLimitsA) + ke := JointDoF(ji, dof, JointStiff) + kd := JointDoF(ji, dof, JointDamp) + targetPos := JointControl(ji, dof, JointTargetPos) + targetVel := JointControl(ji, dof, JointTargetVel) + if ke > 0.0 { // has position control + JointAxisTarget(axis, targetPos, ke, &axisTargetPosKeD, &axisTargetPosKeA) + } + if kd > 0.0 { // has velocity control + JointAxisTarget(axis, targetVel, kd, &axisTargetVelKdD, &axisTargetVelKdA) + } } - // if lin_axis_count > 1: - // axis_idx = axis_start + 1 - // axis = joint_axis[axis_idx] - // lower = joint_limit_lower[axis_idx] - // upper = joint_limit_upper[axis_idx] - // axis_limits = update_joint_axis_limits(axis, lower, upper, axis_limits) - // ke = joint_target_ke[axis_idx] - // kd = joint_target_kd[axis_idx] - // target_pos = joint_target_pos[axis_idx] - // target_vel = joint_target_vel[axis_idx] - // if ke > 0.0: # has position control - // axis_target_pos_ke = update_joint_axis_weighted_target(axis, target_pos, ke, axis_target_pos_ke) - // if kd > 0.0: # has velocity control - // axis_target_vel_kd = update_joint_axis_weighted_target(axis, target_vel, kd, axis_target_vel_kd) - // if lin_axis_count > 2: - // axis_idx = axis_start + 2 - // axis = joint_axis[axis_idx] - // lower = joint_limit_lower[axis_idx] - // upper = joint_limit_upper[axis_idx] - // axis_limits = update_joint_axis_limits(axis, lower, upper, axis_limits) - // ke = joint_target_ke[axis_idx] - // kd = joint_target_kd[axis_idx] - // target_pos = joint_target_pos[axis_idx] - // target_vel = joint_target_vel[axis_idx] - // if ke > 0.0: # has position control - // axis_target_pos_ke = update_joint_axis_weighted_target(axis, target_pos, ke, axis_target_pos_ke) - // if kd > 0.0: # has velocity control - // axis_target_vel_kd = update_joint_axis_weighted_target(axis, target_vel, kd, axis_target_vel_kd) axisStiffness := axisTargetPosKeA axisDamping := axisTargetVelKdA - // for i := range 3 { - if axisStiffness.X > 0.0 { // todo: Dim(i) access - axisTargetPosKeD.X /= axisStiffness.X - } - // } - // for i := range 3 { - if axisDamping.X > 0.0 { // todo Dim - axisTargetVelKdD.X /= axisDamping.X - } - // } + axisTargetPosKeD = slmath.DivSafe3(axisTargetPosKeD, axisStiffness) + axisTargetVelKdD = slmath.DivSafe3(axisTargetVelKdD, axisDamping) axisLimitsLower := axisLimitsD axisLimitsUpper := axisLimitsA - // todo: - // - // frame_p = wp.quat_to_matrix(wp.transform_get_rotation(X_wp)) - // - // note that xc appearing in both is correct: - rp := xc.Sub(worldComp) - rc := xc.Sub(slmath.MulQPPoint(posecP, posecQ, comc)) - - // for loop will be unrolled, so we can modify local variables - // - // for dim := range 3 { - { - e := relPoseP.X // rel_p[dim] + // note that xwCR appearing in both is correct: + dP := xwCR.Sub(wComP) + dC := xwCR.Sub(slmath.MulSpatialPoint(poseCR, poseCQ, comC)) + for dim := range int32(3) { + e := slmath.Dim3(relPoseR, dim) // compute gradients - // linearc := math32.Vec3(frame_p[0, dim], frame_p[1, dim], frame_p[2, dim]) // todo - linearc := math32.Vec3(0, 0, 0) - linearp := slmath.Negate3(linearc) - angularp := slmath.Cross3(rp, linearc) - angularc := slmath.Cross3(rc, linearc) + // matrix indexing is [row, col] here: dim = col + // quat_to_matrix cols are q rotations of axis vectors + dima := slmath.SetDim3(math32.Vec3(0, 0, 0), dim, 1) // axis for dim + linearC := slmath.MulQuatVector(xwPQ, dima) + linearP := slmath.Negate3(linearC) + angularP := slmath.Negate3(slmath.Cross3(dP, linearC)) + angularC := slmath.Cross3(dC, linearC) // constraint time derivative - derr := slmath.Dot3(linearp, velp) + slmath.Dot3(linearc, velc) + slmath.Dot3(angularp, omegap) + slmath.Dot3(angularc, omegac) + derr := slmath.Dot3(linearP, vP) + slmath.Dot3(linearC, vC) + slmath.Dot3(angularP, wP) + slmath.Dot3(angularC, wC) err := float32(0.0) compliance := params.JointLinearComply damping := float32(0.0) - targetVel := axisTargetVelKdD.X // [dim] + targetVel := slmath.Dim3(axisTargetVelKdD, dim) derrRel := derr - targetVel // consider joint limits irrespective of axis mode - lower := axisLimitsLower.X // [dim] - upper := axisLimitsUpper.X // [dim] + lower := slmath.Dim3(axisLimitsLower, dim) + upper := slmath.Dim3(axisLimitsUpper, dim) if e < lower { err = e - lower } else if e > upper { err = e - upper } else { - targetPos := axisTargetPosKeD.X // [dim] + targetPos := slmath.Dim3(axisTargetPosKeD, dim) targetPos = math32.Clamp(targetPos, lower, upper) - if axisStiffness.X > 0.0 { + ke := slmath.Dim3(axisStiffness, dim) + kd := slmath.Dim3(axisDamping, dim) + if ke > 0.0 { err = e - targetPos - compliance = 1.0 / axisStiffness.X // [dim] - damping = axisDamping.X // [dim] - } else if axisDamping.X > 0.0 { - compliance = 1.0 / axisDamping.X // [dim] - damping = axisDamping.X // [dim] + compliance = 1.0 / ke + damping = slmath.Dim3(axisDamping, dim) + } else if kd > 0.0 { + compliance = 1.0 / kd + damping = kd } } if math32.Abs(err) > 1e-9 || math32.Abs(derrRel) > 1e-9 { lambdaIn := float32(0.0) - dLambda := PositionalCorrection(err, derrRel, posepQ, posecQ, mInvp, mInvc, - iInvp, iInvc, linearp, linearc, angularp, angularc, lambdaIn, compliance, damping, params.Dt) + dLambda := PositionalCorrection(err, derrRel, posePQ, poseCQ, mInvP, mInvC, + iInvP, iInvC, linearP, linearC, angularP, angularC, lambdaIn, compliance, damping, params.Dt) + + linDeltaP = linDeltaP.Add(linearP.MulScalar(dLambda * params.JointLinearRelax)) + angDeltaP = angDeltaP.Add(angularP.MulScalar(dLambda * params.JointAngularRelax)) + linDeltaC = linDeltaC.Add(linearC.MulScalar(dLambda * params.JointLinearRelax)) + angDeltaC = angDeltaC.Add(angularC.MulScalar(dLambda * params.JointAngularRelax)) + } + } + } + // todo: does it make sense to have prismatic, fixed here? + if jt == Fixed || jt == Prismatic || jt == Revolute || jt == D6 { // angular + qP := xwPQ + qC := xwCQ + // make quats lie in same hemisphere + if slmath.QuatDot(qP, qC) < 0 { + qC = slmath.QuatMulScalar(qC, -1.0) + } + relQ := slmath.MulQuats(slmath.QuatInverse(qP), qC) + qtwist := slmath.QuatNormalize(math32.NewQuat(relQ.X, 0.0, 0.0, relQ.W)) + qswing := slmath.MulQuats(relQ, slmath.QuatInverse(qtwist)) + + // decompose to a compound rotation each axis + s := math32.Sqrt(relQ.X*relQ.X + relQ.W*relQ.W) + invs := 1.0 / s + invscube := invs * invs * invs + + // handle axis-angle joints + // rescale twist from quaternion space to angular + err0 := 2.0 * math32.Asin(math32.Clamp(qtwist.X, -1.0, 1.0)) + err1 := qswing.Y + err2 := qswing.Z + // analytic gradients of swing-twist decomposition + grad0 := math32.NewQuat(invs-relQ.X*relQ.X*invscube, 0.0, 0.0, -(relQ.W*relQ.X)*invscube) + grad1 := math32.NewQuat( + -relQ.W*(relQ.W*relQ.Z+relQ.X*relQ.Y)*invscube, + relQ.W*invs, + -relQ.X*invs, + relQ.X*(relQ.W*relQ.Z+relQ.X*relQ.Y)*invscube) + grad2 := math32.NewQuat( + relQ.W*(relQ.W*relQ.Y-relQ.X*relQ.Z)*invscube, + relQ.X*invs, + relQ.W*invs, + relQ.X*(relQ.Z*relQ.X-relQ.W*relQ.Y)*invscube) + grad0 = slmath.QuatMulScalar(grad0, 2.0/math32.Abs(qtwist.W)) + // # grad0 *= 2.0 / wp.sqrt(1.0-qtwist[0]*qtwist[0]) # derivative of asin(x) = 1/sqrt(1-x^2) + + // rescale swing + swing_sq := qswing.W * qswing.W + // if swing axis magnitude close to zero vector, just treat in quaternion space + angularEps := float32(1.0e-4) + if swing_sq+angularEps < 1.0 { + d := math32.Sqrt(1.0 - qswing.W*qswing.W) + theta := 2.0 * math32.Acos(math32.Clamp(qswing.W, -1.0, 1.0)) + scale := theta / d + err1 *= scale + err2 *= scale + grad1 = slmath.QuatMulScalar(grad1, scale) + grad2 = slmath.QuatMulScalar(grad2, scale) + } + errs := math32.Vec3(err0, err1, err2) + gradX := math32.Vec3(grad0.X, grad1.X, grad2.X) + gradY := math32.Vec3(grad0.Y, grad1.Y, grad2.Y) + gradZ := math32.Vec3(grad0.Z, grad1.Z, grad2.Z) + gradW := math32.Vec3(grad0.W, grad1.W, grad2.W) + + // compute joint target, stiffness, damping + var axisLimitsD, axisLimitsA math32.Vector3 + var axisTargetPosKeD, axisTargetPosKeA math32.Vector3 + var axisTargetVelKdD, axisTargetVelKdA math32.Vector3 + + for dof := range jAngularN { + di := dof + jLinearN + axis := JointAxis(ji, di) + JointAxisLimitsUpdate(dof, axis, JointDoF(ji, di, JointLimitLower), JointDoF(ji, di, JointLimitUpper), &axisLimitsD, &axisLimitsA) + ke := JointDoF(ji, di, JointStiff) + kd := JointDoF(ji, di, JointDamp) + targetPos := JointControl(ji, di, JointTargetPos) + targetVel := JointControl(ji, di, JointTargetVel) + if ke > 0.0 { // has position control + JointAxisTarget(axis, targetPos, ke, &axisTargetPosKeD, &axisTargetPosKeA) + } + if kd > 0.0 { // has velocity control + JointAxisTarget(axis, targetVel, kd, &axisTargetVelKdD, &axisTargetVelKdA) + } + } + + axisStiffness := axisTargetPosKeA + axisDamping := axisTargetVelKdA + axisTargetPosKeD = slmath.DivSafe3(axisTargetPosKeD, axisStiffness) + axisTargetVelKdD = slmath.DivSafe3(axisTargetVelKdD, axisDamping) + axisLimitsLower := axisLimitsD + axisLimitsUpper := axisLimitsA + + for dim := range int32(3) { + e := slmath.Dim3(errs, dim) + + // analytic gradients of swing-twist decomposition + grad := math32.NewQuat(slmath.Dim3(gradX, dim), slmath.Dim3(gradY, dim), slmath.Dim3(gradZ, dim), slmath.Dim3(gradW, dim)) + // todo: verify -- does the 0.5 go inside?? + // quat_c = 0.5 * q_p * grad * wp.quat_inverse(q_c) + quatC := slmath.MulQuats(slmath.MulQuats(slmath.QuatMulScalar(qP, 0.5), grad), slmath.QuatInverse(qC)) + + angularC := math32.Vec3(quatC.X, quatC.Y, quatC.Z) + angularP := slmath.Negate3(angularC) + // constraint time derivative + derr := slmath.Dot3(angularP, wP) + slmath.Dot3(angularC, wC) + + err := float32(0.0) + compliance := params.JointLinearComply + damping := float32(0.0) + + targetVel := slmath.Dim3(axisTargetVelKdD, dim) + derrRel := derr - targetVel + + // consider joint limits irrespective of axis mode + lower := slmath.Dim3(axisLimitsLower, dim) + upper := slmath.Dim3(axisLimitsUpper, dim) + if e < lower { + err = e - lower + } else if e > upper { + err = e - upper + } else { + targetPos := slmath.Dim3(axisTargetPosKeD, dim) + targetPos = math32.Clamp(targetPos, lower, upper) - linDeltaP = linDeltaP.Add(linearp.MulScalar(dLambda * params.JointLinearRelax)) - angDeltaP = angDeltaP.Add(angularp.MulScalar(dLambda * params.JointAngularRelax)) - linDeltaC = linDeltaC.Add(linearc.MulScalar(dLambda * params.JointLinearRelax)) - angDeltaC = angDeltaC.Add(angularc.MulScalar(dLambda * params.JointAngularRelax)) + ke := slmath.Dim3(axisStiffness, dim) + kd := slmath.Dim3(axisDamping, dim) + if ke > 0.0 { + err = e - targetPos + compliance = 1.0 / ke + damping = slmath.Dim3(axisDamping, dim) + } else if kd > 0.0 { + compliance = 1.0 / kd + damping = kd + } } + lambdaIn := float32(0) + dLambda := AngularCorrection(err, derrRel, posePQ, poseCQ, iInvP, iInvC, angularP, angularC, lambdaIn, compliance, damping, params.Dt) + + // note: no relaxation factors here: + angDeltaP = angDeltaP.Add(angularP.MulScalar(dLambda)) + angDeltaC = angDeltaC.Add(angularC.MulScalar(dLambda)) } } - // - // if type == JointType.FIXED or type == JointType.PRISMATIC or type == JointType.REVOLUTE or type == JointType.D6: - // # handle angular constraints - // - // # local joint rotations - // q_p = wp.transform_get_rotation(X_wp) - // q_c = wp.transform_get_rotation(X_wc) - // - // # make quats lie in same hemisphere - // if slmath.Dot3(q_p, q_c) < 0.0: - // q_c *= -1.0 - // - // rel_q = wp.quat_inverse(q_p) * q_c - // - // qtwist = wp.normalize(wp.quat(rel_q[0], 0.0, 0.0, rel_q[3])) - // qswing = rel_q * wp.quat_inverse(qtwist) - // - // # decompose to a compound rotation each axis - // s = wp.sqrt(rel_q[0] * rel_q[0] + rel_q[3] * rel_q[3]) - // invs = 1.0 / s - // invscube = invs * invs * invs - // - // # handle axis-angle joints - // - // # rescale twist from quaternion space to angular - // err_0 = 2.0 * wp.asin(wp.clamp(qtwist[0], -1.0, 1.0)) - // err_1 = qswing[1] - // err_2 = qswing[2] - // # analytic gradients of swing-twist decomposition - // grad_0 = wp.quat(invs - rel_q[0] * rel_q[0] * invscube, 0.0, 0.0, -(rel_q[3] * rel_q[0]) * invscube) - // grad_1 = wp.quat( - // -rel_q[3] * (rel_q[3] * rel_q[2] + rel_q[0] * rel_q[1]) * invscube, - // rel_q[3] * invs, - // -rel_q[0] * invs, - // rel_q[0] * (rel_q[3] * rel_q[2] + rel_q[0] * rel_q[1]) * invscube, - // ) - // grad_2 = wp.quat( - // rel_q[3] * (rel_q[3] * rel_q[1] - rel_q[0] * rel_q[2]) * invscube, - // rel_q[0] * invs, - // rel_q[3] * invs, - // rel_q[0] * (rel_q[2] * rel_q[0] - rel_q[3] * rel_q[1]) * invscube, - // ) - // grad_0 *= 2.0 / wp.abs(qtwist[3]) - // # grad_0 *= 2.0 / wp.sqrt(1.0-qtwist[0]*qtwist[0]) # derivative of asin(x) = 1/sqrt(1-x^2) - // - // # rescale swing - // swing_sq = qswing[3] * qswing[3] - // # if swing axis magnitude close to zero vector, just treat in quaternion space - // angularEps = 1.0e-4 - // if swing_sq + angularEps < 1.0: - // d = wp.sqrt(1.0 - qswing[3] * qswing[3]) - // theta = 2.0 * wp.acos(wp.clamp(qswing[3], -1.0, 1.0)) - // scale = theta / d - // - // err_1 *= scale - // err_2 *= scale - // - // grad_1 *= scale - // grad_2 *= scale - // - // errs = wp.vec3(err_0, err_1, err_2) - // grad_x = wp.vec3(grad_0[0], grad_1[0], grad_2[0]) - // grad_y = wp.vec3(grad_0[1], grad_1[1], grad_2[1]) - // grad_z = wp.vec3(grad_0[2], grad_1[2], grad_2[2]) - // grad_w = wp.vec3(grad_0[3], grad_1[3], grad_2[3]) - // - // # compute joint target, stiffness, damping - // axis_limits = wp.spatial_vector(0.0, 0.0, 0.0, 0.0, 0.0, 0.0) - // - // axis_target_pos_ke = wp.spatial_vector() # [weighted_target_pos, ke_weights] - // axis_target_vel_kd = wp.spatial_vector() # [weighted_target_vel, kd_weights] - // # avoid a for loop here since local variables would need to be modified which is not yet differentiable - // if ang_axis_count > 0: - // axis_idx = axis_start + lin_axis_count - // axis = joint_axis[axis_idx] - // lo_temp = axis * joint_limit_lower[axis_idx] - // up_temp = axis * joint_limit_upper[axis_idx] - // axis_limits = wp.spatial_vector(vec_min(lo_temp, up_temp), vec_max(lo_temp, up_temp)) - // ke = joint_target_ke[axis_idx] - // kd = joint_target_kd[axis_idx] - // target_pos = joint_target_pos[axis_idx] - // target_vel = joint_target_vel[axis_idx] - // if ke > 0.0: # has position control - // axis_target_pos_ke = update_joint_axis_weighted_target(axis, target_pos, ke, axis_target_pos_ke) - // if kd > 0.0: # has velocity control - // axis_target_vel_kd = update_joint_axis_weighted_target(axis, target_vel, kd, axis_target_vel_kd) - - // if ang_axis_count > 1: - // axis_idx = axis_start + lin_axis_count + 1 - // axis = joint_axis[axis_idx] - // lower = joint_limit_lower[axis_idx] - // upper = joint_limit_upper[axis_idx] - // axis_limits = update_joint_axis_limits(axis, lower, upper, axis_limits) - // ke = joint_target_ke[axis_idx] - // kd = joint_target_kd[axis_idx] - // target_pos = joint_target_pos[axis_idx] - // target_vel = joint_target_vel[axis_idx] - // if ke > 0.0: # has position control - // axis_target_pos_ke = update_joint_axis_weighted_target(axis, target_pos, ke, axis_target_pos_ke) - // if kd > 0.0: # has velocity control - // axis_target_vel_kd = update_joint_axis_weighted_target(axis, target_vel, kd, axis_target_vel_kd) - // if ang_axis_count > 2: - // axis_idx = axis_start + lin_axis_count + 2 - // axis = joint_axis[axis_idx] - // lower = joint_limit_lower[axis_idx] - // upper = joint_limit_upper[axis_idx] - // axis_limits = update_joint_axis_limits(axis, lower, upper, axis_limits) - // ke = joint_target_ke[axis_idx] - // kd = joint_target_kd[axis_idx] - // target_pos = joint_target_pos[axis_idx] - // target_vel = joint_target_vel[axis_idx] - // if ke > 0.0: # has position control - // axis_target_pos_ke = update_joint_axis_weighted_target(axis, target_pos, ke, axis_target_pos_ke) - // if kd > 0.0: # has velocity control - // axis_target_vel_kd = update_joint_axis_weighted_target(axis, target_vel, kd, axis_target_vel_kd) - // - - // axis_target_pos = wp.spatial_top(axis_target_pos_ke) - // axis_stiffness = wp.spatial_bottom(axis_target_pos_ke) - // axis_target_vel = wp.spatial_top(axis_target_vel_kd) - // axis_damping = wp.spatial_bottom(axis_target_vel_kd) - // for i in range(3): - // if axis_stiffness[i] > 0.0: - // axis_target_pos[i] /= axis_stiffness[i] - // for i in range(3): - // if axis_damping[i] > 0.0: - // axis_target_vel[i] /= axis_damping[i] - // axis_limits_lower = wp.spatial_top(axis_limits) - // axis_limits_upper = wp.spatial_bottom(axis_limits) - - // # if type == JointType.D6: - // # wp.printf("axis_target: %f %f %f\t axis_stiffness: %f %f %f\t axis_damping: %f %f %f\t axis_limits_lower: %f %f %f \t axis_limits_upper: %f %f %f\n", - // # axis_target[0], axis_target[1], axis_target[2], - // # axis_stiffness[0], axis_stiffness[1], axis_stiffness[2], - // # axis_damping[0], axis_damping[1], axis_damping[2], - // # axis_limits_lower[0], axis_limits_lower[1], axis_limits_lower[2], - // # axis_limits_upper[0], axis_limits_upper[1], axis_limits_upper[2]) - // # # wp.printf("wp.sqrt(1.0-qtwist[0]*qtwist[0]) = %f\n", wp.sqrt(1.0-qtwist[0]*qtwist[0])) - - // for dim in range(3): - // e = errs[dim] - // - // # analytic gradients of swing-twist decomposition - // grad = wp.quat(grad_x[dim], grad_y[dim], grad_z[dim], grad_w[dim]) - // - // quat_c = 0.5 * q_p * grad * wp.quat_inverse(q_c) - // angular_c = wp.vec3(quat_c[0], quat_c[1], quat_c[2]) - // angular_p = -angular_c - // # time derivative of the constraint - // derr = slmath.Dot3(angular_p, omega_p) + slmath.Dot3(angular_c, omega_c) - // - // err = 0.0 - // compliance = angular_compliance - // damping = 0.0 - // - // target_vel = axis_target_vel[dim] - // derr_rel = derr - target_vel - // - // # consider joint limits irrespective of mode - // lower = axis_limits_lower[dim] - // upper = axis_limits_upper[dim] - // if e < lower: - // err = e - lower - // elif e > upper: - // err = e - upper - // else: - // target_pos = axis_target_pos[dim] - // target_pos = wp.clamp(target_pos, lower, upper) - // - // if axis_stiffness[dim] > 0.0: - // err = e - target_pos - // compliance = 1.0 / axis_stiffness[dim] - // damping = axis_damping[dim] - // elif axis_damping[dim] > 0.0: - // damping = axis_damping[dim] - // compliance = 1.0 / axis_damping[dim] - // - // d_lambda = ( - // compute_angular_correction( - // err, derr_rel, pose_p, pose_c, I_inv_p, I_inv_c, angular_p, angular_c, 0.0, compliance, damping, dt - // ) - // * angular_relaxation - // ) - // - // # update deltas - // angDelta_p += angular_p * d_lambda - // angDelta_c += angular_c * d_lambda // These are unique to joint: aggregate into dynamics Next in separate step. SetJointPDelta(ji, linDeltaP) @@ -552,26 +447,43 @@ func StepSolveJoints(i uint32) { //gosl:kernel SetJointCAngDelta(ji, angDeltaC) } -func UpdateJointAxisWeightedTarget(axis math32.Vector3, targ, weight float32, axisTargets, axisWeights *math32.Vector3) { +func JointAxisTarget(axis math32.Vector3, targ, weight float32, axisTargets, axisWeights *math32.Vector3) { weightedAxis := axis.MulScalar(weight) *axisTargets = (*axisTargets).Add(weightedAxis.MulScalar(targ)) // weighted target (to be normalized later by sum of weights) *axisWeights = (*axisWeights).Add(slmath.Abs3(weightedAxis)) } -func PositionalCorrection(err, derr float32, tfaQ, tfbQ math32.Quat, mInva, mInvb float32, Iinva, Iinvb math32.Matrix3, lineara, linearb, angulara, angularb math32.Vector3, lambdaIn, compliance, damping, dt float32) float32 { +func PositionalCorrection(err, derr float32, tfaQ, tfbQ math32.Quat, mInva, mInvb float32, iInva, iInvb math32.Matrix3, lineara, linearb, angulara, angularb math32.Vector3, lambdaIn, compliance, damping, dt float32) float32 { denom := float32(0.0) denom += slmath.LengthSquared3(lineara) * mInva denom += slmath.LengthSquared3(linearb) * mInvb - q1 := tfaQ - q2 := tfbQ + // # Eq. 2-3 (make sure to project into the frame of the body) + rotAngulara := slmath.MulQuatVectorInverse(tfaQ, angulara) + rotAngularb := slmath.MulQuatVectorInverse(tfbQ, angularb) + + denom += slmath.Dot3(rotAngulara, iInva.MulVector3(rotAngulara)) + denom += slmath.Dot3(rotAngularb, iInvb.MulVector3(rotAngularb)) + + alpha := compliance + gamma := compliance * damping + + deltaLambda := -(err + alpha*lambdaIn + gamma*derr) + if denom+alpha > 0.0 { + deltaLambda /= (dt+gamma)*denom + alpha/dt + } + return deltaLambda +} + +func AngularCorrection(err, derr float32, tfaQ, tfbQ math32.Quat, iInva, iInvb math32.Matrix3, angulara, angularb math32.Vector3, lambdaIn, compliance, damping, dt float32) float32 { // # Eq. 2-3 (make sure to project into the frame of the body) - rotAngulara := slmath.MulQuatVectorInverse(q1, angulara) - rotAngularb := slmath.MulQuatVectorInverse(q2, angularb) + rotAngulara := slmath.MulQuatVectorInverse(tfaQ, angulara) + rotAngularb := slmath.MulQuatVectorInverse(tfbQ, angularb) - denom += slmath.Dot3(rotAngulara, Iinva.MulVector3(rotAngulara)) - denom += slmath.Dot3(rotAngularb, Iinvb.MulVector3(rotAngularb)) + denom := float32(0.0) + denom += slmath.Dot3(rotAngulara, iInva.MulVector3(rotAngulara)) + denom += slmath.Dot3(rotAngularb, iInvb.MulVector3(rotAngularb)) alpha := compliance gamma := compliance * damping @@ -584,4 +496,20 @@ func PositionalCorrection(err, derr float32, tfaQ, tfbQ math32.Quat, mInva, mInv return deltaLambda } +// update the 3D linear/angular limits (spatial_vector [lower, upper]) +// given the axis vector and limits +func JointAxisLimitsUpdate(dof int32, axis math32.Vector3, lower, upper float32, axisLimitsD, axisLimitsA *math32.Vector3) { + loTemp := axis.MulScalar(lower) + upTemp := axis.MulScalar(upper) + lo := slmath.Min3(loTemp, upTemp) + up := slmath.Max3(loTemp, upTemp) + if dof == 0 { + *axisLimitsD = lo + *axisLimitsA = up + } else { + *axisLimitsD = slmath.Min3(*axisLimitsD, lo) + *axisLimitsA = slmath.Max3(*axisLimitsD, up) + } +} + //gosl:end diff --git a/physics/step_joint.goal b/physics/step_joint.goal index 8a11b3f2..547cf47c 100644 --- a/physics/step_joint.goal +++ b/physics/step_joint.goal @@ -12,6 +12,12 @@ import ( "cogentcore.org/lab/gosl/slmath" ) +// notation convention: +// spatial transform: R = position, Q = quat rotation +// P = parent, C = child +// x = transform, w = world +// d = moment arm + //gosl:start //gosl:import "cogentcore.org/lab/gosl/slmath" @@ -22,62 +28,78 @@ func StepJointForces(i uint32) { //gosl:kernel if ji >= params.JointsN { return } - // ndof := JointDoFN(ji) - // todo: enabled - jpi := JointParentIndex(ji) - jpbi := int32(-1) - if jpi >= 0 { - jpbi = DynamicIndex(jpi, params.Cur) - } - jci := JointChildIndex(ji) - jcbi := DynamicIndex(jci, params.Cur) jt := GetJointType(ji) + if !GetJointEnabled(ji) { + return + } + + jPi := JointParentIndex(ji) + jPbi := int32(-1) + if jPi >= 0 { + jPbi = DynamicIndex(jPi, params.Cur) + } + jCi := JointChildIndex(ji) + jCbi := DynamicIndex(jCi, params.Cur) - jpP := JointPPos(ji) - jpQ := JointPRot(ji) + jLinearN := GetJointLinearDoFN(ji) + jAngularN := GetJointAngularDoFN(ji) + + jPR := JointPPos(ji) + jPQ := JointPQuat(ji) // parent world transform - xwpP := jpP - xwpQ := jpQ - posepP := jpP - posepQ := jpQ - var comp math32.Vector3 - - if jpi >= 0 { // can be fixed - posepP = DynamicPos(jpi, params.Cur) - posepQ = DynamicRot(jpi, params.Cur) - slmath.MulQPTransforms(posepP, posepQ, jpP, jpQ, &xwpP, &xwpQ) - comp = BodyCom(jpbi) + xwPR := jPR + xwPQ := jPQ + posePR := jPR + posePQ := jPQ + comP := math32.Vec3(0, 0, 0) + + if jPi >= 0 { // can be fixed + posePR = DynamicPos(jPi, params.Cur) + posePQ = DynamicQuat(jPi, params.Cur) + slmath.MulSpatialTransforms(posePR, posePQ, jPR, jPQ, &xwPR, &xwPQ) + comP = BodyCom(jPbi) } - rp := xwpP.Sub(slmath.MulQPPoint(posepP, posepQ, comp)) // parent moment arm + dP := xwPR.Sub(slmath.MulSpatialPoint(posePR, posePQ, comP)) // parent moment arm // child world transform - posecP := DynamicPos(jci, params.Cur) - posecQ := DynamicRot(jci, params.Cur) - xwcP := posecP - // xwcQ := posecQ - comc := BodyCom(jcbi) - rc := xwcP.Sub(slmath.MulQPPoint(posecP, posecQ, comc)) // child moment arm + poseCR := DynamicPos(jCi, params.Cur) + poseCQ := DynamicQuat(jCi, params.Cur) + // note: NOT doing this: slmath.MulSpatialTransforms(poseCR, poseCQ, jCR, jCQ, &xwCR, &xwCQ) + // https://github.com/newton-physics/newton/issues/1261 + comC := BodyCom(jCbi) + dC := poseCR.Sub(slmath.MulSpatialPoint(poseCR, poseCQ, comC)) // child moment arm var f, t math32.Vector3 switch jt { case Free, Distance: - f = math32.Vec3(GetJointControlForce(ji, 0), GetJointControlForce(ji, 1), GetJointControlForce(ji, 2)) - t = math32.Vec3(GetJointControlForce(ji, 3), GetJointControlForce(ji, 4), GetJointControlForce(ji, 5)) + f = math32.Vec3(JointControl(ji, 0, JointControlForce), JointControl(ji, 1, JointControlForce), JointControl(ji, 2, JointControlForce)) + t = math32.Vec3(JointControl(ji, 3, JointControlForce), JointControl(ji, 4, JointControlForce), JointControl(ji, 5, JointControlForce)) case Ball: - t = math32.Vec3(GetJointControlForce(ji, 0), GetJointControlForce(ji, 1), GetJointControlForce(ji, 2)) - case Revolute, Prismatic: + // note: assuming the axes are x, y, z + t = math32.Vec3(JointControl(ji, 0, JointControlForce), JointControl(ji, 1, JointControlForce), JointControl(ji, 2, JointControlForce)) + case Revolute: + axis := JointAxis(ji, 0) + t = slmath.MulScalar3(slmath.MulQuatVector(xwPQ, axis), JointControl(ji, 0, JointControlForce)) + case Prismatic: axis := JointAxis(ji, 0) - ap := slmath.MulQuatVector(xwpQ, axis) - f = slmath.MulScalar3(ap, GetJointControlForce(ji, 0)) + f = slmath.MulScalar3(slmath.MulQuatVector(xwPQ, axis), JointControl(ji, 0, JointControlForce)) default: - // todo: D6 requires more iteration! + for dof := range jLinearN { + axis := JointAxis(ji, int32(dof)) + f = f.Add(slmath.MulScalar3(slmath.MulQuatVector(xwPQ, axis), JointControl(ji, int32(dof), JointControlForce))) + } + for dof := range jAngularN { + di := int32(jLinearN) + int32(dof) + axis := JointAxis(ji, di) + t = t.Add(slmath.MulScalar3(slmath.MulQuatVector(xwPQ, axis), JointControl(ji, di, JointControlForce))) + } } - // These are unique to joint: aggregate into dynamics Next in separate step. + // These are unique to joint: aggregate into dynamics Next in [ForcesFromJoints] SetJointPForce(ji, f) SetJointCForce(ji, f) - SetJointPTorque(ji, t.Add(slmath.Cross3(rp, f))) - SetJointCTorque(ji, t.Add(slmath.Cross3(rc, f))) + SetJointPTorque(ji, t.Add(slmath.Cross3(dP, f))) + SetJointCTorque(ji, t.Add(slmath.Cross3(dC, f))) } // StepSolveJoints fixes joints after updating bodies. @@ -87,453 +109,334 @@ func StepSolveJoints(i uint32) { //gosl:kernel if ji >= params.JointsN { return } - - // todo: enabled - jpi := JointParentIndex(ji) - jpbi := int32(-1) - if jpi >= 0 { - jpbi = DynamicIndex(jpi, params.Cur) - } - jci := JointChildIndex(ji) - jcbi := DynamicIndex(jci, params.Cur) jt := GetJointType(ji) - - if jt == Free { + if jt == Free || !GetJointEnabled(ji) { return } + jPi := JointParentIndex(ji) + jPbi := int32(-1) + if jPi >= 0 { + jPbi = DynamicIndex(jPi, params.Cur) + } + jCi := JointChildIndex(ji) + jCbi := DynamicIndex(jCi, params.Cur) + + jLinearN := GetJointLinearDoFN(ji) + jAngularN := GetJointAngularDoFN(ji) - jpP := JointPPos(ji) - jpQ := JointPRot(ji) - xwpP := jpP // world xform, parent, pos - xwpQ := jpQ // quat - mInvp := float32(0.0) - iInvp := math32.Mat3(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0) - posepP := jpP - posepQ := jpQ + jPR := JointPPos(ji) + jPQ := JointPQuat(ji) + xwPR := jPR // world xform, parent, pos + xwPQ := jPQ // quat + mInvP := float32(0.0) + iInvP := math32.Mat3(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0) + posePR := jPR + posePQ := jPQ - var comp, velp, omegap math32.Vector3 + var comP, vP, wP math32.Vector3 // parent transform and moment arm - if jpi >= 0 { - posepP = DynamicPos(jpi, params.Next) // now using next - posepQ = DynamicRot(jpi, params.Next) - slmath.MulQPTransforms(posepP, posepQ, jpP, jpQ, &xwpP, &xwpQ) - comp = BodyCom(jpbi) - mInvp = Bodies[jpbi, BodyInvMass] - iInvp = BodyInvInertia(jpbi) - velp = DynamicDelta(jpi, params.Next) - omegap = DynamicAngDelta(jpi, params.Next) + if jPi >= 0 { + posePR = DynamicPos(jPi, params.Next) // now using next + posePQ = DynamicQuat(jPi, params.Next) + slmath.MulSpatialTransforms(posePR, posePQ, jPR, jPQ, &xwPR, &xwPQ) + comP = BodyCom(jPbi) + mInvP = Bodies[jPbi, BodyInvMass] + iInvP = BodyInvInertia(jPbi) + vP = DynamicDelta(jPi, params.Next) + wP = DynamicAngDelta(jPi, params.Next) } // child transform and moment arm - posecP := DynamicPos(jci, params.Next) - posecQ := DynamicRot(jci, params.Next) - jcP := JointCPos(ji) - jcQ := JointCRot(ji) - xwcP := jcP - xwcQ := jcQ - slmath.MulQPTransforms(posecP, posecQ, jcP, jcQ, &xwcP, &xwcQ) - comc := BodyCom(jcbi) - mInvc := Bodies[jcbi, BodyInvMass] - iInvc := BodyInvInertia(jcbi) - velc := DynamicDelta(jci, params.Next) - omegac := DynamicAngDelta(jci, params.Next) - - if mInvp == 0.0 && mInvc == 0.0 { // connection between two immovable bodies + poseCR := DynamicPos(jCi, params.Next) + poseCQ := DynamicQuat(jCi, params.Next) + jCR := JointCPos(ji) + jCQ := JointCQuat(ji) + xwCR := jCR + xwCQ := jCQ + slmath.MulSpatialTransforms(poseCR, poseCQ, jCR, jCQ, &xwCR, &xwCQ) + comC := BodyCom(jCbi) + mInvC := Bodies[jCbi, BodyInvMass] + iInvC := BodyInvInertia(jCbi) + vC := DynamicDelta(jCi, params.Next) + wC := DynamicAngDelta(jCi, params.Next) + + if mInvP == 0.0 && mInvC == 0.0 { // connection between two immovable bodies return } // accumulate constraint deltas var linDeltaP, angDeltaP, linDeltaC, angDeltaC math32.Vector3 - relPoseP := xwpP - relPoseQ := xwpQ - slmath.QPTransformInverse(xwpP, xwpQ, &relPoseP, &relPoseQ) - slmath.MulQPTransforms(relPoseP, relPoseQ, xwcP, xwcQ, &relPoseP, &relPoseQ) - - // joint connection points - xc := xwcP + relPoseR := xwPR + relPoseQ := xwPQ + slmath.SpatialTransformInverse(xwPR, xwPQ, &relPoseR, &relPoseQ) + slmath.MulSpatialTransforms(relPoseR, relPoseQ, xwCR, xwCQ, &relPoseR, &relPoseQ) - // axis_start = joint_qd_start[tid] - // lin_axis_count = joint_dof_dim[tid, 0] - // ang_axis_count = joint_dof_dim[tid, 1] - - worldComp := slmath.MulQPPoint(posepP, posepQ, comp) - worldComc := slmath.MulQPPoint(posecP, posecQ, comc) - _ = worldComc + wComP := slmath.MulSpatialPoint(posePR, posePQ, comP) + wComC := slmath.MulSpatialPoint(poseCR, poseCQ, comC) // handle positional constraints if jt == Distance { - // r_p = wp.transform_get_translation(X_wp) - world_com_p - // r_c = wp.transform_get_translation(X_wc) - world_com_c - // lower = joint_limit_lower[axis_start] - // upper = joint_limit_upper[axis_start] - // if lower < 0.0 and upper < 0.0: - // # no limits - // return - // d = wp.length(rel_p) - // err = 0.0 - // if lower >= 0.0 and d < lower: - // err = d - lower - // # use a more descriptive direction vector for the constraint - // # in case the joint parent and child anchors are very close - // rel_p = err * wp.normalize(world_com_c - world_com_p) - // elif upper >= 0.0 and d > upper: - // err = d - upper - // - // if wp.abs(err) > 1e-9: - // # compute gradients - // linear_c = rel_p - // linear_p = -linear_c - // r_c = x_c - world_com_c - // angular_p = -wp.cross(r_p, linear_c) - // angular_c = wp.cross(r_c, linear_c) - // # constraint time derivative - // derr = ( - // wp.dot(linear_p, vel_p) - // + wp.dot(linear_c, vel_c) - // + wp.dot(angular_p, omega_p) - // + wp.dot(angular_c, omega_c) - // ) - // lambda_in = 0.0 - // compliance = linear_compliance - // ke = joint_target_ke[axis_start] - // if ke > 0.0: - // compliance = 1.0 / ke - // damping = joint_target_kd[axis_start] - // d_lambda = compute_positional_correction( - // err, - // derr, - // pose_p, - // pose_c, - // m_inv_p, - // m_inv_c, - // I_inv_p, - // I_inv_c, - // linear_p, - // linear_c, - // angular_p, - // angular_c, - // lambda_in, - // compliance, - // damping, - // dt, - // ) - // - // linDelta_p += linear_p * (d_lambda * params.JointLinearRelax) - // angDelta_p += angular_p * (d_lambda * angular_relaxation) - // linDelta_c += linear_c * (d_lambda * params.JointLinearRelax) - // angDelta_c += angular_c * (d_lambda * angular_relaxation) - // + dP := xwPR.Sub(wComP) + dC := xwCR.Sub(wComC) + lo := JointDoF(ji, 0, JointLimitLower) // only first one has constraint + up := JointDoF(ji, 0, JointLimitUpper) + if lo < 0 && up < 0 { // not limited + return + } + d := slmath.Length3(relPoseR) + err := float32(0.0) + if lo >= 0.0 && d < lo { + err = d - lo + // use a more descriptive direction vector for the constraint + // in case the joint parent and child anchors are very close + relPoseR = slmath.Normal3(wComC.Sub(wComP)).MulScalar(err) + } else if up >= 0.0 && d > up { + err = d - up + } + if math32.Abs(err) > 1e-9 { + // compute gradients + linearC := relPoseR + linearP := slmath.Negate3(linearC) + dC = xwCR.Sub(wComC) + angularP := slmath.Negate3(slmath.Cross3(dP, linearC)) + angularC := slmath.Cross3(dC, linearC) + // constraint time derivative + derr := slmath.Dot3(linearP, vP) + slmath.Dot3(linearC, vC) + slmath.Dot3(angularP, wP) + slmath.Dot3(angularC, wC) + lambdaIn := float32(0.0) + compliance := params.JointLinearComply + ke := JointDoF(ji, 0, JointStiff) + kd := JointDoF(ji, 0, JointDamp) + if ke > 0.0 { + compliance = 1.0 / ke + } + dLambda := PositionalCorrection(err, derr, posePQ, poseCQ, mInvP, mInvC, + iInvP, iInvC, linearP, linearC, angularP, angularC, lambdaIn, compliance, kd, params.Dt) + linDeltaP = linDeltaP.Add(linearP.MulScalar(dLambda * params.JointLinearRelax)) + angDeltaP = angDeltaP.Add(angularP.MulScalar(dLambda * params.JointAngularRelax)) + linDeltaC = linDeltaC.Add(linearC.MulScalar(dLambda * params.JointLinearRelax)) + angDeltaC = angDeltaC.Add(angularC.MulScalar(dLambda * params.JointAngularRelax)) + } } else { // compute joint target, stiffness, damping var axisLimitsD, axisLimitsA math32.Vector3 var axisTargetPosKeD, axisTargetPosKeA math32.Vector3 var axisTargetVelKdD, axisTargetVelKdA math32.Vector3 - axis := JointAxis(ji, 0) - loTemp := axis.MulScalar(JointDoF(ji, 0, JointLimitLower)) - upTemp := axis.MulScalar(JointDoF(ji, 0, JointLimitUpper)) - axisLimitsD = slmath.Min3(loTemp, upTemp) - axisLimitsA = slmath.Max3(loTemp, upTemp) - ke := JointDoF(ji, 0, JointStiff) - kd := JointDoF(ji, 0, JointDamp) - targetPos := GetJointTargetPos(ji, 0) - targetVel := GetJointTargetVel(ji, 0) - if ke > 0.0 { // has position control - UpdateJointAxisWeightedTarget(axis, targetPos, ke, &axisTargetPosKeD, &axisTargetPosKeA) - } - if kd > 0.0 { // has velocity control - UpdateJointAxisWeightedTarget(axis, targetVel, kd, &axisTargetVelKdD, &axisTargetVelKdA) + for dof := range jLinearN { + axis := JointAxis(ji, dof) + JointAxisLimitsUpdate(dof, axis, JointDoF(ji, dof, JointLimitLower), JointDoF(ji, dof, JointLimitUpper), &axisLimitsD, &axisLimitsA) + ke := JointDoF(ji, dof, JointStiff) + kd := JointDoF(ji, dof, JointDamp) + targetPos := JointControl(ji, dof, JointTargetPos) + targetVel := JointControl(ji, dof, JointTargetVel) + if ke > 0.0 { // has position control + JointAxisTarget(axis, targetPos, ke, &axisTargetPosKeD, &axisTargetPosKeA) + } + if kd > 0.0 { // has velocity control + JointAxisTarget(axis, targetVel, kd, &axisTargetVelKdD, &axisTargetVelKdA) + } } - // if lin_axis_count > 1: - // axis_idx = axis_start + 1 - // axis = joint_axis[axis_idx] - // lower = joint_limit_lower[axis_idx] - // upper = joint_limit_upper[axis_idx] - // axis_limits = update_joint_axis_limits(axis, lower, upper, axis_limits) - // ke = joint_target_ke[axis_idx] - // kd = joint_target_kd[axis_idx] - // target_pos = joint_target_pos[axis_idx] - // target_vel = joint_target_vel[axis_idx] - // if ke > 0.0: # has position control - // axis_target_pos_ke = update_joint_axis_weighted_target(axis, target_pos, ke, axis_target_pos_ke) - // if kd > 0.0: # has velocity control - // axis_target_vel_kd = update_joint_axis_weighted_target(axis, target_vel, kd, axis_target_vel_kd) - // if lin_axis_count > 2: - // axis_idx = axis_start + 2 - // axis = joint_axis[axis_idx] - // lower = joint_limit_lower[axis_idx] - // upper = joint_limit_upper[axis_idx] - // axis_limits = update_joint_axis_limits(axis, lower, upper, axis_limits) - // ke = joint_target_ke[axis_idx] - // kd = joint_target_kd[axis_idx] - // target_pos = joint_target_pos[axis_idx] - // target_vel = joint_target_vel[axis_idx] - // if ke > 0.0: # has position control - // axis_target_pos_ke = update_joint_axis_weighted_target(axis, target_pos, ke, axis_target_pos_ke) - // if kd > 0.0: # has velocity control - // axis_target_vel_kd = update_joint_axis_weighted_target(axis, target_vel, kd, axis_target_vel_kd) axisStiffness := axisTargetPosKeA axisDamping := axisTargetVelKdA - // for i := range 3 { - if axisStiffness.X > 0.0 { // todo: Dim(i) access - axisTargetPosKeD.X /= axisStiffness.X - } - // } - // for i := range 3 { - if axisDamping.X > 0.0 { // todo Dim - axisTargetVelKdD.X /= axisDamping.X - } - // } + axisTargetPosKeD = slmath.DivSafe3(axisTargetPosKeD, axisStiffness) + axisTargetVelKdD = slmath.DivSafe3(axisTargetVelKdD, axisDamping) axisLimitsLower := axisLimitsD axisLimitsUpper := axisLimitsA - // todo: - // frame_p = wp.quat_to_matrix(wp.transform_get_rotation(X_wp)) - // note that xc appearing in both is correct: - rp := xc.Sub(worldComp) - rc := xc.Sub(slmath.MulQPPoint(posecP, posecQ, comc)) - - // for loop will be unrolled, so we can modify local variables - // for dim := range 3 { - { - e := relPoseP.X // rel_p[dim] + // note that xwCR appearing in both is correct: + dP := xwCR.Sub(wComP) + dC := xwCR.Sub(slmath.MulSpatialPoint(poseCR, poseCQ, comC)) + for dim := range int32(3) { + e := slmath.Dim3(relPoseR, dim) // compute gradients - // linearc := math32.Vec3(frame_p[0, dim], frame_p[1, dim], frame_p[2, dim]) // todo - linearc := math32.Vec3(0, 0, 0) - linearp := slmath.Negate3(linearc) - angularp := slmath.Cross3(rp, linearc) - angularc := slmath.Cross3(rc, linearc) + // matrix indexing is [row, col] here: dim = col + // quat_to_matrix cols are q rotations of axis vectors + dima := slmath.SetDim3(math32.Vec3(0, 0, 0), dim, 1) // axis for dim + linearC := slmath.MulQuatVector(xwPQ, dima) + linearP := slmath.Negate3(linearC) + angularP := slmath.Negate3(slmath.Cross3(dP, linearC)) + angularC := slmath.Cross3(dC, linearC) // constraint time derivative - derr := slmath.Dot3(linearp, velp) + slmath.Dot3(linearc, velc) + slmath.Dot3(angularp, omegap) + slmath.Dot3(angularc, omegac) + derr := slmath.Dot3(linearP, vP) + slmath.Dot3(linearC, vC) + slmath.Dot3(angularP, wP) + slmath.Dot3(angularC, wC) err := float32(0.0) compliance := params.JointLinearComply damping := float32(0.0) - targetVel := axisTargetVelKdD.X // [dim] + targetVel := slmath.Dim3(axisTargetVelKdD, dim) derrRel := derr - targetVel // consider joint limits irrespective of axis mode - lower := axisLimitsLower.X // [dim] - upper := axisLimitsUpper.X // [dim] + lower := slmath.Dim3(axisLimitsLower, dim) + upper := slmath.Dim3(axisLimitsUpper, dim) if e < lower { err = e - lower } else if e > upper { err = e - upper } else { - targetPos := axisTargetPosKeD.X // [dim] + targetPos := slmath.Dim3(axisTargetPosKeD, dim) targetPos = math32.Clamp(targetPos, lower, upper) - if axisStiffness.X > 0.0 { + ke := slmath.Dim3(axisStiffness, dim) + kd := slmath.Dim3(axisDamping, dim) + if ke > 0.0 { err = e - targetPos - compliance = 1.0 / axisStiffness.X // [dim] - damping = axisDamping.X // [dim] - } else if axisDamping.X > 0.0 { - compliance = 1.0 / axisDamping.X // [dim] - damping = axisDamping.X // [dim] + compliance = 1.0 / ke + damping = slmath.Dim3(axisDamping, dim) + } else if kd > 0.0 { + compliance = 1.0 / kd + damping = kd } } if math32.Abs(err) > 1e-9 || math32.Abs(derrRel) > 1e-9 { lambdaIn := float32(0.0) - dLambda := PositionalCorrection(err, derrRel, posepQ, posecQ, mInvp, mInvc, - iInvp, iInvc, linearp, linearc, angularp, angularc, lambdaIn, compliance, damping, params.Dt) + dLambda := PositionalCorrection(err, derrRel, posePQ, poseCQ, mInvP, mInvC, + iInvP, iInvC, linearP, linearC, angularP, angularC, lambdaIn, compliance, damping, params.Dt) + + linDeltaP = linDeltaP.Add(linearP.MulScalar(dLambda * params.JointLinearRelax)) + angDeltaP = angDeltaP.Add(angularP.MulScalar(dLambda * params.JointAngularRelax)) + linDeltaC = linDeltaC.Add(linearC.MulScalar(dLambda * params.JointLinearRelax)) + angDeltaC = angDeltaC.Add(angularC.MulScalar(dLambda * params.JointAngularRelax)) + } + } + } + // todo: does it make sense to have prismatic, fixed here? + if jt == Fixed || jt == Prismatic || jt == Revolute || jt == D6 { // angular + qP := xwPQ + qC := xwCQ + // make quats lie in same hemisphere + if slmath.QuatDot(qP, qC) < 0 { + qC = slmath.QuatMulScalar(qC, -1.0) + } + relQ := slmath.MulQuats(slmath.QuatInverse(qP), qC) + qtwist := slmath.QuatNormalize(math32.NewQuat(relQ.X, 0.0, 0.0, relQ.W)) + qswing := slmath.MulQuats(relQ, slmath.QuatInverse(qtwist)) + + // decompose to a compound rotation each axis + s := math32.Sqrt(relQ.X*relQ.X + relQ.W*relQ.W) + invs := 1.0 / s + invscube := invs * invs * invs + + // handle axis-angle joints + // rescale twist from quaternion space to angular + err0 := 2.0 * math32.Asin(math32.Clamp(qtwist.X, -1.0, 1.0)) + err1 := qswing.Y + err2 := qswing.Z + // analytic gradients of swing-twist decomposition + grad0 := math32.NewQuat(invs-relQ.X*relQ.X*invscube, 0.0, 0.0, -(relQ.W*relQ.X)*invscube) + grad1 := math32.NewQuat( + -relQ.W*(relQ.W*relQ.Z+relQ.X*relQ.Y)*invscube, + relQ.W*invs, + -relQ.X*invs, + relQ.X*(relQ.W*relQ.Z+relQ.X*relQ.Y)*invscube) + grad2 := math32.NewQuat( + relQ.W*(relQ.W*relQ.Y-relQ.X*relQ.Z)*invscube, + relQ.X*invs, + relQ.W*invs, + relQ.X*(relQ.Z*relQ.X-relQ.W*relQ.Y)*invscube) + grad0 = slmath.QuatMulScalar(grad0, 2.0/math32.Abs(qtwist.W)) + // # grad0 *= 2.0 / wp.sqrt(1.0-qtwist[0]*qtwist[0]) # derivative of asin(x) = 1/sqrt(1-x^2) + + // rescale swing + swing_sq := qswing.W * qswing.W + // if swing axis magnitude close to zero vector, just treat in quaternion space + angularEps := float32(1.0e-4) + if swing_sq+angularEps < 1.0 { + d := math32.Sqrt(1.0 - qswing.W*qswing.W) + theta := 2.0 * math32.Acos(math32.Clamp(qswing.W, -1.0, 1.0)) + scale := theta / d + err1 *= scale + err2 *= scale + grad1 = slmath.QuatMulScalar(grad1, scale) + grad2 = slmath.QuatMulScalar(grad2, scale) + } + errs := math32.Vec3(err0, err1, err2) + gradX := math32.Vec3(grad0.X, grad1.X, grad2.X) + gradY := math32.Vec3(grad0.Y, grad1.Y, grad2.Y) + gradZ := math32.Vec3(grad0.Z, grad1.Z, grad2.Z) + gradW := math32.Vec3(grad0.W, grad1.W, grad2.W) + + // compute joint target, stiffness, damping + var axisLimitsD, axisLimitsA math32.Vector3 + var axisTargetPosKeD, axisTargetPosKeA math32.Vector3 + var axisTargetVelKdD, axisTargetVelKdA math32.Vector3 + + for dof := range jAngularN { + di := dof + jLinearN + axis := JointAxis(ji, di) + JointAxisLimitsUpdate(dof, axis, JointDoF(ji, di, JointLimitLower), JointDoF(ji, di, JointLimitUpper), &axisLimitsD, &axisLimitsA) + ke := JointDoF(ji, di, JointStiff) + kd := JointDoF(ji, di, JointDamp) + targetPos := JointControl(ji, di, JointTargetPos) + targetVel := JointControl(ji, di, JointTargetVel) + if ke > 0.0 { // has position control + JointAxisTarget(axis, targetPos, ke, &axisTargetPosKeD, &axisTargetPosKeA) + } + if kd > 0.0 { // has velocity control + JointAxisTarget(axis, targetVel, kd, &axisTargetVelKdD, &axisTargetVelKdA) + } + } + + axisStiffness := axisTargetPosKeA + axisDamping := axisTargetVelKdA + axisTargetPosKeD = slmath.DivSafe3(axisTargetPosKeD, axisStiffness) + axisTargetVelKdD = slmath.DivSafe3(axisTargetVelKdD, axisDamping) + axisLimitsLower := axisLimitsD + axisLimitsUpper := axisLimitsA + + for dim := range int32(3) { + e := slmath.Dim3(errs, dim) + + // analytic gradients of swing-twist decomposition + grad := math32.NewQuat(slmath.Dim3(gradX, dim), slmath.Dim3(gradY, dim), slmath.Dim3(gradZ, dim), slmath.Dim3(gradW, dim)) + // todo: verify -- does the 0.5 go inside?? + // quat_c = 0.5 * q_p * grad * wp.quat_inverse(q_c) + quatC := slmath.MulQuats(slmath.MulQuats(slmath.QuatMulScalar(qP, 0.5), grad), slmath.QuatInverse(qC)) + + angularC := math32.Vec3(quatC.X, quatC.Y, quatC.Z) + angularP := slmath.Negate3(angularC) + // constraint time derivative + derr := slmath.Dot3(angularP, wP) + slmath.Dot3(angularC, wC) + + err := float32(0.0) + compliance := params.JointLinearComply + damping := float32(0.0) + + targetVel := slmath.Dim3(axisTargetVelKdD, dim) + derrRel := derr - targetVel + + // consider joint limits irrespective of axis mode + lower := slmath.Dim3(axisLimitsLower, dim) + upper := slmath.Dim3(axisLimitsUpper, dim) + if e < lower { + err = e - lower + } else if e > upper { + err = e - upper + } else { + targetPos := slmath.Dim3(axisTargetPosKeD, dim) + targetPos = math32.Clamp(targetPos, lower, upper) - linDeltaP = linDeltaP.Add(linearp.MulScalar(dLambda * params.JointLinearRelax)) - angDeltaP = angDeltaP.Add(angularp.MulScalar(dLambda * params.JointAngularRelax)) - linDeltaC = linDeltaC.Add(linearc.MulScalar(dLambda * params.JointLinearRelax)) - angDeltaC = angDeltaC.Add(angularc.MulScalar(dLambda * params.JointAngularRelax)) + ke := slmath.Dim3(axisStiffness, dim) + kd := slmath.Dim3(axisDamping, dim) + if ke > 0.0 { + err = e - targetPos + compliance = 1.0 / ke + damping = slmath.Dim3(axisDamping, dim) + } else if kd > 0.0 { + compliance = 1.0 / kd + damping = kd + } } + lambdaIn := float32(0) + dLambda := AngularCorrection(err, derrRel, posePQ, poseCQ, iInvP, iInvC, angularP, angularC, lambdaIn, compliance, damping, params.Dt) + + // note: no relaxation factors here: + angDeltaP = angDeltaP.Add(angularP.MulScalar(dLambda)) + angDeltaC = angDeltaC.Add(angularC.MulScalar(dLambda)) } } - // - // if type == JointType.FIXED or type == JointType.PRISMATIC or type == JointType.REVOLUTE or type == JointType.D6: - // # handle angular constraints - // - // # local joint rotations - // q_p = wp.transform_get_rotation(X_wp) - // q_c = wp.transform_get_rotation(X_wc) - // - // # make quats lie in same hemisphere - // if slmath.Dot3(q_p, q_c) < 0.0: - // q_c *= -1.0 - // - // rel_q = wp.quat_inverse(q_p) * q_c - // - // qtwist = wp.normalize(wp.quat(rel_q[0], 0.0, 0.0, rel_q[3])) - // qswing = rel_q * wp.quat_inverse(qtwist) - // - // # decompose to a compound rotation each axis - // s = wp.sqrt(rel_q[0] * rel_q[0] + rel_q[3] * rel_q[3]) - // invs = 1.0 / s - // invscube = invs * invs * invs - // - // # handle axis-angle joints - // - // # rescale twist from quaternion space to angular - // err_0 = 2.0 * wp.asin(wp.clamp(qtwist[0], -1.0, 1.0)) - // err_1 = qswing[1] - // err_2 = qswing[2] - // # analytic gradients of swing-twist decomposition - // grad_0 = wp.quat(invs - rel_q[0] * rel_q[0] * invscube, 0.0, 0.0, -(rel_q[3] * rel_q[0]) * invscube) - // grad_1 = wp.quat( - // -rel_q[3] * (rel_q[3] * rel_q[2] + rel_q[0] * rel_q[1]) * invscube, - // rel_q[3] * invs, - // -rel_q[0] * invs, - // rel_q[0] * (rel_q[3] * rel_q[2] + rel_q[0] * rel_q[1]) * invscube, - // ) - // grad_2 = wp.quat( - // rel_q[3] * (rel_q[3] * rel_q[1] - rel_q[0] * rel_q[2]) * invscube, - // rel_q[0] * invs, - // rel_q[3] * invs, - // rel_q[0] * (rel_q[2] * rel_q[0] - rel_q[3] * rel_q[1]) * invscube, - // ) - // grad_0 *= 2.0 / wp.abs(qtwist[3]) - // # grad_0 *= 2.0 / wp.sqrt(1.0-qtwist[0]*qtwist[0]) # derivative of asin(x) = 1/sqrt(1-x^2) - // - // # rescale swing - // swing_sq = qswing[3] * qswing[3] - // # if swing axis magnitude close to zero vector, just treat in quaternion space - // angularEps = 1.0e-4 - // if swing_sq + angularEps < 1.0: - // d = wp.sqrt(1.0 - qswing[3] * qswing[3]) - // theta = 2.0 * wp.acos(wp.clamp(qswing[3], -1.0, 1.0)) - // scale = theta / d - // - // err_1 *= scale - // err_2 *= scale - // - // grad_1 *= scale - // grad_2 *= scale - // - // errs = wp.vec3(err_0, err_1, err_2) - // grad_x = wp.vec3(grad_0[0], grad_1[0], grad_2[0]) - // grad_y = wp.vec3(grad_0[1], grad_1[1], grad_2[1]) - // grad_z = wp.vec3(grad_0[2], grad_1[2], grad_2[2]) - // grad_w = wp.vec3(grad_0[3], grad_1[3], grad_2[3]) - // - // # compute joint target, stiffness, damping - // axis_limits = wp.spatial_vector(0.0, 0.0, 0.0, 0.0, 0.0, 0.0) - // - // axis_target_pos_ke = wp.spatial_vector() # [weighted_target_pos, ke_weights] - // axis_target_vel_kd = wp.spatial_vector() # [weighted_target_vel, kd_weights] - // # avoid a for loop here since local variables would need to be modified which is not yet differentiable - // if ang_axis_count > 0: - // axis_idx = axis_start + lin_axis_count - // axis = joint_axis[axis_idx] - // lo_temp = axis * joint_limit_lower[axis_idx] - // up_temp = axis * joint_limit_upper[axis_idx] - // axis_limits = wp.spatial_vector(vec_min(lo_temp, up_temp), vec_max(lo_temp, up_temp)) - // ke = joint_target_ke[axis_idx] - // kd = joint_target_kd[axis_idx] - // target_pos = joint_target_pos[axis_idx] - // target_vel = joint_target_vel[axis_idx] - // if ke > 0.0: # has position control - // axis_target_pos_ke = update_joint_axis_weighted_target(axis, target_pos, ke, axis_target_pos_ke) - // if kd > 0.0: # has velocity control - // axis_target_vel_kd = update_joint_axis_weighted_target(axis, target_vel, kd, axis_target_vel_kd) - - // if ang_axis_count > 1: - // axis_idx = axis_start + lin_axis_count + 1 - // axis = joint_axis[axis_idx] - // lower = joint_limit_lower[axis_idx] - // upper = joint_limit_upper[axis_idx] - // axis_limits = update_joint_axis_limits(axis, lower, upper, axis_limits) - // ke = joint_target_ke[axis_idx] - // kd = joint_target_kd[axis_idx] - // target_pos = joint_target_pos[axis_idx] - // target_vel = joint_target_vel[axis_idx] - // if ke > 0.0: # has position control - // axis_target_pos_ke = update_joint_axis_weighted_target(axis, target_pos, ke, axis_target_pos_ke) - // if kd > 0.0: # has velocity control - // axis_target_vel_kd = update_joint_axis_weighted_target(axis, target_vel, kd, axis_target_vel_kd) - // if ang_axis_count > 2: - // axis_idx = axis_start + lin_axis_count + 2 - // axis = joint_axis[axis_idx] - // lower = joint_limit_lower[axis_idx] - // upper = joint_limit_upper[axis_idx] - // axis_limits = update_joint_axis_limits(axis, lower, upper, axis_limits) - // ke = joint_target_ke[axis_idx] - // kd = joint_target_kd[axis_idx] - // target_pos = joint_target_pos[axis_idx] - // target_vel = joint_target_vel[axis_idx] - // if ke > 0.0: # has position control - // axis_target_pos_ke = update_joint_axis_weighted_target(axis, target_pos, ke, axis_target_pos_ke) - // if kd > 0.0: # has velocity control - // axis_target_vel_kd = update_joint_axis_weighted_target(axis, target_vel, kd, axis_target_vel_kd) - // - - // axis_target_pos = wp.spatial_top(axis_target_pos_ke) - // axis_stiffness = wp.spatial_bottom(axis_target_pos_ke) - // axis_target_vel = wp.spatial_top(axis_target_vel_kd) - // axis_damping = wp.spatial_bottom(axis_target_vel_kd) - // for i in range(3): - // if axis_stiffness[i] > 0.0: - // axis_target_pos[i] /= axis_stiffness[i] - // for i in range(3): - // if axis_damping[i] > 0.0: - // axis_target_vel[i] /= axis_damping[i] - // axis_limits_lower = wp.spatial_top(axis_limits) - // axis_limits_upper = wp.spatial_bottom(axis_limits) - - // # if type == JointType.D6: - // # wp.printf("axis_target: %f %f %f\t axis_stiffness: %f %f %f\t axis_damping: %f %f %f\t axis_limits_lower: %f %f %f \t axis_limits_upper: %f %f %f\n", - // # axis_target[0], axis_target[1], axis_target[2], - // # axis_stiffness[0], axis_stiffness[1], axis_stiffness[2], - // # axis_damping[0], axis_damping[1], axis_damping[2], - // # axis_limits_lower[0], axis_limits_lower[1], axis_limits_lower[2], - // # axis_limits_upper[0], axis_limits_upper[1], axis_limits_upper[2]) - // # # wp.printf("wp.sqrt(1.0-qtwist[0]*qtwist[0]) = %f\n", wp.sqrt(1.0-qtwist[0]*qtwist[0])) - - // for dim in range(3): - // e = errs[dim] - // - // # analytic gradients of swing-twist decomposition - // grad = wp.quat(grad_x[dim], grad_y[dim], grad_z[dim], grad_w[dim]) - // - // quat_c = 0.5 * q_p * grad * wp.quat_inverse(q_c) - // angular_c = wp.vec3(quat_c[0], quat_c[1], quat_c[2]) - // angular_p = -angular_c - // # time derivative of the constraint - // derr = slmath.Dot3(angular_p, omega_p) + slmath.Dot3(angular_c, omega_c) - // - // err = 0.0 - // compliance = angular_compliance - // damping = 0.0 - // - // target_vel = axis_target_vel[dim] - // derr_rel = derr - target_vel - // - // # consider joint limits irrespective of mode - // lower = axis_limits_lower[dim] - // upper = axis_limits_upper[dim] - // if e < lower: - // err = e - lower - // elif e > upper: - // err = e - upper - // else: - // target_pos = axis_target_pos[dim] - // target_pos = wp.clamp(target_pos, lower, upper) - // - // if axis_stiffness[dim] > 0.0: - // err = e - target_pos - // compliance = 1.0 / axis_stiffness[dim] - // damping = axis_damping[dim] - // elif axis_damping[dim] > 0.0: - // damping = axis_damping[dim] - // compliance = 1.0 / axis_damping[dim] - // - // d_lambda = ( - // compute_angular_correction( - // err, derr_rel, pose_p, pose_c, I_inv_p, I_inv_c, angular_p, angular_c, 0.0, compliance, damping, dt - // ) - // * angular_relaxation - // ) - // - // # update deltas - // angDelta_p += angular_p * d_lambda - // angDelta_c += angular_c * d_lambda // These are unique to joint: aggregate into dynamics Next in separate step. SetJointPDelta(ji, linDeltaP) @@ -542,26 +445,43 @@ func StepSolveJoints(i uint32) { //gosl:kernel SetJointCAngDelta(ji, angDeltaC) } -func UpdateJointAxisWeightedTarget(axis math32.Vector3, targ, weight float32, axisTargets, axisWeights *math32.Vector3) { +func JointAxisTarget(axis math32.Vector3, targ, weight float32, axisTargets, axisWeights *math32.Vector3) { weightedAxis := axis.MulScalar(weight) *axisTargets = (*axisTargets).Add(weightedAxis.MulScalar(targ)) // weighted target (to be normalized later by sum of weights) *axisWeights = (*axisWeights).Add(slmath.Abs3(weightedAxis)) } -func PositionalCorrection(err, derr float32, tfaQ, tfbQ math32.Quat, mInva, mInvb float32, Iinva, Iinvb math32.Matrix3, lineara, linearb, angulara, angularb math32.Vector3, lambdaIn, compliance, damping, dt float32) float32 { +func PositionalCorrection(err, derr float32, tfaQ, tfbQ math32.Quat, mInva, mInvb float32, iInva, iInvb math32.Matrix3, lineara, linearb, angulara, angularb math32.Vector3, lambdaIn, compliance, damping, dt float32) float32 { denom := float32(0.0) denom += slmath.LengthSquared3(lineara) * mInva denom += slmath.LengthSquared3(linearb) * mInvb - q1 := tfaQ - q2 := tfbQ + // # Eq. 2-3 (make sure to project into the frame of the body) + rotAngulara := slmath.MulQuatVectorInverse(tfaQ, angulara) + rotAngularb := slmath.MulQuatVectorInverse(tfbQ, angularb) + + denom += slmath.Dot3(rotAngulara, iInva.MulVector3(rotAngulara)) + denom += slmath.Dot3(rotAngularb, iInvb.MulVector3(rotAngularb)) + + alpha := compliance + gamma := compliance * damping + + deltaLambda := -(err + alpha*lambdaIn + gamma*derr) + if denom+alpha > 0.0 { + deltaLambda /= (dt+gamma)*denom + alpha/dt + } + return deltaLambda +} + +func AngularCorrection(err, derr float32, tfaQ, tfbQ math32.Quat, iInva, iInvb math32.Matrix3, angulara, angularb math32.Vector3, lambdaIn, compliance, damping, dt float32) float32 { // # Eq. 2-3 (make sure to project into the frame of the body) - rotAngulara := slmath.MulQuatVectorInverse(q1, angulara) - rotAngularb := slmath.MulQuatVectorInverse(q2, angularb) + rotAngulara := slmath.MulQuatVectorInverse(tfaQ, angulara) + rotAngularb := slmath.MulQuatVectorInverse(tfbQ, angularb) - denom += slmath.Dot3(rotAngulara, Iinva.MulVector3(rotAngulara)) - denom += slmath.Dot3(rotAngularb, Iinvb.MulVector3(rotAngularb)) + denom := float32(0.0) + denom += slmath.Dot3(rotAngulara, iInva.MulVector3(rotAngulara)) + denom += slmath.Dot3(rotAngularb, iInvb.MulVector3(rotAngularb)) alpha := compliance gamma := compliance * damping @@ -574,4 +494,20 @@ func PositionalCorrection(err, derr float32, tfaQ, tfbQ math32.Quat, mInva, mInv return deltaLambda } +// update the 3D linear/angular limits (spatial_vector [lower, upper]) +// given the axis vector and limits +func JointAxisLimitsUpdate(dof int32, axis math32.Vector3, lower, upper float32, axisLimitsD, axisLimitsA *math32.Vector3) { + loTemp := axis.MulScalar(lower) + upTemp := axis.MulScalar(upper) + lo := slmath.Min3(loTemp, upTemp) + up := slmath.Max3(loTemp, upTemp) + if dof == 0 { + *axisLimitsD = lo + *axisLimitsA = up + } else { + *axisLimitsD = slmath.Min3(*axisLimitsD, lo) + *axisLimitsA = slmath.Max3(*axisLimitsD, up) + } +} + //gosl:end diff --git a/physics/world.go b/physics/world.go index d049be4d..059ebf1e 100644 --- a/physics/world.go +++ b/physics/world.go @@ -85,7 +85,7 @@ func (wl *World) NewBody(shape Shapes, size, pos math32.Vector3, rot math32.Quat SetBodyShape(n, shape) SetBodySize(n, size) SetBodyPos(n, pos) - SetBodyRot(n, rot) + SetBodyQuat(n, rot) return n } @@ -102,42 +102,6 @@ func (wl *World) NewDynamic(shape Shapes, mass float32, size, pos math32.Vector3 return } -// NewJoint1D adds a new 1 DoF joint between parent and child dynamic object indexes, -// for Prismatic and Revolute joint types. -// Use -1 for parent to add a world-anchored joint. -// ppos, cpos are the relative positions from the parent, child. -// Sets relative rotation matricies to identity by default. -// axis is the axis of articulation for the joint. -// Use [JointDoFIndex] to access the remaining DoF parameters. -func (wl *World) NewJoint1D(joint JointTypes, parent, child int32, ppos, cpos, axis math32.Vector3) int32 { - sizes := wl.Joints.ShapeSizes() - idx := int32(sizes[0]) - wl.Joints.SetShapeSizes(int(idx+1), int(JointVarsN)) - SetJointParent(idx, parent) - SetJointChild(idx, child) - SetJointPPos(idx, ppos) - SetJointCPos(idx, cpos) - SetJointDoFN(idx, 1) - didx := wl.newJointDoF(0, axis) - SetJointDoFIndex(idx, 0, didx) - wl.JointDefaults(idx) - wl.Params[0].JointsN = idx + 1 - return idx -} - -// newJointDoF adds new JointDoFs and JointControls entries -// initialized to detfaults. Returns index. -func (wl *World) newJointDoF(dof int32, axis math32.Vector3) int32 { - sizes := wl.JointDoFs.ShapeSizes() - idx := int32(sizes[0]) - wl.JointDoFs.SetShapeSizes(int(idx+1), int(JointDoFVarsN)) - wl.JointControls.SetShapeSizes(int(idx+1), int(JointControlVarsN)) - wl.JointDoFDefaults(idx) - SetJointAxis(idx, 0, axis) - wl.Params[0].JointDoFsN = idx + 1 - return idx -} - // SetAsCurrent sets these as the current global values that are // processed in the code (on the GPU). If this was not the setter of // the current variables, then the parameter variables are copied up diff --git a/physics/world.goal b/physics/world.goal index d8fc6c97..30a399b0 100644 --- a/physics/world.goal +++ b/physics/world.goal @@ -83,7 +83,7 @@ func (wl *World) NewBody(shape Shapes, size, pos math32.Vector3, rot math32.Quat SetBodyShape(n, shape) SetBodySize(n, size) SetBodyPos(n, pos) - SetBodyRot(n, rot) + SetBodyQuat(n, rot) return n } @@ -100,42 +100,6 @@ func (wl *World) NewDynamic(shape Shapes, mass float32, size, pos math32.Vector3 return } -// NewJoint1D adds a new 1 DoF joint between parent and child dynamic object indexes, -// for Prismatic and Revolute joint types. -// Use -1 for parent to add a world-anchored joint. -// ppos, cpos are the relative positions from the parent, child. -// Sets relative rotation matricies to identity by default. -// axis is the axis of articulation for the joint. -// Use [JointDoFIndex] to access the remaining DoF parameters. -func (wl *World) NewJoint1D(joint JointTypes, parent, child int32, ppos, cpos, axis math32.Vector3) int32 { - sizes := wl.Joints.ShapeSizes() - idx := int32(sizes[0]) - wl.Joints.SetShapeSizes(int(idx+1), int(JointVarsN)) - SetJointParent(idx, parent) - SetJointChild(idx, child) - SetJointPPos(idx, ppos) - SetJointCPos(idx, cpos) - SetJointDoFN(idx, 1) - didx := wl.newJointDoF(0, axis) - SetJointDoFIndex(idx, 0, didx) - wl.JointDefaults(idx) - wl.Params[0].JointsN = idx + 1 - return idx -} - -// newJointDoF adds new JointDoFs and JointControls entries -// initialized to detfaults. Returns index. -func (wl *World) newJointDoF(dof int32, axis math32.Vector3) int32 { - sizes := wl.JointDoFs.ShapeSizes() - idx := int32(sizes[0]) - wl.JointDoFs.SetShapeSizes(int(idx+1), int(JointDoFVarsN)) - wl.JointControls.SetShapeSizes(int(idx+1), int(JointControlVarsN)) - wl.JointDoFDefaults(idx) - SetJointAxis(idx, 0, axis) - wl.Params[0].JointDoFsN = idx + 1 - return idx -} - // SetAsCurrent sets these as the current global values that are // processed in the code (on the GPU). If this was not the setter of // the current variables, then the parameter variables are copied up diff --git a/physics/world/view.go b/physics/world/view.go index b6d74f62..a0c1f912 100644 --- a/physics/world/view.go +++ b/physics/world/view.go @@ -32,8 +32,8 @@ type View struct { // Pos is the position. Pos math32.Vector3 - // Rot is the rotation as a quaternion. - Rot math32.Quat + // Quat is the rotation as a quaternion. + Quat math32.Quat // NewView is a function that returns a new [xyz.Node] // to represent this element. If nil, uses appropriate defaults. @@ -55,7 +55,7 @@ type View struct { // Use this for Static elements; NewDynamic for dynamic elements. func (wr *World) NewBody(wl *physics.World, name string, shape physics.Shapes, clr string, size, pos math32.Vector3, rot math32.Quat) *View { idx := wl.NewBody(shape, size, pos, rot) - vw := &View{Name: name, Index: idx, DynamicIndex: -1, Shape: shape, Color: clr, Size: size, Pos: pos, Rot: rot} + vw := &View{Name: name, Index: idx, DynamicIndex: -1, Shape: shape, Color: clr, Size: size, Pos: pos, Quat: rot} wr.Views = append(wr.Views, vw) return vw } @@ -64,7 +64,7 @@ func (wr *World) NewBody(wl *physics.World, name string, shape physics.Shapes, c // Returns the View which can then be further customized. func (wr *World) NewDynamic(wl *physics.World, name string, shape physics.Shapes, clr string, mass float32, size, pos math32.Vector3, rot math32.Quat) *View { idx, dyIdx := wl.NewDynamic(shape, mass, size, pos, rot) - vw := &View{Name: name, Index: idx, DynamicIndex: dyIdx, Shape: shape, Color: clr, Size: size, Pos: pos, Rot: rot} + vw := &View{Name: name, Index: idx, DynamicIndex: dyIdx, Shape: shape, Color: clr, Size: size, Pos: pos, Quat: rot} wr.Views = append(wr.Views, vw) return vw } @@ -75,18 +75,18 @@ func (vw *View) UpdateFromPhysics() { if vw.DynamicIndex >= 0 { ix := int32(vw.DynamicIndex) vw.Pos = physics.DynamicPos(ix, params.Cur) - vw.Rot = physics.DynamicRot(ix, params.Cur) + vw.Quat = physics.DynamicQuat(ix, params.Cur) } else { ix := int32(vw.Index) vw.Pos = physics.BodyPos(ix) - vw.Rot = physics.BodyRot(ix) + vw.Quat = physics.BodyQuat(ix) } } // UpdatePose updates the xyz node pose from view. func (vw *View) UpdatePose(sld *xyz.Solid) { sld.Pose.Pos = vw.Pos - sld.Pose.Quat = vw.Rot + sld.Pose.Quat = vw.Quat } // UpdateColor updates the xyz node color from view. diff --git a/physics/world/world.go b/physics/world/world.go index b5861f68..8251de87 100644 --- a/physics/world/world.go +++ b/physics/world/world.go @@ -89,7 +89,7 @@ func (wr *World) RenderFromNode(vw *View, cam *Camera) image.Image { sc.Camera.Near = cam.Near sc.Camera.Far = cam.Far sc.Camera.Pose.Pos = vw.Pos - sc.Camera.Pose.Quat = vw.Rot + sc.Camera.Pose.Quat = vw.Quat sc.Camera.Pose.Scale.Set(1, 1, 1) sc.UseAltFrame(cam.Size) From 24f8b17a4f22037e302138d21e49539b3e179ac1 Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Thu, 18 Dec 2025 22:56:02 +0100 Subject: [PATCH 22/97] physics: collision infra in place -- just need actual collision logic. --- gosl/gotosl/nodes.go | 6 +- physics/body.go | 69 +++++- physics/body.goal | 69 +++++- physics/config.go | 4 + physics/config.goal | 4 + physics/contact.go | 228 +++++++++++++++++++- physics/contact.goal | 214 ++++++++++++++++++- physics/dynamics.go | 16 +- physics/dynamics.goal | 16 +- physics/enumgen.go | 36 ++-- physics/examples/test1/test1.go | 9 +- physics/gosl.go | 84 ++++++-- physics/params.go | 19 +- physics/shaders/CollisionBroad.wgsl | 260 +++++++++++++++++++++++ physics/shaders/DeltasFromJoints.wgsl | 123 ++++++----- physics/shaders/DynamicsCurToNext.wgsl | 115 +++++----- physics/shaders/ForcesFromJoints.wgsl | 123 ++++++----- physics/shaders/InitDynamics.wgsl | 133 ++++++------ physics/shaders/StepBodyDeltas.wgsl | 157 +++++++------- physics/shaders/StepIntegrateBodies.wgsl | 170 ++++++++------- physics/shaders/StepJointForces.wgsl | 125 ++++++----- physics/shaders/StepSolveJoints.wgsl | 138 ++++++------ physics/step_body.go | 16 +- physics/step_body.goal | 16 +- physics/step_joint.go | 20 +- physics/step_joint.goal | 20 +- physics/typegen.go | 4 +- physics/vars.go | 8 +- physics/world.go | 13 +- physics/world.goal | 13 +- 30 files changed, 1608 insertions(+), 620 deletions(-) create mode 100644 physics/shaders/CollisionBroad.wgsl diff --git a/gosl/gotosl/nodes.go b/gosl/gotosl/nodes.go index a5c05966..3825e803 100644 --- a/gosl/gotosl/nodes.go +++ b/gosl/gotosl/nodes.go @@ -1794,8 +1794,10 @@ func (p *printer) getGlobalVar(ae *ast.AssignStmt, gvr *Var) { p.expr(cf.Args[0]) p.print(token.RBRACK, token.SEMICOLON) gvars := p.GoToSL.GetVarStack.Peek() - gvars[tmpVar] = &GetGlobalVar{Var: gvr, TmpVar: tmpVar, IdxExpr: cf.Args[0], ReadWrite: rwoverride} - p.GoToSL.GetVarStack[len(p.GoToSL.GetVarStack)-1] = gvars + if gvars != nil { + gvars[tmpVar] = &GetGlobalVar{Var: gvr, TmpVar: tmpVar, IdxExpr: cf.Args[0], ReadWrite: rwoverride} + p.GoToSL.GetVarStack[len(p.GoToSL.GetVarStack)-1] = gvars + } } // gosl: set non-read-only global vars back from temp var diff --git a/physics/body.go b/physics/body.go index 3ac71393..e205de8e 100644 --- a/physics/body.go +++ b/physics/body.go @@ -21,8 +21,29 @@ const ( // BodyShape is the shape type of the object, as a Shapes type. BodyShape BodyVars = iota - // BodyWorldIndex partitions body into different worlds; Global are -1 - BodyWorldIndex + // BodyDynamic is the index into Dynamics for this body, + // which is -1 for static bodies. Use this to get current + // Pos and Quat values for a dynamic body. + BodyDynamic + + // BodyWorld partitions bodies into different worlds for + // collision detection: Global bodies = -1 can collide with + // everything; otherwise only items within the same world collide. + BodyWorld + + // BodyGroup partitions bodies within worlds into different groups + // for collision detection. 0 does not collide with anything. + // Negative numbers are global within a world, except they don't + // collide amongst themselves (all non-dynamic bodies should go + // in -1 because they don't collide amongst each-other, but do + // potentially collide with dynamics). + // Positive numbers only collide amongst themselves, and with + // negative groups, but not other positive groups. This is for + // more special-purpose dynamics: in general use 1 for all dynamic + // bodies. There is an automatic constraint that the two objects + // within a single joint do not collide with each other, so this + // does not need to be handled here. + BodyGroup // BodySize is the size of the object (values depend on shape type). BodySizeX @@ -93,6 +114,30 @@ func SetBodyShape(idx int32, shape Shapes) { Bodies.Set(math.Float32frombits(uint32(shape)), int(idx), int(BodyShape)) } +func SetBodyDynamic(idx, dynIdx int32) { + Bodies.Set(math.Float32frombits(uint32(dynIdx)), int(idx), int(BodyDynamic)) +} + +func GetBodyDynamic(idx int32) int32 { + return int32(math.Float32bits(Bodies.Value(int(idx), int(BodyDynamic)))) +} + +func SetBodyWorld(idx, w int32) { + Bodies.Set(math.Float32frombits(uint32(w)), int(idx), int(BodyWorld)) +} + +func GetBodyWorld(idx int32) int32 { + return int32(math.Float32bits(Bodies.Value(int(idx), int(BodyWorld)))) +} + +func SetBodyGroup(idx, w int32) { + Bodies.Set(math.Float32frombits(uint32(w)), int(idx), int(BodyGroup)) +} + +func GetBodyGroup(idx int32) int32 { + return int32(math.Float32bits(Bodies.Value(int(idx), int(BodyGroup)))) +} + func BodySize(idx int32) math32.Vector3 { return math32.Vec3(Bodies.Value(int(idx), int(BodySizeX)), Bodies.Value(int(idx), int(BodySizeY)), Bodies.Value(int(idx), int(BodySizeZ))) } @@ -124,6 +169,26 @@ func SetBodyQuat(idx int32, rot math32.Quat) { Bodies.Set(rot.W, int(idx), int(BodyQuatW)) } +// BodyDynamicPos gets the position for dynamic bodies or +// static position if not dynamic. cni is the current / next index. +func BodyDynamicPos(idx, cni int32) math32.Vector3 { + didx := GetBodyDynamic(idx) + if didx < 0 { + return BodyPos(idx) + } + return DynamicPos(didx, cni) +} + +// BodyDynamicQuat gets the quat rotation for dynamic bodies or +// static rotation if not dynamic. cni is the current / next index. +func BodyDynamicQuat(idx, cni int32) math32.Quat { + didx := GetBodyDynamic(idx) + if didx < 0 { + return BodyQuat(idx) + } + return DynamicQuat(didx, cni) +} + func BodyCom(idx int32) math32.Vector3 { return math32.Vec3(Bodies.Value(int(idx), int(BodyComX)), Bodies.Value(int(idx), int(BodyComY)), Bodies.Value(int(idx), int(BodyComZ))) } diff --git a/physics/body.goal b/physics/body.goal index 0c05e969..fd97c441 100644 --- a/physics/body.goal +++ b/physics/body.goal @@ -19,8 +19,29 @@ const ( // BodyShape is the shape type of the object, as a Shapes type. BodyShape BodyVars = iota - // BodyWorldIndex partitions body into different worlds; Global are -1 - BodyWorldIndex + // BodyDynamic is the index into Dynamics for this body, + // which is -1 for static bodies. Use this to get current + // Pos and Quat values for a dynamic body. + BodyDynamic + + // BodyWorld partitions bodies into different worlds for + // collision detection: Global bodies = -1 can collide with + // everything; otherwise only items within the same world collide. + BodyWorld + + // BodyGroup partitions bodies within worlds into different groups + // for collision detection. 0 does not collide with anything. + // Negative numbers are global within a world, except they don't + // collide amongst themselves (all non-dynamic bodies should go + // in -1 because they don't collide amongst each-other, but do + // potentially collide with dynamics). + // Positive numbers only collide amongst themselves, and with + // negative groups, but not other positive groups. This is for + // more special-purpose dynamics: in general use 1 for all dynamic + // bodies. There is an automatic constraint that the two objects + // within a single joint do not collide with each other, so this + // does not need to be handled here. + BodyGroup // BodySize is the size of the object (values depend on shape type). BodySizeX @@ -91,6 +112,30 @@ func SetBodyShape(idx int32, shape Shapes) { Bodies[idx, BodyShape] = math.Float32frombits(uint32(shape)) } +func SetBodyDynamic(idx, dynIdx int32) { + Bodies[idx, BodyDynamic] = math.Float32frombits(uint32(dynIdx)) +} + +func GetBodyDynamic(idx int32) int32 { + return int32(math.Float32bits(Bodies[idx, BodyDynamic])) +} + +func SetBodyWorld(idx, w int32) { + Bodies[idx, BodyWorld] = math.Float32frombits(uint32(w)) +} + +func GetBodyWorld(idx int32) int32 { + return int32(math.Float32bits(Bodies[idx, BodyWorld])) +} + +func SetBodyGroup(idx, w int32) { + Bodies[idx, BodyGroup] = math.Float32frombits(uint32(w)) +} + +func GetBodyGroup(idx int32) int32 { + return int32(math.Float32bits(Bodies[idx, BodyGroup])) +} + func BodySize(idx int32) math32.Vector3 { return math32.Vec3(Bodies[idx, BodySizeX], Bodies[idx, BodySizeY], Bodies[idx, BodySizeZ]) } @@ -122,6 +167,26 @@ func SetBodyQuat(idx int32, rot math32.Quat) { Bodies[idx, BodyQuatW] = rot.W } +// BodyDynamicPos gets the position for dynamic bodies or +// static position if not dynamic. cni is the current / next index. +func BodyDynamicPos(idx, cni int32) math32.Vector3 { + didx := GetBodyDynamic(idx) + if didx < 0 { + return BodyPos(idx) + } + return DynamicPos(didx, cni) +} + +// BodyDynamicQuat gets the quat rotation for dynamic bodies or +// static rotation if not dynamic. cni is the current / next index. +func BodyDynamicQuat(idx, cni int32) math32.Quat { + didx := GetBodyDynamic(idx) + if didx < 0 { + return BodyQuat(idx) + } + return DynamicQuat(didx, cni) +} + func BodyCom(idx int32) math32.Vector3 { return math32.Vec3(Bodies[idx, BodyComX], Bodies[idx, BodyComY], Bodies[idx, BodyComZ]) } diff --git a/physics/config.go b/physics/config.go index 02e12531..c327d3c1 100644 --- a/physics/config.go +++ b/physics/config.go @@ -12,6 +12,7 @@ package physics // after everything has been added. Does SetAsCurrent, GPUInit. func (wl *World) Config() { wl.ConfigJoints() + wl.ConfigBodyCollidePairs() wl.SetAsCurrent() wl.GPUInit() wl.InitState() @@ -30,6 +31,9 @@ func (wl *World) ConfigJoints() { for ji := range nj { jpi := JointParentIndex(ji) jci := JointChildIndex(ji) + // bpi := DynamicBody(jpi) + // bci := DynamicBody(jci) + // todo: could ensure that all elements are in same world, but not really needed if jpi >= 0 { bjp[jpi] = append(bjp[jpi], ji) maxi = max(maxi, len(bjp[jpi])) diff --git a/physics/config.goal b/physics/config.goal index 65f103a8..69712aab 100644 --- a/physics/config.goal +++ b/physics/config.goal @@ -10,6 +10,7 @@ package physics // after everything has been added. Does SetAsCurrent, GPUInit. func (wl *World) Config() { wl.ConfigJoints() + wl.ConfigBodyCollidePairs() wl.SetAsCurrent() wl.GPUInit() wl.InitState() @@ -28,6 +29,9 @@ func (wl *World) ConfigJoints() { for ji := range nj { jpi := JointParentIndex(ji) jci := JointChildIndex(ji) + // bpi := DynamicBody(jpi) + // bci := DynamicBody(jci) + // todo: could ensure that all elements are in same world, but not really needed if jpi >= 0 { bjp[jpi] = append(bjp[jpi], ji) maxi = max(maxi, len(bjp[jpi])) diff --git a/physics/contact.go b/physics/contact.go index c76cdd5c..934c12f2 100644 --- a/physics/contact.go +++ b/physics/contact.go @@ -6,6 +6,11 @@ package physics +import ( + "cogentcore.org/lab/tensor" + "fmt" +) + //gosl:start // Contact is one pairwise point of contact between two bodies. @@ -14,29 +19,232 @@ package physics type ContactVars int32 //enums:enum const ( - // first body + // first body index ContactA ContactVars = iota - // the other body + // the other body index ContactB + // contact point on body A + ContactAPointX + ContactAPointY + ContactAPointZ + + // contact point on body B + ContactBPointX + ContactBPointY + ContactBPointZ + + // Contact depths (thickness in newton) + ContactADepth + ContactBDepth + // normal pointing from center of B to center of A ContactNormX ContactNormY ContactNormZ - // point on spherical shell of B where A is contacting - ContactPointX - ContactPointY - ContactPointZ - - // ContactDist is the distance from PtB along NormB to contact - // point on spherical shell of A. - ContactDist + // computed contact force vector + ContactForceX + ContactForceY + ContactForceZ ) +func WorldsCollide(wa, wb int32) bool { + if wa != -1 && wb != -1 && wa != wb { + return false + } + return true +} + +func GroupsCollide(ga, gb int32) bool { + if ga == 0 || gb == 0 { + return false + } + if ga > 0 { + return ga == gb || gb < 0 + } + if ga < 0 { + return ga != gb + } + return false +} + +// CollisionBroad +func CollisionBroad(i uint32) { //gosl:kernel + params := GetParams(0) + ci := int32(i) + if ci >= params.BodyCollidePairsN { + return + } + ba := BodyCollidePairs.Value(int(ci), int(0)) + bb := BodyCollidePairs.Value(int(ci), int(1)) + _ = ba + _ = bb + + // rigid_a = shape_body[shape_a] + // if rigid_a == -1: + // + // X_ws_a = shape_transform[shape_a] + // + // else: + // + // X_ws_a = body_q[rigid_a] * shape_transform[shape_a] + // + // rigid_b = shape_body[shape_b] + // if rigid_b == -1: + // + // X_ws_b = shape_transform[shape_b] + // + // else: + // + // X_ws_b = body_q[rigid_b] * shape_transform[shape_b] + // + // type_a = shape_type[shape_a] + // type_b = shape_type[shape_b] + // # ensure unique ordering of shape pairs + // if type_a > type_b: + // + // shape_tmp = shape_a + // shape_a = shape_b + // shape_b = shape_tmp + // + // type_tmp = type_a + // type_a = type_b + // type_b = type_tmp + // + // X_tmp = X_ws_a + // X_ws_a = X_ws_b + // X_ws_b = X_tmp + // + // p_b = wp.transform_get_translation(X_ws_b) + // r_b = shape_radius[shape_b] + // if type_a == GeoType.PLANE and type_b == GeoType.PLANE: + // + // return + // + // # Use per-shape contact margins + // margin = wp.max(shape_contact_margin[shape_a], shape_contact_margin[shape_b]) + // + // # bounding sphere check + // if type_a == GeoType.PLANE: + // + // query_b = wp.transform_point(wp.transform_inverse(X_ws_a), p_b) + // scale = shape_scale[shape_a] + // closest = closest_point_plane(scale[0], scale[1], query_b) + // d = wp.length(query_b - closest) + // if d > r_b + margin: + // return + // + // else: + // + // p_a = wp.transform_get_translation(X_ws_a) + // d = wp.length(p_a - p_b) + // r_a = shape_radius[shape_a] + // r_b = shape_radius[shape_b] + // if d > r_a + r_b + margin: + // return + // + // pair_index_ab = shape_a * num_shapes + shape_b + // pair_index_ba = shape_b * num_shapes + shape_a + // + // num_contacts_a, num_contacts_b = count_contact_points_for_pair( + // + // shape_a, shape_b, type_a, type_b, shape_scale, shape_source_ptr + // + // ) + // + // if contact_point_limit: + // + // # assign a limit per contact pair, if max_per_pair is set + // if max_per_pair > 0: + // # distribute maximum number of contact per pair in both directions + // max_per_pair_half = max_per_pair // 2 + // if num_contacts_b > 0: + // contact_point_limit[pair_index_ab] = max_per_pair_half + // contact_point_limit[pair_index_ba] = max_per_pair_half + // else: + // contact_point_limit[pair_index_ab] = max_per_pair + // contact_point_limit[pair_index_ba] = 0 + // else: + // contact_point_limit[pair_index_ab] = 0 + // contact_point_limit[pair_index_ba] = 0 + // + // # Allocate contact points using reusable method + // _success = allocate_contact_points( + // + // num_contacts_a, + // num_contacts_b, + // shape_a, + // shape_b, + // rigid_contact_max, + // contact_count, + // contact_shape0, + // contact_shape1, + // contact_point_id, + // + // ) +} + //gosl:end +// IsChildDynamic returns true if dic is a direct child +// on any joint where dip is the parent. +func (wl *World) IsChildDynamic(dip, dic int32) bool { + npja := wl.BodyJoints.Value(int(dip), int(0), int(0)) + for j := range npja { + ji := wl.BodyJoints.Value(int(dip), int(0), int(1+j)) + jci := JointChildIndex(ji) + if jci == dic { + return true + } + } + return false +} + +// ConfigBodyCollidePairs compiles a list of body paris that could collide +// based on world and group settings and not being direct parent +// child relationship within a joint. +func (wl *World) ConfigBodyCollidePairs() { + params := &wl.Params[0] + nb := params.BodiesN + nalc := int(nb) * 10 + pt := tensor.NewInt32(nalc, 2) + np := 0 + for a := range nb { + wa := GetBodyWorld(a) + ga := GetBodyGroup(a) + dia := GetBodyDynamic(a) + for b := range nb { + wb := GetBodyWorld(b) + gb := GetBodyGroup(b) + if !WorldsCollide(wa, wb) { + continue + } + if !GroupsCollide(ga, gb) { + continue + } + dib := GetBodyDynamic(b) + // now check joints (ConfigJoints must have been called first) + if wl.IsChildDynamic(dia, dib) { + continue + } + if np >= nalc { + nalc += int(nb) + pt.SetShapeSizes(nalc, 2) + fmt.Println("body pairs realoc", nalc) + } + pt.Set(a, int(np), int(0)) + pt.Set(b, int(np), int(1)) + np++ + } + } + params.BodyCollidePairsN = int32(np) + pt.SetShapeSizes(np, 2) + wl.BodyCollidePairs = pt + fmt.Println("body pairs over alloc", nalc, np) +} + // New adds a new contact to the list // func (cs *Contacts) New(a, b Body) *Contact { // c := &Contact{A: a, B: b} diff --git a/physics/contact.goal b/physics/contact.goal index d007d021..24aa2bad 100644 --- a/physics/contact.goal +++ b/physics/contact.goal @@ -4,6 +4,11 @@ package physics +import ( + "fmt" + "cogentcore.org/lab/tensor" +) + //gosl:start // Contact is one pairwise point of contact between two bodies. @@ -12,29 +17,218 @@ package physics type ContactVars int32 //enums:enum const ( - // first body + // first body index ContactA ContactVars = iota - // the other body + // the other body index ContactB + // contact point on body A + ContactAPointX + ContactAPointY + ContactAPointZ + + // contact point on body B + ContactBPointX + ContactBPointY + ContactBPointZ + + // Contact depths (thickness in newton) + ContactADepth + ContactBDepth + // normal pointing from center of B to center of A ContactNormX ContactNormY ContactNormZ - // point on spherical shell of B where A is contacting - ContactPointX - ContactPointY - ContactPointZ - - // ContactDist is the distance from PtB along NormB to contact - // point on spherical shell of A. - ContactDist + // computed contact force vector + ContactForceX + ContactForceY + ContactForceZ ) +func WorldsCollide(wa, wb int32) bool { + if wa != -1 && wb != -1 && wa != wb { + return false + } + return true +} + +func GroupsCollide(ga, gb int32) bool { + if ga == 0 || gb == 0 { + return false + } + if ga > 0 { + return ga == gb || gb < 0 + } + if ga < 0 { + return ga != gb + } + return false +} + +// CollisionBroad +func CollisionBroad(i uint32) { //gosl:kernel + params := GetParams(0) + ci := int32(i) + if ci >= params.BodyCollidePairsN { + return + } + ba := BodyCollidePairs[ci, 0] + bb := BodyCollidePairs[ci, 1] + _ = ba + _ = bb + + // rigid_a = shape_body[shape_a] + // if rigid_a == -1: + // X_ws_a = shape_transform[shape_a] + // else: + // X_ws_a = body_q[rigid_a] * shape_transform[shape_a] + // rigid_b = shape_body[shape_b] + // if rigid_b == -1: + // X_ws_b = shape_transform[shape_b] + // else: + // X_ws_b = body_q[rigid_b] * shape_transform[shape_b] +// + // type_a = shape_type[shape_a] + // type_b = shape_type[shape_b] + // # ensure unique ordering of shape pairs + // if type_a > type_b: + // shape_tmp = shape_a + // shape_a = shape_b + // shape_b = shape_tmp +// + // type_tmp = type_a + // type_a = type_b + // type_b = type_tmp +// + // X_tmp = X_ws_a + // X_ws_a = X_ws_b + // X_ws_b = X_tmp +// + // p_b = wp.transform_get_translation(X_ws_b) + // r_b = shape_radius[shape_b] + // if type_a == GeoType.PLANE and type_b == GeoType.PLANE: + // return +// + // # Use per-shape contact margins + // margin = wp.max(shape_contact_margin[shape_a], shape_contact_margin[shape_b]) +// + // # bounding sphere check + // if type_a == GeoType.PLANE: + // query_b = wp.transform_point(wp.transform_inverse(X_ws_a), p_b) + // scale = shape_scale[shape_a] + // closest = closest_point_plane(scale[0], scale[1], query_b) + // d = wp.length(query_b - closest) + // if d > r_b + margin: + // return + // else: + // p_a = wp.transform_get_translation(X_ws_a) + // d = wp.length(p_a - p_b) + // r_a = shape_radius[shape_a] + // r_b = shape_radius[shape_b] + // if d > r_a + r_b + margin: + // return +// + // pair_index_ab = shape_a * num_shapes + shape_b + // pair_index_ba = shape_b * num_shapes + shape_a +// + // num_contacts_a, num_contacts_b = count_contact_points_for_pair( + // shape_a, shape_b, type_a, type_b, shape_scale, shape_source_ptr + // ) +// + // if contact_point_limit: + // # assign a limit per contact pair, if max_per_pair is set + // if max_per_pair > 0: + // # distribute maximum number of contact per pair in both directions + // max_per_pair_half = max_per_pair // 2 + // if num_contacts_b > 0: + // contact_point_limit[pair_index_ab] = max_per_pair_half + // contact_point_limit[pair_index_ba] = max_per_pair_half + // else: + // contact_point_limit[pair_index_ab] = max_per_pair + // contact_point_limit[pair_index_ba] = 0 + // else: + // contact_point_limit[pair_index_ab] = 0 + // contact_point_limit[pair_index_ba] = 0 +// + // # Allocate contact points using reusable method + // _success = allocate_contact_points( + // num_contacts_a, + // num_contacts_b, + // shape_a, + // shape_b, + // rigid_contact_max, + // contact_count, + // contact_shape0, + // contact_shape1, + // contact_point_id, + // ) +} + + + //gosl:end +// IsChildDynamic returns true if dic is a direct child +// on any joint where dip is the parent. +func (wl *World) IsChildDynamic(dip, dic int32) bool { + npja := wl.BodyJoints[dip, 0, 0] + for j := range npja { + ji := wl.BodyJoints[dip, 0, 1+j] + jci := JointChildIndex(ji) + if jci == dic { + return true + } + } + return false +} + + +// ConfigBodyCollidePairs compiles a list of body paris that could collide +// based on world and group settings and not being direct parent +// child relationship within a joint. +func (wl *World) ConfigBodyCollidePairs() { + params := &wl.Params[0] + nb := params.BodiesN + nalc := int(nb) * 10 + pt := tensor.NewInt32(nalc, 2) + np := 0 + for a := range nb { + wa := GetBodyWorld(a) + ga := GetBodyGroup(a) + dia := GetBodyDynamic(a) + for b := range nb { + wb := GetBodyWorld(b) + gb := GetBodyGroup(b) + if !WorldsCollide(wa, wb) { + continue + } + if !GroupsCollide(ga, gb) { + continue + } + dib := GetBodyDynamic(b) + // now check joints (ConfigJoints must have been called first) + if wl.IsChildDynamic(dia, dib) { + continue + } + if np >= nalc { + nalc += int(nb) + pt.SetShapeSizes(nalc, 2) + fmt.Println("body pairs realoc", nalc) + } + pt[np, 0] = a + pt[np, 1] = b + np++ + } + } + params.BodyCollidePairsN = int32(np) + pt.SetShapeSizes(np, 2) + wl.BodyCollidePairs = pt + fmt.Println("body pairs over alloc", nalc, np) +} + // New adds a new contact to the list // func (cs *Contacts) New(a, b Body) *Contact { // c := &Contact{A: a, B: b} diff --git a/physics/dynamics.go b/physics/dynamics.go index 23363690..7620c019 100644 --- a/physics/dynamics.go +++ b/physics/dynamics.go @@ -19,9 +19,9 @@ type DynamicVars int32 //enums:enum const ( // Index of body in list of bodies. - DynIndex DynamicVars = iota + DynBody DynamicVars = iota - // 3D position of center of mass. + // 3D position of structural center. DynPosX DynPosY DynPosZ @@ -73,12 +73,16 @@ const ( DynAngDeltaZ ) -func SetDynamicIndex(idx, cni, bodyIdx int32) { - Dynamics.Set(math.Float32frombits(uint32(bodyIdx)), int(idx), int(cni), int(DynIndex)) +// cni = current / next index + +func SetDynamicBody(idx, bodyIdx int32) { + bi := math.Float32frombits(uint32(bodyIdx)) + Dynamics.Set(bi, int(idx), int(0), int(DynBody)) + Dynamics.Set(bi, int(idx), int(1), int(DynBody)) } -func DynamicIndex(idx, cni int32) int32 { - return int32(math.Float32bits(Dynamics.Value(int(idx), int(cni), int(DynIndex)))) +func DynamicBody(idx int32) int32 { + return int32(math.Float32bits(Dynamics.Value(int(idx), int(0), int(DynBody)))) } func DynamicPos(idx, cni int32) math32.Vector3 { diff --git a/physics/dynamics.goal b/physics/dynamics.goal index 460ff98e..22694d90 100644 --- a/physics/dynamics.goal +++ b/physics/dynamics.goal @@ -17,9 +17,9 @@ type DynamicVars int32 //enums:enum const ( // Index of body in list of bodies. - DynIndex DynamicVars = iota + DynBody DynamicVars = iota - // 3D position of center of mass. + // 3D position of structural center. DynPosX DynPosY DynPosZ @@ -71,12 +71,16 @@ const ( DynAngDeltaZ ) -func SetDynamicIndex(idx, cni, bodyIdx int32) { - Dynamics[idx, cni, DynIndex] = math.Float32frombits(uint32(bodyIdx)) +// cni = current / next index + +func SetDynamicBody(idx, bodyIdx int32) { + bi := math.Float32frombits(uint32(bodyIdx)) + Dynamics[idx, 0, DynBody] = bi + Dynamics[idx, 1, DynBody] = bi } -func DynamicIndex(idx, cni int32) int32 { - return int32(math.Float32bits(Dynamics[idx, cni, DynIndex])) +func DynamicBody(idx int32) int32 { + return int32(math.Float32bits(Dynamics[idx, 0, DynBody])) } func DynamicPos(idx, cni int32) math32.Vector3 { diff --git a/physics/enumgen.go b/physics/enumgen.go index abed223b..b08b11da 100644 --- a/physics/enumgen.go +++ b/physics/enumgen.go @@ -6,20 +6,20 @@ import ( "cogentcore.org/core/enums" ) -var _BodyVarsValues = []BodyVars{0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36} +var _BodyVarsValues = []BodyVars{0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38} // BodyVarsN is the highest valid value for type BodyVars, plus one. // //gosl:start -const BodyVarsN BodyVars = 37 +const BodyVarsN BodyVars = 39 //gosl:end -var _BodyVarsValueMap = map[string]BodyVars{`BodyShape`: 0, `BodyWorldIndex`: 1, `BodySizeX`: 2, `BodySizeY`: 3, `BodySizeZ`: 4, `BodyMass`: 5, `BodyInvMass`: 6, `BodyBounce`: 7, `BodyFriction`: 8, `BodyPosX`: 9, `BodyPosY`: 10, `BodyPosZ`: 11, `BodyQuatX`: 12, `BodyQuatY`: 13, `BodyQuatZ`: 14, `BodyQuatW`: 15, `BodyComX`: 16, `BodyComY`: 17, `BodyComZ`: 18, `BodyInertiaXX`: 19, `BodyInertiaYX`: 20, `BodyInertiaZX`: 21, `BodyInertiaXY`: 22, `BodyInertiaYY`: 23, `BodyInertiaZY`: 24, `BodyInertiaXZ`: 25, `BodyInertiaYZ`: 26, `BodyInertiaZZ`: 27, `BodyInvInertiaXX`: 28, `BodyInvInertiaYX`: 29, `BodyInvInertiaZX`: 30, `BodyInvInertiaXY`: 31, `BodyInvInertiaYY`: 32, `BodyInvInertiaZY`: 33, `BodyInvInertiaXZ`: 34, `BodyInvInertiaYZ`: 35, `BodyInvInertiaZZ`: 36} +var _BodyVarsValueMap = map[string]BodyVars{`BodyShape`: 0, `BodyDynamic`: 1, `BodyWorld`: 2, `BodyGroup`: 3, `BodySizeX`: 4, `BodySizeY`: 5, `BodySizeZ`: 6, `BodyMass`: 7, `BodyInvMass`: 8, `BodyBounce`: 9, `BodyFriction`: 10, `BodyPosX`: 11, `BodyPosY`: 12, `BodyPosZ`: 13, `BodyQuatX`: 14, `BodyQuatY`: 15, `BodyQuatZ`: 16, `BodyQuatW`: 17, `BodyComX`: 18, `BodyComY`: 19, `BodyComZ`: 20, `BodyInertiaXX`: 21, `BodyInertiaYX`: 22, `BodyInertiaZX`: 23, `BodyInertiaXY`: 24, `BodyInertiaYY`: 25, `BodyInertiaZY`: 26, `BodyInertiaXZ`: 27, `BodyInertiaYZ`: 28, `BodyInertiaZZ`: 29, `BodyInvInertiaXX`: 30, `BodyInvInertiaYX`: 31, `BodyInvInertiaZX`: 32, `BodyInvInertiaXY`: 33, `BodyInvInertiaYY`: 34, `BodyInvInertiaZY`: 35, `BodyInvInertiaXZ`: 36, `BodyInvInertiaYZ`: 37, `BodyInvInertiaZZ`: 38} -var _BodyVarsDescMap = map[BodyVars]string{0: `BodyShape is the shape type of the object, as a Shapes type.`, 1: `BodyWorldIndex partitions body into different worlds; Global are -1`, 2: `BodySize is the size of the object (values depend on shape type).`, 3: ``, 4: ``, 5: `BodyMass is the mass of the object.`, 6: `BodyInvMass is 1/mass of the object or 0 if no mass.`, 7: `BodyBounce specifies the COR or coefficient of restitution (0..1), which determines how elastic the collision is, i.e., final velocity / initial velocity.`, 8: `BodyFriction coefficient: how much friction is generated by transverse motion. Additive across the two surfaces.`, 9: `3D position of body (structural center).`, 10: ``, 11: ``, 12: `Quaternion rotation of body.`, 13: ``, 14: ``, 15: ``, 16: `Relative center-of-mass offset from 3D position of body.`, 17: ``, 18: ``, 19: `Inertia 3x3 matrix (column matrix organization, r,c labels).`, 20: ``, 21: ``, 22: ``, 23: ``, 24: ``, 25: ``, 26: ``, 27: ``, 28: `InvInertia inverse inertia 3x3 matrix (column matrix organization, r,c labels).`, 29: ``, 30: ``, 31: ``, 32: ``, 33: ``, 34: ``, 35: ``, 36: ``} +var _BodyVarsDescMap = map[BodyVars]string{0: `BodyShape is the shape type of the object, as a Shapes type.`, 1: `BodyDynamic is the index into Dynamics for this body, which is -1 for static bodies. Use this to get current Pos and Quat values for a dynamic body.`, 2: `BodyWorld partitions bodies into different worlds for collision detection: Global bodies = -1 can collide with everything; otherwise only items within the same world collide.`, 3: `BodyGroup partitions bodies within worlds into different groups for collision detection. 0 does not collide with anything. Negative numbers are global within a world, except they don't collide amongst themselves (all non-dynamic bodies should go in -1 because they don't collide amongst each-other, but do potentially collide with dynamics). Positive numbers only collide amongst themselves, and with negative groups, but not other positive groups. This is for more special-purpose dynamics: in general use 1 for all dynamic bodies. There is an automatic constraint that the two objects within a single joint do not collide with each other, so this does not need to be handled here.`, 4: `BodySize is the size of the object (values depend on shape type).`, 5: ``, 6: ``, 7: `BodyMass is the mass of the object.`, 8: `BodyInvMass is 1/mass of the object or 0 if no mass.`, 9: `BodyBounce specifies the COR or coefficient of restitution (0..1), which determines how elastic the collision is, i.e., final velocity / initial velocity.`, 10: `BodyFriction coefficient: how much friction is generated by transverse motion. Additive across the two surfaces.`, 11: `3D position of body (structural center).`, 12: ``, 13: ``, 14: `Quaternion rotation of body.`, 15: ``, 16: ``, 17: ``, 18: `Relative center-of-mass offset from 3D position of body.`, 19: ``, 20: ``, 21: `Inertia 3x3 matrix (column matrix organization, r,c labels).`, 22: ``, 23: ``, 24: ``, 25: ``, 26: ``, 27: ``, 28: ``, 29: ``, 30: `InvInertia inverse inertia 3x3 matrix (column matrix organization, r,c labels).`, 31: ``, 32: ``, 33: ``, 34: ``, 35: ``, 36: ``, 37: ``, 38: ``} -var _BodyVarsMap = map[BodyVars]string{0: `BodyShape`, 1: `BodyWorldIndex`, 2: `BodySizeX`, 3: `BodySizeY`, 4: `BodySizeZ`, 5: `BodyMass`, 6: `BodyInvMass`, 7: `BodyBounce`, 8: `BodyFriction`, 9: `BodyPosX`, 10: `BodyPosY`, 11: `BodyPosZ`, 12: `BodyQuatX`, 13: `BodyQuatY`, 14: `BodyQuatZ`, 15: `BodyQuatW`, 16: `BodyComX`, 17: `BodyComY`, 18: `BodyComZ`, 19: `BodyInertiaXX`, 20: `BodyInertiaYX`, 21: `BodyInertiaZX`, 22: `BodyInertiaXY`, 23: `BodyInertiaYY`, 24: `BodyInertiaZY`, 25: `BodyInertiaXZ`, 26: `BodyInertiaYZ`, 27: `BodyInertiaZZ`, 28: `BodyInvInertiaXX`, 29: `BodyInvInertiaYX`, 30: `BodyInvInertiaZX`, 31: `BodyInvInertiaXY`, 32: `BodyInvInertiaYY`, 33: `BodyInvInertiaZY`, 34: `BodyInvInertiaXZ`, 35: `BodyInvInertiaYZ`, 36: `BodyInvInertiaZZ`} +var _BodyVarsMap = map[BodyVars]string{0: `BodyShape`, 1: `BodyDynamic`, 2: `BodyWorld`, 3: `BodyGroup`, 4: `BodySizeX`, 5: `BodySizeY`, 6: `BodySizeZ`, 7: `BodyMass`, 8: `BodyInvMass`, 9: `BodyBounce`, 10: `BodyFriction`, 11: `BodyPosX`, 12: `BodyPosY`, 13: `BodyPosZ`, 14: `BodyQuatX`, 15: `BodyQuatY`, 16: `BodyQuatZ`, 17: `BodyQuatW`, 18: `BodyComX`, 19: `BodyComY`, 20: `BodyComZ`, 21: `BodyInertiaXX`, 22: `BodyInertiaYX`, 23: `BodyInertiaZX`, 24: `BodyInertiaXY`, 25: `BodyInertiaYY`, 26: `BodyInertiaZY`, 27: `BodyInertiaXZ`, 28: `BodyInertiaYZ`, 29: `BodyInertiaZZ`, 30: `BodyInvInertiaXX`, 31: `BodyInvInertiaYX`, 32: `BodyInvInertiaZX`, 33: `BodyInvInertiaXY`, 34: `BodyInvInertiaYY`, 35: `BodyInvInertiaZY`, 36: `BodyInvInertiaXZ`, 37: `BodyInvInertiaYZ`, 38: `BodyInvInertiaZZ`} // String returns the string representation of this BodyVars value. func (i BodyVars) String() string { return enums.String(i, _BodyVarsMap) } @@ -51,20 +51,20 @@ func (i BodyVars) MarshalText() ([]byte, error) { return []byte(i.String()), nil // UnmarshalText implements the [encoding.TextUnmarshaler] interface. func (i *BodyVars) UnmarshalText(text []byte) error { return enums.UnmarshalText(i, text, "BodyVars") } -var _ContactVarsValues = []ContactVars{0, 1, 2, 3, 4, 5, 6, 7, 8} +var _ContactVarsValues = []ContactVars{0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15} // ContactVarsN is the highest valid value for type ContactVars, plus one. // //gosl:start -const ContactVarsN ContactVars = 9 +const ContactVarsN ContactVars = 16 //gosl:end -var _ContactVarsValueMap = map[string]ContactVars{`ContactA`: 0, `ContactB`: 1, `ContactNormX`: 2, `ContactNormY`: 3, `ContactNormZ`: 4, `ContactPointX`: 5, `ContactPointY`: 6, `ContactPointZ`: 7, `ContactDist`: 8} +var _ContactVarsValueMap = map[string]ContactVars{`ContactA`: 0, `ContactB`: 1, `ContactAPointX`: 2, `ContactAPointY`: 3, `ContactAPointZ`: 4, `ContactBPointX`: 5, `ContactBPointY`: 6, `ContactBPointZ`: 7, `ContactADepth`: 8, `ContactBDepth`: 9, `ContactNormX`: 10, `ContactNormY`: 11, `ContactNormZ`: 12, `ContactForceX`: 13, `ContactForceY`: 14, `ContactForceZ`: 15} -var _ContactVarsDescMap = map[ContactVars]string{0: `first body`, 1: `the other body`, 2: `normal pointing from center of B to center of A`, 3: ``, 4: ``, 5: `point on spherical shell of B where A is contacting`, 6: ``, 7: ``, 8: `ContactDist is the distance from PtB along NormB to contact point on spherical shell of A.`} +var _ContactVarsDescMap = map[ContactVars]string{0: `first body index`, 1: `the other body index`, 2: `contact point on body A`, 3: ``, 4: ``, 5: `contact point on body B`, 6: ``, 7: ``, 8: `Contact depths (thickness in newton)`, 9: ``, 10: `normal pointing from center of B to center of A`, 11: ``, 12: ``, 13: `computed contact force vector`, 14: ``, 15: ``} -var _ContactVarsMap = map[ContactVars]string{0: `ContactA`, 1: `ContactB`, 2: `ContactNormX`, 3: `ContactNormY`, 4: `ContactNormZ`, 5: `ContactPointX`, 6: `ContactPointY`, 7: `ContactPointZ`, 8: `ContactDist`} +var _ContactVarsMap = map[ContactVars]string{0: `ContactA`, 1: `ContactB`, 2: `ContactAPointX`, 3: `ContactAPointY`, 4: `ContactAPointZ`, 5: `ContactBPointX`, 6: `ContactBPointY`, 7: `ContactBPointZ`, 8: `ContactADepth`, 9: `ContactBDepth`, 10: `ContactNormX`, 11: `ContactNormY`, 12: `ContactNormZ`, 13: `ContactForceX`, 14: `ContactForceY`, 15: `ContactForceZ`} // String returns the string representation of this ContactVars value. func (i ContactVars) String() string { return enums.String(i, _ContactVarsMap) } @@ -154,11 +154,11 @@ const DynamicVarsN DynamicVars = 32 //gosl:end -var _DynamicVarsValueMap = map[string]DynamicVars{`DynIndex`: 0, `DynPosX`: 1, `DynPosY`: 2, `DynPosZ`: 3, `DynQuatX`: 4, `DynQuatY`: 5, `DynQuatZ`: 6, `DynQuatW`: 7, `DynVelX`: 8, `DynVelY`: 9, `DynVelZ`: 10, `DynAngVelX`: 11, `DynAngVelY`: 12, `DynAngVelZ`: 13, `DynAccX`: 14, `DynAccY`: 15, `DynAccZ`: 16, `DynAngAccX`: 17, `DynAngAccY`: 18, `DynAngAccZ`: 19, `DynForceX`: 20, `DynForceY`: 21, `DynForceZ`: 22, `DynTorqueX`: 23, `DynTorqueY`: 24, `DynTorqueZ`: 25, `DynDeltaX`: 26, `DynDeltaY`: 27, `DynDeltaZ`: 28, `DynAngDeltaX`: 29, `DynAngDeltaY`: 30, `DynAngDeltaZ`: 31} +var _DynamicVarsValueMap = map[string]DynamicVars{`DynBody`: 0, `DynPosX`: 1, `DynPosY`: 2, `DynPosZ`: 3, `DynQuatX`: 4, `DynQuatY`: 5, `DynQuatZ`: 6, `DynQuatW`: 7, `DynVelX`: 8, `DynVelY`: 9, `DynVelZ`: 10, `DynAngVelX`: 11, `DynAngVelY`: 12, `DynAngVelZ`: 13, `DynAccX`: 14, `DynAccY`: 15, `DynAccZ`: 16, `DynAngAccX`: 17, `DynAngAccY`: 18, `DynAngAccZ`: 19, `DynForceX`: 20, `DynForceY`: 21, `DynForceZ`: 22, `DynTorqueX`: 23, `DynTorqueY`: 24, `DynTorqueZ`: 25, `DynDeltaX`: 26, `DynDeltaY`: 27, `DynDeltaZ`: 28, `DynAngDeltaX`: 29, `DynAngDeltaY`: 30, `DynAngDeltaZ`: 31} -var _DynamicVarsDescMap = map[DynamicVars]string{0: `Index of body in list of bodies.`, 1: `3D position of center of mass.`, 2: ``, 3: ``, 4: `Quaternion rotation.`, 5: ``, 6: ``, 7: ``, 8: `Linear velocity.`, 9: ``, 10: ``, 11: `Angular velocity.`, 12: ``, 13: ``, 14: `Linear acceleration.`, 15: ``, 16: ``, 17: `Angular acceleration due to applied torques.`, 18: ``, 19: ``, 20: `Linear force driving linear acceleration (from joints, etc).`, 21: ``, 22: ``, 23: `Torque driving angular acceleration (from joints, etc).`, 24: ``, 25: ``, 26: `Linear deltas.`, 27: ``, 28: ``, 29: `Angular deltas.`, 30: ``, 31: ``} +var _DynamicVarsDescMap = map[DynamicVars]string{0: `Index of body in list of bodies.`, 1: `3D position of structural center.`, 2: ``, 3: ``, 4: `Quaternion rotation.`, 5: ``, 6: ``, 7: ``, 8: `Linear velocity.`, 9: ``, 10: ``, 11: `Angular velocity.`, 12: ``, 13: ``, 14: `Linear acceleration.`, 15: ``, 16: ``, 17: `Angular acceleration due to applied torques.`, 18: ``, 19: ``, 20: `Linear force driving linear acceleration (from joints, etc).`, 21: ``, 22: ``, 23: `Torque driving angular acceleration (from joints, etc).`, 24: ``, 25: ``, 26: `Linear deltas.`, 27: ``, 28: ``, 29: `Angular deltas.`, 30: ``, 31: ``} -var _DynamicVarsMap = map[DynamicVars]string{0: `DynIndex`, 1: `DynPosX`, 2: `DynPosY`, 3: `DynPosZ`, 4: `DynQuatX`, 5: `DynQuatY`, 6: `DynQuatZ`, 7: `DynQuatW`, 8: `DynVelX`, 9: `DynVelY`, 10: `DynVelZ`, 11: `DynAngVelX`, 12: `DynAngVelY`, 13: `DynAngVelZ`, 14: `DynAccX`, 15: `DynAccY`, 16: `DynAccZ`, 17: `DynAngAccX`, 18: `DynAngAccY`, 19: `DynAngAccZ`, 20: `DynForceX`, 21: `DynForceY`, 22: `DynForceZ`, 23: `DynTorqueX`, 24: `DynTorqueY`, 25: `DynTorqueZ`, 26: `DynDeltaX`, 27: `DynDeltaY`, 28: `DynDeltaZ`, 29: `DynAngDeltaX`, 30: `DynAngDeltaY`, 31: `DynAngDeltaZ`} +var _DynamicVarsMap = map[DynamicVars]string{0: `DynBody`, 1: `DynPosX`, 2: `DynPosY`, 3: `DynPosZ`, 4: `DynQuatX`, 5: `DynQuatY`, 6: `DynQuatZ`, 7: `DynQuatW`, 8: `DynVelX`, 9: `DynVelY`, 10: `DynVelZ`, 11: `DynAngVelX`, 12: `DynAngVelY`, 13: `DynAngVelZ`, 14: `DynAccX`, 15: `DynAccY`, 16: `DynAccZ`, 17: `DynAngAccX`, 18: `DynAngAccY`, 19: `DynAngAccZ`, 20: `DynForceX`, 21: `DynForceY`, 22: `DynForceZ`, 23: `DynTorqueX`, 24: `DynTorqueY`, 25: `DynTorqueZ`, 26: `DynDeltaX`, 27: `DynDeltaY`, 28: `DynDeltaZ`, 29: `DynAngDeltaX`, 30: `DynAngDeltaY`, 31: `DynAngDeltaZ`} // String returns the string representation of this DynamicVars value. func (i DynamicVars) String() string { return enums.String(i, _DynamicVarsMap) } @@ -192,20 +192,20 @@ func (i *DynamicVars) UnmarshalText(text []byte) error { return enums.UnmarshalText(i, text, "DynamicVars") } -var _GPUVarsValues = []GPUVars{0, 1, 2, 3, 4, 5, 6, 7} +var _GPUVarsValues = []GPUVars{0, 1, 2, 3, 4, 5, 6, 7, 8} // GPUVarsN is the highest valid value for type GPUVars, plus one. // //gosl:start -const GPUVarsN GPUVars = 8 +const GPUVarsN GPUVars = 9 //gosl:end -var _GPUVarsValueMap = map[string]GPUVars{`ParamsVar`: 0, `BodiesVar`: 1, `JointsVar`: 2, `JointDoFsVar`: 3, `BodyJointsVar`: 4, `DynamicsVar`: 5, `ContactsVar`: 6, `JointControlsVar`: 7} +var _GPUVarsValueMap = map[string]GPUVars{`ParamsVar`: 0, `BodiesVar`: 1, `JointsVar`: 2, `JointDoFsVar`: 3, `BodyJointsVar`: 4, `BodyCollidePairsVar`: 5, `DynamicsVar`: 6, `ContactsVar`: 7, `JointControlsVar`: 8} -var _GPUVarsDescMap = map[GPUVars]string{0: ``, 1: ``, 2: ``, 3: ``, 4: ``, 5: ``, 6: ``, 7: ``} +var _GPUVarsDescMap = map[GPUVars]string{0: ``, 1: ``, 2: ``, 3: ``, 4: ``, 5: ``, 6: ``, 7: ``, 8: ``} -var _GPUVarsMap = map[GPUVars]string{0: `ParamsVar`, 1: `BodiesVar`, 2: `JointsVar`, 3: `JointDoFsVar`, 4: `BodyJointsVar`, 5: `DynamicsVar`, 6: `ContactsVar`, 7: `JointControlsVar`} +var _GPUVarsMap = map[GPUVars]string{0: `ParamsVar`, 1: `BodiesVar`, 2: `JointsVar`, 3: `JointDoFsVar`, 4: `BodyJointsVar`, 5: `BodyCollidePairsVar`, 6: `DynamicsVar`, 7: `ContactsVar`, 8: `JointControlsVar`} // String returns the string representation of this GPUVars value. func (i GPUVars) String() string { return enums.String(i, _GPUVarsMap) } diff --git a/physics/examples/test1/test1.go b/physics/examples/test1/test1.go index e6e1175f..8aec4214 100644 --- a/physics/examples/test1/test1.go +++ b/physics/examples/test1/test1.go @@ -59,8 +59,11 @@ func main() { math32.Vec3(0, height/2, 0), rot) bj := wl.NewJointRevolute(-1, b1.DynamicIndex, math32.Vec3(0, 0, 0), math32.Vec3(0, -height/2, 0), math32.Vec3(0, 1, 0)) - physics.SetJointControlForce(bj, 0, 2) + // bj := wl.NewJointPrismatic(-1, b1.DynamicIndex, math32.Vec3(0, 0, 0), math32.Vec3(0, -height/2, 0), math32.Vec3(1, 0, 0)) + // physics.SetJointControlForce(bj, 0, 5) + physics.SetJointTargetPos(bj, 0, 1) // physics.SetJointDoF(bj, 0, physics.JointDamp, 0.01) + // physics.SetJointDoF(bj, 0, physics.JointStiff, 1) // this makes a big difference wl.Config() params := physics.GetParams(0) @@ -78,8 +81,8 @@ func main() { sc.Camera.LookAt(math32.Vec3(0, 5, 0), math32.Vec3(0, 1, 0)) sc.SaveCamera("2") - sc.Camera.Pose.Pos = math32.Vec3(-.86, .97, 2.7) - sc.Camera.LookAt(math32.Vec3(0, .8, 0), math32.Vec3(0, 1, 0)) + sc.Camera.Pose.Pos = math32.Vec3(-1.33, 2.24, 3.55) + sc.Camera.LookAt(math32.Vec3(0, .5, 0), math32.Vec3(0, 1, 0)) sc.SaveCamera("1") sc.SaveCamera("default") diff --git a/physics/gosl.go b/physics/gosl.go index c76f9dfc..bdf9557e 100644 --- a/physics/gosl.go +++ b/physics/gosl.go @@ -44,9 +44,10 @@ const ( JointsVar GPUVars = 2 JointDoFsVar GPUVars = 3 BodyJointsVar GPUVars = 4 - DynamicsVar GPUVars = 5 - ContactsVar GPUVars = 6 - JointControlsVar GPUVars = 7 + BodyCollidePairsVar GPUVars = 5 + DynamicsVar GPUVars = 6 + ContactsVar GPUVars = 7 + JointControlsVar GPUVars = 8 ) // Tensor stride variables @@ -90,10 +91,11 @@ func GPUInit() { vr = sgp.Add("Joints", gpu.Float32, 1, gpu.ComputeShader) vr = sgp.Add("JointDoFs", gpu.Float32, 1, gpu.ComputeShader) vr = sgp.Add("BodyJoints", gpu.Int32, 1, gpu.ComputeShader) + vr = sgp.Add("BodyCollidePairs", gpu.Int32, 1, gpu.ComputeShader) sgp.SetNValues(1) } { - sgp := vars.AddGroup(gpu.Storage, "Bodies") + sgp := vars.AddGroup(gpu.Storage, "Dynamics") var vr *gpu.Var _ = vr vr = sgp.Add("Dynamics", gpu.Float32, 1, gpu.ComputeShader) @@ -108,6 +110,10 @@ func GPUInit() { sgp.SetNValues(1) } var pl *gpu.ComputePipeline + pl = gpu.NewComputePipelineShaderFS(shaders, "shaders/CollisionBroad.wgsl", sy) + pl.AddVarUsed(0, "TensorStrides") + pl.AddVarUsed(1, "BodyCollidePairs") + pl.AddVarUsed(0, "Params") pl = gpu.NewComputePipelineShaderFS(shaders, "shaders/DeltasFromJoints.wgsl", sy) pl.AddVarUsed(0, "TensorStrides") pl.AddVarUsed(1, "BodyJoints") @@ -174,6 +180,48 @@ func GPURelease() { ComputeGPU = nil } +// RunCollisionBroad runs the CollisionBroad kernel with given number of elements, +// on either the CPU or GPU depending on the UseGPU variable. +// Can call multiple Run* kernels in a row, which are then all launched +// in the same command submission on the GPU, which is by far the most efficient. +// MUST call RunDone (with optional vars to sync) after all Run calls. +// Alternatively, a single-shot RunOneCollisionBroad call does Run and Done for a +// single run-and-sync case. +func RunCollisionBroad(n int) { + if UseGPU { + RunCollisionBroadGPU(n) + } else { + RunCollisionBroadCPU(n) + } +} + +// RunCollisionBroadGPU runs the CollisionBroad kernel on the GPU. See [RunCollisionBroad] for more info. +func RunCollisionBroadGPU(n int) { + sy := GPUSystem + pl := sy.ComputePipelines["CollisionBroad"] + ce, _ := sy.BeginComputePass() + pl.Dispatch1D(ce, n, 64) +} + +// RunCollisionBroadCPU runs the CollisionBroad kernel on the CPU. +func RunCollisionBroadCPU(n int) { + gpu.VectorizeFunc(0, n, CollisionBroad) +} + +// RunOneCollisionBroad runs the CollisionBroad kernel with given number of elements, +// on either the CPU or GPU depending on the UseGPU variable. +// This version then calls RunDone with the given variables to sync +// after the Run, for a single-shot Run-and-Done call. If multiple kernels +// can be run in sequence, it is much more efficient to do multiple Run* +// calls followed by a RunDone call. +func RunOneCollisionBroad(n int, syncVars ...GPUVars) { + if UseGPU { + RunCollisionBroadGPU(n) + RunDone(syncVars...) + } else { + RunCollisionBroadCPU(n) + } +} // RunDeltasFromJoints runs the DeltasFromJoints kernel with given number of elements, // on either the CPU or GPU depending on the UseGPU variable. // Can call multiple Run* kernels in a row, which are then all launched @@ -550,6 +598,9 @@ func ToGPU(vars ...GPUVars) { case BodyJointsVar: v, _ := syVars.ValueByIndex(1, "BodyJoints", 0) gpu.SetValueFrom(v, BodyJoints.Values) + case BodyCollidePairsVar: + v, _ := syVars.ValueByIndex(1, "BodyCollidePairs", 0) + gpu.SetValueFrom(v, BodyCollidePairs.Values) case DynamicsVar: v, _ := syVars.ValueByIndex(2, "Dynamics", 0) gpu.SetValueFrom(v, Dynamics.Values) @@ -580,7 +631,7 @@ func ToGPUTensorStrides() { } sy := GPUSystem syVars := sy.Vars() - TensorStrides.SetShapeSizes(70) + TensorStrides.SetShapeSizes(80) TensorStrides.SetInt1D(Bodies.Shape().Strides[0], 0) TensorStrides.SetInt1D(Bodies.Shape().Strides[1], 1) TensorStrides.SetInt1D(Joints.Shape().Strides[0], 10) @@ -590,13 +641,15 @@ func ToGPUTensorStrides() { TensorStrides.SetInt1D(BodyJoints.Shape().Strides[0], 30) TensorStrides.SetInt1D(BodyJoints.Shape().Strides[1], 31) TensorStrides.SetInt1D(BodyJoints.Shape().Strides[2], 32) - TensorStrides.SetInt1D(Dynamics.Shape().Strides[0], 40) - TensorStrides.SetInt1D(Dynamics.Shape().Strides[1], 41) - TensorStrides.SetInt1D(Dynamics.Shape().Strides[2], 42) - TensorStrides.SetInt1D(Contacts.Shape().Strides[0], 50) - TensorStrides.SetInt1D(Contacts.Shape().Strides[1], 51) - TensorStrides.SetInt1D(JointControls.Shape().Strides[0], 60) - TensorStrides.SetInt1D(JointControls.Shape().Strides[1], 61) + TensorStrides.SetInt1D(BodyCollidePairs.Shape().Strides[0], 40) + TensorStrides.SetInt1D(BodyCollidePairs.Shape().Strides[1], 41) + TensorStrides.SetInt1D(Dynamics.Shape().Strides[0], 50) + TensorStrides.SetInt1D(Dynamics.Shape().Strides[1], 51) + TensorStrides.SetInt1D(Dynamics.Shape().Strides[2], 52) + TensorStrides.SetInt1D(Contacts.Shape().Strides[0], 60) + TensorStrides.SetInt1D(Contacts.Shape().Strides[1], 61) + TensorStrides.SetInt1D(JointControls.Shape().Strides[0], 70) + TensorStrides.SetInt1D(JointControls.Shape().Strides[1], 71) v, _ := syVars.ValueByIndex(0, "TensorStrides", 0) gpu.SetValueFrom(v, TensorStrides.Values) } @@ -622,6 +675,9 @@ func ReadFromGPU(vars ...GPUVars) { case BodyJointsVar: v, _ := syVars.ValueByIndex(1, "BodyJoints", 0) v.GPUToRead(sy.CommandEncoder) + case BodyCollidePairsVar: + v, _ := syVars.ValueByIndex(1, "BodyCollidePairs", 0) + v.GPUToRead(sy.CommandEncoder) case DynamicsVar: v, _ := syVars.ValueByIndex(2, "Dynamics", 0) v.GPUToRead(sy.CommandEncoder) @@ -661,6 +717,10 @@ func SyncFromGPU(vars ...GPUVars) { v, _ := syVars.ValueByIndex(1, "BodyJoints", 0) v.ReadSync() gpu.ReadToBytes(v, BodyJoints.Values) + case BodyCollidePairsVar: + v, _ := syVars.ValueByIndex(1, "BodyCollidePairs", 0) + v.ReadSync() + gpu.ReadToBytes(v, BodyCollidePairs.Values) case DynamicsVar: v, _ := syVars.ValueByIndex(2, "Dynamics", 0) v.ReadSync() diff --git a/physics/params.go b/physics/params.go index 7a11e505..e3624a2a 100644 --- a/physics/params.go +++ b/physics/params.go @@ -46,6 +46,15 @@ type PhysParams struct { // Restitution Restitution slbool.Bool `default:"false"` + // Index for the current state (0 or 1, alternates with Next). + Cur int32 `edit:"-"` + + // Index for the next state (1 or 0, alternates with Cur). + Next int32 `edit:"-"` + + // BodiesN is number of rigid bodies. + BodiesN int32 `edit:"-"` + // DynamicsN is number of dynamics bodies. DynamicsN int32 `edit:"-"` @@ -58,13 +67,11 @@ type PhysParams struct { // BodyJointsMax is max number of joints per body + 1 for actual n. BodyJointsMax int32 `edit:"-"` - // Index for the current state (0 or 1, alternates with Next). - Cur int32 `edit:"-"` - - // Index for the next state (1 or 0, alternates with Cur). - Next int32 `edit:"-"` + // BodyCollidePairsN is the total number of pre-compiled collision pairs + // to examine. + BodyCollidePairsN int32 `edit:"-"` - pad, pad1, pad2 int32 + pad int32 // Gravity is the gravity acceleration function Gravity slvec.Vector3 diff --git a/physics/shaders/CollisionBroad.wgsl b/physics/shaders/CollisionBroad.wgsl new file mode 100644 index 00000000..63e1bb2a --- /dev/null +++ b/physics/shaders/CollisionBroad.wgsl @@ -0,0 +1,260 @@ +// Code generated by "gosl"; DO NOT EDIT +// kernel: CollisionBroad + +// // Params are global parameters. +@group(0) @binding(0) +var TensorStrides: array; +@group(0) @binding(1) +var Params: array; +// // Bodies are the rigid body elements (dynamic and static), // specifying the constant, non-dynamic properties, // which is initial state for dynamics. // [body][BodyVarsN] +@group(1) @binding(4) +var BodyCollidePairs: array; +// // Dynamics are the dynamic rigid body elements: these actually move. // Two alternating states are used: Params.Cur and Params.Next. // [dyn body][cur/next][DynamicVarsN] +// // JointControls are dynamic joint control inputs, per joint DoF // (in correspondence with [JointDoFs]). This can be uploaded to the // GPU at every step. // [dof][JointControlVarsN] + +alias GPUVars = i32; + +@compute @workgroup_size(64, 1, 1) +fn main(@builtin(workgroup_id) wgid: vec3, @builtin(num_workgroups) nwg: vec3, @builtin(local_invocation_index) loci: u32) { + let idx = loci + (wgid.x + wgid.y * nwg.x + wgid.z * nwg.x * nwg.y) * 64; + CollisionBroad(idx); +} + +fn Index2D(s0: u32, s1: u32, i0: u32, i1: u32) -> u32 { + return s0 * i0 + s1 * i1; +} + + +//////// import: "vars.go" + +//////// import: "body.go" +alias BodyVars = i32; //enums:enum +const BodyShape: BodyVars = 0; +const BodyDynamic: BodyVars = 1; +const BodyWorld: BodyVars = 2; +const BodyGroup: BodyVars = 3; +const BodySizeX: BodyVars = 4; +const BodySizeY: BodyVars = 5; +const BodySizeZ: BodyVars = 6; +const BodyMass: BodyVars = 7; +const BodyInvMass: BodyVars = 8; +const BodyBounce: BodyVars = 9; +const BodyFriction: BodyVars = 10; +const BodyPosX: BodyVars = 11; +const BodyPosY: BodyVars = 12; +const BodyPosZ: BodyVars = 13; +const BodyQuatX: BodyVars = 14; +const BodyQuatY: BodyVars = 15; +const BodyQuatZ: BodyVars = 16; +const BodyQuatW: BodyVars = 17; +const BodyComX: BodyVars = 18; +const BodyComY: BodyVars = 19; +const BodyComZ: BodyVars = 20; +const BodyInertiaXX: BodyVars = 21; +const BodyInertiaYX: BodyVars = 22; +const BodyInertiaZX: BodyVars = 23; +const BodyInertiaXY: BodyVars = 24; +const BodyInertiaYY: BodyVars = 25; +const BodyInertiaZY: BodyVars = 26; +const BodyInertiaXZ: BodyVars = 27; +const BodyInertiaYZ: BodyVars = 28; +const BodyInertiaZZ: BodyVars = 29; +const BodyInvInertiaXX: BodyVars = 30; +const BodyInvInertiaYX: BodyVars = 31; +const BodyInvInertiaZX: BodyVars = 32; +const BodyInvInertiaXY: BodyVars = 33; +const BodyInvInertiaYY: BodyVars = 34; +const BodyInvInertiaZY: BodyVars = 35; +const BodyInvInertiaXZ: BodyVars = 36; +const BodyInvInertiaYZ: BodyVars = 37; +const BodyInvInertiaZZ: BodyVars = 38; + +//////// import: "contact.go" +alias ContactVars = i32; //enums:enum +const ContactA: ContactVars = 0; +const ContactB: ContactVars = 1; +const ContactAPointX: ContactVars = 2; +const ContactAPointY: ContactVars = 3; +const ContactAPointZ: ContactVars = 4; +const ContactBPointX: ContactVars = 5; +const ContactBPointY: ContactVars = 6; +const ContactBPointZ: ContactVars = 7; +const ContactADepth: ContactVars = 8; +const ContactBDepth: ContactVars = 9; +const ContactNormX: ContactVars = 10; +const ContactNormY: ContactVars = 11; +const ContactNormZ: ContactVars = 12; +const ContactForceX: ContactVars = 13; +const ContactForceY: ContactVars = 14; +const ContactForceZ: ContactVars = 15; +fn CollisionBroad(i: u32) { //gosl:kernel +let params = Params[0];; var ci = i32(i);; if (ci >= params.BodyCollidePairsN) { + return; +}; var ba = BodyCollidePairs[Index2D(TensorStrides[40], TensorStrides[41], u32(ci), u32(0))];; var bb = BodyCollidePairs[Index2D(TensorStrides[40], TensorStrides[41], +u32(ci), u32(1))];; _ = ba;; _ = bb; } + +//////// import: "control.go" +alias JointControlVars = i32; //enums:enum +const JointControlForce: JointControlVars = 0; +const JointTargetPos: JointControlVars = 1; +const JointTargetVel: JointControlVars = 2; + +//////// import: "dynamics.go" +alias DynamicVars = i32; //enums:enum +const DynBody: DynamicVars = 0; +const DynPosX: DynamicVars = 1; +const DynPosY: DynamicVars = 2; +const DynPosZ: DynamicVars = 3; +const DynQuatX: DynamicVars = 4; +const DynQuatY: DynamicVars = 5; +const DynQuatZ: DynamicVars = 6; +const DynQuatW: DynamicVars = 7; +const DynVelX: DynamicVars = 8; +const DynVelY: DynamicVars = 9; +const DynVelZ: DynamicVars = 10; +const DynAngVelX: DynamicVars = 11; +const DynAngVelY: DynamicVars = 12; +const DynAngVelZ: DynamicVars = 13; +const DynAccX: DynamicVars = 14; +const DynAccY: DynamicVars = 15; +const DynAccZ: DynamicVars = 16; +const DynAngAccX: DynamicVars = 17; +const DynAngAccY: DynamicVars = 18; +const DynAngAccZ: DynamicVars = 19; +const DynForceX: DynamicVars = 20; +const DynForceY: DynamicVars = 21; +const DynForceZ: DynamicVars = 22; +const DynTorqueX: DynamicVars = 23; +const DynTorqueY: DynamicVars = 24; +const DynTorqueZ: DynamicVars = 25; +const DynDeltaX: DynamicVars = 26; +const DynDeltaY: DynamicVars = 27; +const DynDeltaZ: DynamicVars = 28; +const DynAngDeltaX: DynamicVars = 29; +const DynAngDeltaY: DynamicVars = 30; +const DynAngDeltaZ: DynamicVars = 31; + +//////// import: "enumgen.go" +const BodyVarsN: BodyVars = 39; +const ContactVarsN: ContactVars = 16; +const JointControlVarsN: JointControlVars = 3; +const DynamicVarsN: DynamicVars = 32; +const GPUVarsN: GPUVars = 9; +const JointTypesN: JointTypes = 7; +const JointVarsN: JointVars = 50; +const JointDoFVarsN: JointDoFVars = 7; +const ShapesN: Shapes = 4; + +//////// import: "joint.go" +const JointLimitUnlimited = 1e10; +alias JointTypes = i32; //enums:enum +const Prismatic: JointTypes = 0; +const Revolute: JointTypes = 1; +const Ball: JointTypes = 2; +const Fixed: JointTypes = 3; +const Free: JointTypes = 4; +const Distance: JointTypes = 5; +const D6: JointTypes = 6; +alias JointVars = i32; //enums:enum +const JointType: JointVars = 0; +const JointEnabled: JointVars = 1; +const JointParent: JointVars = 2; +const JointChild: JointVars = 3; +const JointPPosX: JointVars = 4; +const JointPPosY: JointVars = 5; +const JointPPosZ: JointVars = 6; +const JointPQuatX: JointVars = 7; +const JointPQuatY: JointVars = 8; +const JointPQuatZ: JointVars = 9; +const JointPQuatW: JointVars = 10; +const JointCPosX: JointVars = 11; +const JointCPosY: JointVars = 12; +const JointCPosZ: JointVars = 13; +const JointCQuatX: JointVars = 14; +const JointCQuatY: JointVars = 15; +const JointCQuatZ: JointVars = 16; +const JointCQuatW: JointVars = 17; +const JointLinearDoFN: JointVars = 18; +const JointAngularDoFN: JointVars = 19; +const JointDoF1: JointVars = 20; +const JointDoF2: JointVars = 21; +const JointDoF3: JointVars = 22; +const JointDoF4: JointVars = 23; +const JointDoF5: JointVars = 24; +const JointDoF6: JointVars = 25; +const JointPForceX: JointVars = 26; +const JointPForceY: JointVars = 27; +const JointPForceZ: JointVars = 28; +const JointPTorqueX: JointVars = 29; +const JointPTorqueY: JointVars = 30; +const JointPTorqueZ: JointVars = 31; +const JointCForceX: JointVars = 32; +const JointCForceY: JointVars = 33; +const JointCForceZ: JointVars = 34; +const JointCTorqueX: JointVars = 35; +const JointCTorqueY: JointVars = 36; +const JointCTorqueZ: JointVars = 37; +const JointPDeltaX: JointVars = 38; +const JointPDeltaY: JointVars = 39; +const JointPDeltaZ: JointVars = 40; +const JointPAngDeltaX: JointVars = 41; +const JointPAngDeltaY: JointVars = 42; +const JointPAngDeltaZ: JointVars = 43; +const JointCDeltaX: JointVars = 44; +const JointCDeltaY: JointVars = 45; +const JointCDeltaZ: JointVars = 46; +const JointCAngDeltaX: JointVars = 47; +const JointCAngDeltaY: JointVars = 48; +const JointCAngDeltaZ: JointVars = 49; +alias JointDoFVars = i32; //enums:enum +const JointAxisX: JointDoFVars = 0; +const JointAxisY: JointDoFVars = 1; +const JointAxisZ: JointDoFVars = 2; +const JointLimitLower: JointDoFVars = 3; +const JointLimitUpper: JointDoFVars = 4; +const JointStiff: JointDoFVars = 5; +const JointDamp: JointDoFVars = 6; + +//////// import: "params.go" +struct PhysParams { + Iterations: i32, + Dt: f32, + SoftRelax: f32, + JointLinearRelax: f32, + JointAngularRelax: f32, + JointLinearComply: f32, + JointAngularComply: f32, + ContactRelax: f32, + AngularDamping: f32, + ContactWeighting: i32, + Restitution: i32, + Cur: i32, + Next: i32, + BodiesN: i32, + DynamicsN: i32, + JointsN: i32, + JointDoFsN: i32, + BodyJointsMax: i32, + BodyCollidePairsN: i32, + pad: i32, + Gravity: vec4, +} + +//////// import: "shapes.go" +alias Shapes = i32; //enums:enum +const Box: Shapes = 0; +const Sphere: Shapes = 1; +const Cylinder: Shapes = 2; +const Capsule: Shapes = 3; + +//////// import: "slmath-matrix3.go" + +//////// import: "slmath-quaternion.go" + +//////// import: "slmath-vector3.go" + +//////// import: "step.go" + +//////// import: "step_body.go" + +//////// import: "step_joint.go" \ No newline at end of file diff --git a/physics/shaders/DeltasFromJoints.wgsl b/physics/shaders/DeltasFromJoints.wgsl index dc0fb5d8..5a46bbf6 100644 --- a/physics/shaders/DeltasFromJoints.wgsl +++ b/physics/shaders/DeltasFromJoints.wgsl @@ -38,54 +38,63 @@ fn Index3D(s0: u32, s1: u32, s2: u32, i0: u32, i1: u32, i2: u32) -> u32 { //////// import: "body.go" alias BodyVars = i32; //enums:enum const BodyShape: BodyVars = 0; -const BodyWorldIndex: BodyVars = 1; -const BodySizeX: BodyVars = 2; -const BodySizeY: BodyVars = 3; -const BodySizeZ: BodyVars = 4; -const BodyMass: BodyVars = 5; -const BodyInvMass: BodyVars = 6; -const BodyBounce: BodyVars = 7; -const BodyFriction: BodyVars = 8; -const BodyPosX: BodyVars = 9; -const BodyPosY: BodyVars = 10; -const BodyPosZ: BodyVars = 11; -const BodyQuatX: BodyVars = 12; -const BodyQuatY: BodyVars = 13; -const BodyQuatZ: BodyVars = 14; -const BodyQuatW: BodyVars = 15; -const BodyComX: BodyVars = 16; -const BodyComY: BodyVars = 17; -const BodyComZ: BodyVars = 18; -const BodyInertiaXX: BodyVars = 19; -const BodyInertiaYX: BodyVars = 20; -const BodyInertiaZX: BodyVars = 21; -const BodyInertiaXY: BodyVars = 22; -const BodyInertiaYY: BodyVars = 23; -const BodyInertiaZY: BodyVars = 24; -const BodyInertiaXZ: BodyVars = 25; -const BodyInertiaYZ: BodyVars = 26; -const BodyInertiaZZ: BodyVars = 27; -const BodyInvInertiaXX: BodyVars = 28; -const BodyInvInertiaYX: BodyVars = 29; -const BodyInvInertiaZX: BodyVars = 30; -const BodyInvInertiaXY: BodyVars = 31; -const BodyInvInertiaYY: BodyVars = 32; -const BodyInvInertiaZY: BodyVars = 33; -const BodyInvInertiaXZ: BodyVars = 34; -const BodyInvInertiaYZ: BodyVars = 35; -const BodyInvInertiaZZ: BodyVars = 36; +const BodyDynamic: BodyVars = 1; +const BodyWorld: BodyVars = 2; +const BodyGroup: BodyVars = 3; +const BodySizeX: BodyVars = 4; +const BodySizeY: BodyVars = 5; +const BodySizeZ: BodyVars = 6; +const BodyMass: BodyVars = 7; +const BodyInvMass: BodyVars = 8; +const BodyBounce: BodyVars = 9; +const BodyFriction: BodyVars = 10; +const BodyPosX: BodyVars = 11; +const BodyPosY: BodyVars = 12; +const BodyPosZ: BodyVars = 13; +const BodyQuatX: BodyVars = 14; +const BodyQuatY: BodyVars = 15; +const BodyQuatZ: BodyVars = 16; +const BodyQuatW: BodyVars = 17; +const BodyComX: BodyVars = 18; +const BodyComY: BodyVars = 19; +const BodyComZ: BodyVars = 20; +const BodyInertiaXX: BodyVars = 21; +const BodyInertiaYX: BodyVars = 22; +const BodyInertiaZX: BodyVars = 23; +const BodyInertiaXY: BodyVars = 24; +const BodyInertiaYY: BodyVars = 25; +const BodyInertiaZY: BodyVars = 26; +const BodyInertiaXZ: BodyVars = 27; +const BodyInertiaYZ: BodyVars = 28; +const BodyInertiaZZ: BodyVars = 29; +const BodyInvInertiaXX: BodyVars = 30; +const BodyInvInertiaYX: BodyVars = 31; +const BodyInvInertiaZX: BodyVars = 32; +const BodyInvInertiaXY: BodyVars = 33; +const BodyInvInertiaYY: BodyVars = 34; +const BodyInvInertiaZY: BodyVars = 35; +const BodyInvInertiaXZ: BodyVars = 36; +const BodyInvInertiaYZ: BodyVars = 37; +const BodyInvInertiaZZ: BodyVars = 38; //////// import: "contact.go" alias ContactVars = i32; //enums:enum const ContactA: ContactVars = 0; const ContactB: ContactVars = 1; -const ContactNormX: ContactVars = 2; -const ContactNormY: ContactVars = 3; -const ContactNormZ: ContactVars = 4; -const ContactPointX: ContactVars = 5; -const ContactPointY: ContactVars = 6; -const ContactPointZ: ContactVars = 7; -const ContactDist: ContactVars = 8; +const ContactAPointX: ContactVars = 2; +const ContactAPointY: ContactVars = 3; +const ContactAPointZ: ContactVars = 4; +const ContactBPointX: ContactVars = 5; +const ContactBPointY: ContactVars = 6; +const ContactBPointZ: ContactVars = 7; +const ContactADepth: ContactVars = 8; +const ContactBDepth: ContactVars = 9; +const ContactNormX: ContactVars = 10; +const ContactNormY: ContactVars = 11; +const ContactNormZ: ContactVars = 12; +const ContactForceX: ContactVars = 13; +const ContactForceY: ContactVars = 14; +const ContactForceZ: ContactVars = 15; //////// import: "control.go" alias JointControlVars = i32; //enums:enum @@ -95,7 +104,7 @@ const JointTargetVel: JointControlVars = 2; //////// import: "dynamics.go" alias DynamicVars = i32; //enums:enum -const DynIndex: DynamicVars = 0; +const DynBody: DynamicVars = 0; const DynPosX: DynamicVars = 1; const DynPosY: DynamicVars = 2; const DynPosZ: DynamicVars = 3; @@ -128,22 +137,22 @@ const DynAngDeltaX: DynamicVars = 29; const DynAngDeltaY: DynamicVars = 30; const DynAngDeltaZ: DynamicVars = 31; fn SetDynamicDelta(idx: i32,cni: i32, delta: vec3) { - Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynDeltaX))] = delta.x; - Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynDeltaY))] = delta.y; - Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynDeltaZ))] = delta.z; + Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaX))] = delta.x; + Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaY))] = delta.y; + Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaZ))] = delta.z; } fn SetDynamicAngDelta(idx: i32,cni: i32, angDelta: vec3) { - Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynAngDeltaX))] = angDelta.x; - Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynAngDeltaY))] = angDelta.y; - Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynAngDeltaZ))] = angDelta.z; + Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaX))] = angDelta.x; + Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaY))] = angDelta.y; + Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaZ))] = angDelta.z; } //////// import: "enumgen.go" -const BodyVarsN: BodyVars = 37; -const ContactVarsN: ContactVars = 9; +const BodyVarsN: BodyVars = 39; +const ContactVarsN: ContactVars = 16; const JointControlVarsN: JointControlVars = 3; const DynamicVarsN: DynamicVars = 32; -const GPUVarsN: GPUVars = 8; +const GPUVarsN: GPUVars = 9; const JointTypesN: JointTypes = 7; const JointVarsN: JointVars = 50; const JointDoFVarsN: JointDoFVars = 7; @@ -244,15 +253,15 @@ struct PhysParams { AngularDamping: f32, ContactWeighting: i32, Restitution: i32, + Cur: i32, + Next: i32, + BodiesN: i32, DynamicsN: i32, JointsN: i32, JointDoFsN: i32, BodyJointsMax: i32, - Cur: i32, - Next: i32, + BodyCollidePairsN: i32, pad: i32, - pad1: i32, - pad2: i32, Gravity: vec4, } diff --git a/physics/shaders/DynamicsCurToNext.wgsl b/physics/shaders/DynamicsCurToNext.wgsl index 8f209879..d06cdf81 100644 --- a/physics/shaders/DynamicsCurToNext.wgsl +++ b/physics/shaders/DynamicsCurToNext.wgsl @@ -30,54 +30,63 @@ fn Index3D(s0: u32, s1: u32, s2: u32, i0: u32, i1: u32, i2: u32) -> u32 { //////// import: "body.go" alias BodyVars = i32; //enums:enum const BodyShape: BodyVars = 0; -const BodyWorldIndex: BodyVars = 1; -const BodySizeX: BodyVars = 2; -const BodySizeY: BodyVars = 3; -const BodySizeZ: BodyVars = 4; -const BodyMass: BodyVars = 5; -const BodyInvMass: BodyVars = 6; -const BodyBounce: BodyVars = 7; -const BodyFriction: BodyVars = 8; -const BodyPosX: BodyVars = 9; -const BodyPosY: BodyVars = 10; -const BodyPosZ: BodyVars = 11; -const BodyQuatX: BodyVars = 12; -const BodyQuatY: BodyVars = 13; -const BodyQuatZ: BodyVars = 14; -const BodyQuatW: BodyVars = 15; -const BodyComX: BodyVars = 16; -const BodyComY: BodyVars = 17; -const BodyComZ: BodyVars = 18; -const BodyInertiaXX: BodyVars = 19; -const BodyInertiaYX: BodyVars = 20; -const BodyInertiaZX: BodyVars = 21; -const BodyInertiaXY: BodyVars = 22; -const BodyInertiaYY: BodyVars = 23; -const BodyInertiaZY: BodyVars = 24; -const BodyInertiaXZ: BodyVars = 25; -const BodyInertiaYZ: BodyVars = 26; -const BodyInertiaZZ: BodyVars = 27; -const BodyInvInertiaXX: BodyVars = 28; -const BodyInvInertiaYX: BodyVars = 29; -const BodyInvInertiaZX: BodyVars = 30; -const BodyInvInertiaXY: BodyVars = 31; -const BodyInvInertiaYY: BodyVars = 32; -const BodyInvInertiaZY: BodyVars = 33; -const BodyInvInertiaXZ: BodyVars = 34; -const BodyInvInertiaYZ: BodyVars = 35; -const BodyInvInertiaZZ: BodyVars = 36; +const BodyDynamic: BodyVars = 1; +const BodyWorld: BodyVars = 2; +const BodyGroup: BodyVars = 3; +const BodySizeX: BodyVars = 4; +const BodySizeY: BodyVars = 5; +const BodySizeZ: BodyVars = 6; +const BodyMass: BodyVars = 7; +const BodyInvMass: BodyVars = 8; +const BodyBounce: BodyVars = 9; +const BodyFriction: BodyVars = 10; +const BodyPosX: BodyVars = 11; +const BodyPosY: BodyVars = 12; +const BodyPosZ: BodyVars = 13; +const BodyQuatX: BodyVars = 14; +const BodyQuatY: BodyVars = 15; +const BodyQuatZ: BodyVars = 16; +const BodyQuatW: BodyVars = 17; +const BodyComX: BodyVars = 18; +const BodyComY: BodyVars = 19; +const BodyComZ: BodyVars = 20; +const BodyInertiaXX: BodyVars = 21; +const BodyInertiaYX: BodyVars = 22; +const BodyInertiaZX: BodyVars = 23; +const BodyInertiaXY: BodyVars = 24; +const BodyInertiaYY: BodyVars = 25; +const BodyInertiaZY: BodyVars = 26; +const BodyInertiaXZ: BodyVars = 27; +const BodyInertiaYZ: BodyVars = 28; +const BodyInertiaZZ: BodyVars = 29; +const BodyInvInertiaXX: BodyVars = 30; +const BodyInvInertiaYX: BodyVars = 31; +const BodyInvInertiaZX: BodyVars = 32; +const BodyInvInertiaXY: BodyVars = 33; +const BodyInvInertiaYY: BodyVars = 34; +const BodyInvInertiaZY: BodyVars = 35; +const BodyInvInertiaXZ: BodyVars = 36; +const BodyInvInertiaYZ: BodyVars = 37; +const BodyInvInertiaZZ: BodyVars = 38; //////// import: "contact.go" alias ContactVars = i32; //enums:enum const ContactA: ContactVars = 0; const ContactB: ContactVars = 1; -const ContactNormX: ContactVars = 2; -const ContactNormY: ContactVars = 3; -const ContactNormZ: ContactVars = 4; -const ContactPointX: ContactVars = 5; -const ContactPointY: ContactVars = 6; -const ContactPointZ: ContactVars = 7; -const ContactDist: ContactVars = 8; +const ContactAPointX: ContactVars = 2; +const ContactAPointY: ContactVars = 3; +const ContactAPointZ: ContactVars = 4; +const ContactBPointX: ContactVars = 5; +const ContactBPointY: ContactVars = 6; +const ContactBPointZ: ContactVars = 7; +const ContactADepth: ContactVars = 8; +const ContactBDepth: ContactVars = 9; +const ContactNormX: ContactVars = 10; +const ContactNormY: ContactVars = 11; +const ContactNormZ: ContactVars = 12; +const ContactForceX: ContactVars = 13; +const ContactForceY: ContactVars = 14; +const ContactForceZ: ContactVars = 15; //////// import: "control.go" alias JointControlVars = i32; //enums:enum @@ -87,7 +96,7 @@ const JointTargetVel: JointControlVars = 2; //////// import: "dynamics.go" alias DynamicVars = i32; //enums:enum -const DynIndex: DynamicVars = 0; +const DynBody: DynamicVars = 0; const DynPosX: DynamicVars = 1; const DynPosY: DynamicVars = 2; const DynPosZ: DynamicVars = 3; @@ -121,11 +130,11 @@ const DynAngDeltaY: DynamicVars = 30; const DynAngDeltaZ: DynamicVars = 31; //////// import: "enumgen.go" -const BodyVarsN: BodyVars = 37; -const ContactVarsN: ContactVars = 9; +const BodyVarsN: BodyVars = 39; +const ContactVarsN: ContactVars = 16; const JointControlVarsN: JointControlVars = 3; const DynamicVarsN: DynamicVars = 32; -const GPUVarsN: GPUVars = 8; +const GPUVarsN: GPUVars = 9; const JointTypesN: JointTypes = 7; const JointVarsN: JointVars = 50; const JointDoFVarsN: JointDoFVars = 7; @@ -214,15 +223,15 @@ struct PhysParams { AngularDamping: f32, ContactWeighting: i32, Restitution: i32, + Cur: i32, + Next: i32, + BodiesN: i32, DynamicsN: i32, JointsN: i32, JointDoFsN: i32, BodyJointsMax: i32, - Cur: i32, - Next: i32, + BodyCollidePairsN: i32, pad: i32, - pad1: i32, - pad2: i32, Gravity: vec4, } @@ -248,8 +257,8 @@ fn DynamicsCurToNext(i: u32) { //gosl:kernel if (ii >= params.DynamicsN) { return; } - for (var di = DynIndex; di < DynamicVarsN; di++) { - Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(ii), u32(params.Next), u32(di))] = Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(ii), u32(params.Cur), u32(di))]; + for (var di = DynBody; di < DynamicVarsN; di++) { + Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(ii), u32(params.Next), u32(di))] = Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(ii), u32(params.Cur), u32(di))]; } } diff --git a/physics/shaders/ForcesFromJoints.wgsl b/physics/shaders/ForcesFromJoints.wgsl index 815170d2..b476fb5b 100644 --- a/physics/shaders/ForcesFromJoints.wgsl +++ b/physics/shaders/ForcesFromJoints.wgsl @@ -38,54 +38,63 @@ fn Index3D(s0: u32, s1: u32, s2: u32, i0: u32, i1: u32, i2: u32) -> u32 { //////// import: "body.go" alias BodyVars = i32; //enums:enum const BodyShape: BodyVars = 0; -const BodyWorldIndex: BodyVars = 1; -const BodySizeX: BodyVars = 2; -const BodySizeY: BodyVars = 3; -const BodySizeZ: BodyVars = 4; -const BodyMass: BodyVars = 5; -const BodyInvMass: BodyVars = 6; -const BodyBounce: BodyVars = 7; -const BodyFriction: BodyVars = 8; -const BodyPosX: BodyVars = 9; -const BodyPosY: BodyVars = 10; -const BodyPosZ: BodyVars = 11; -const BodyQuatX: BodyVars = 12; -const BodyQuatY: BodyVars = 13; -const BodyQuatZ: BodyVars = 14; -const BodyQuatW: BodyVars = 15; -const BodyComX: BodyVars = 16; -const BodyComY: BodyVars = 17; -const BodyComZ: BodyVars = 18; -const BodyInertiaXX: BodyVars = 19; -const BodyInertiaYX: BodyVars = 20; -const BodyInertiaZX: BodyVars = 21; -const BodyInertiaXY: BodyVars = 22; -const BodyInertiaYY: BodyVars = 23; -const BodyInertiaZY: BodyVars = 24; -const BodyInertiaXZ: BodyVars = 25; -const BodyInertiaYZ: BodyVars = 26; -const BodyInertiaZZ: BodyVars = 27; -const BodyInvInertiaXX: BodyVars = 28; -const BodyInvInertiaYX: BodyVars = 29; -const BodyInvInertiaZX: BodyVars = 30; -const BodyInvInertiaXY: BodyVars = 31; -const BodyInvInertiaYY: BodyVars = 32; -const BodyInvInertiaZY: BodyVars = 33; -const BodyInvInertiaXZ: BodyVars = 34; -const BodyInvInertiaYZ: BodyVars = 35; -const BodyInvInertiaZZ: BodyVars = 36; +const BodyDynamic: BodyVars = 1; +const BodyWorld: BodyVars = 2; +const BodyGroup: BodyVars = 3; +const BodySizeX: BodyVars = 4; +const BodySizeY: BodyVars = 5; +const BodySizeZ: BodyVars = 6; +const BodyMass: BodyVars = 7; +const BodyInvMass: BodyVars = 8; +const BodyBounce: BodyVars = 9; +const BodyFriction: BodyVars = 10; +const BodyPosX: BodyVars = 11; +const BodyPosY: BodyVars = 12; +const BodyPosZ: BodyVars = 13; +const BodyQuatX: BodyVars = 14; +const BodyQuatY: BodyVars = 15; +const BodyQuatZ: BodyVars = 16; +const BodyQuatW: BodyVars = 17; +const BodyComX: BodyVars = 18; +const BodyComY: BodyVars = 19; +const BodyComZ: BodyVars = 20; +const BodyInertiaXX: BodyVars = 21; +const BodyInertiaYX: BodyVars = 22; +const BodyInertiaZX: BodyVars = 23; +const BodyInertiaXY: BodyVars = 24; +const BodyInertiaYY: BodyVars = 25; +const BodyInertiaZY: BodyVars = 26; +const BodyInertiaXZ: BodyVars = 27; +const BodyInertiaYZ: BodyVars = 28; +const BodyInertiaZZ: BodyVars = 29; +const BodyInvInertiaXX: BodyVars = 30; +const BodyInvInertiaYX: BodyVars = 31; +const BodyInvInertiaZX: BodyVars = 32; +const BodyInvInertiaXY: BodyVars = 33; +const BodyInvInertiaYY: BodyVars = 34; +const BodyInvInertiaZY: BodyVars = 35; +const BodyInvInertiaXZ: BodyVars = 36; +const BodyInvInertiaYZ: BodyVars = 37; +const BodyInvInertiaZZ: BodyVars = 38; //////// import: "contact.go" alias ContactVars = i32; //enums:enum const ContactA: ContactVars = 0; const ContactB: ContactVars = 1; -const ContactNormX: ContactVars = 2; -const ContactNormY: ContactVars = 3; -const ContactNormZ: ContactVars = 4; -const ContactPointX: ContactVars = 5; -const ContactPointY: ContactVars = 6; -const ContactPointZ: ContactVars = 7; -const ContactDist: ContactVars = 8; +const ContactAPointX: ContactVars = 2; +const ContactAPointY: ContactVars = 3; +const ContactAPointZ: ContactVars = 4; +const ContactBPointX: ContactVars = 5; +const ContactBPointY: ContactVars = 6; +const ContactBPointZ: ContactVars = 7; +const ContactADepth: ContactVars = 8; +const ContactBDepth: ContactVars = 9; +const ContactNormX: ContactVars = 10; +const ContactNormY: ContactVars = 11; +const ContactNormZ: ContactVars = 12; +const ContactForceX: ContactVars = 13; +const ContactForceY: ContactVars = 14; +const ContactForceZ: ContactVars = 15; //////// import: "control.go" alias JointControlVars = i32; //enums:enum @@ -95,7 +104,7 @@ const JointTargetVel: JointControlVars = 2; //////// import: "dynamics.go" alias DynamicVars = i32; //enums:enum -const DynIndex: DynamicVars = 0; +const DynBody: DynamicVars = 0; const DynPosX: DynamicVars = 1; const DynPosY: DynamicVars = 2; const DynPosZ: DynamicVars = 3; @@ -128,22 +137,22 @@ const DynAngDeltaX: DynamicVars = 29; const DynAngDeltaY: DynamicVars = 30; const DynAngDeltaZ: DynamicVars = 31; fn SetDynamicForce(idx: i32,cni: i32, force: vec3) { - Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynForceX))] = force.x; - Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynForceY))] = force.y; - Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynForceZ))] = force.z; + Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynForceX))] = force.x; + Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynForceY))] = force.y; + Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynForceZ))] = force.z; } fn SetDynamicTorque(idx: i32,cni: i32, torque: vec3) { - Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynTorqueX))] = torque.x; - Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynTorqueY))] = torque.y; - Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynTorqueZ))] = torque.z; + Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynTorqueX))] = torque.x; + Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynTorqueY))] = torque.y; + Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynTorqueZ))] = torque.z; } //////// import: "enumgen.go" -const BodyVarsN: BodyVars = 37; -const ContactVarsN: ContactVars = 9; +const BodyVarsN: BodyVars = 39; +const ContactVarsN: ContactVars = 16; const JointControlVarsN: JointControlVars = 3; const DynamicVarsN: DynamicVars = 32; -const GPUVarsN: GPUVars = 8; +const GPUVarsN: GPUVars = 9; const JointTypesN: JointTypes = 7; const JointVarsN: JointVars = 50; const JointDoFVarsN: JointDoFVars = 7; @@ -244,15 +253,15 @@ struct PhysParams { AngularDamping: f32, ContactWeighting: i32, Restitution: i32, + Cur: i32, + Next: i32, + BodiesN: i32, DynamicsN: i32, JointsN: i32, JointDoFsN: i32, BodyJointsMax: i32, - Cur: i32, - Next: i32, + BodyCollidePairsN: i32, pad: i32, - pad1: i32, - pad2: i32, Gravity: vec4, } diff --git a/physics/shaders/InitDynamics.wgsl b/physics/shaders/InitDynamics.wgsl index 605ffbed..ea6d843c 100644 --- a/physics/shaders/InitDynamics.wgsl +++ b/physics/shaders/InitDynamics.wgsl @@ -36,54 +36,63 @@ fn Index3D(s0: u32, s1: u32, s2: u32, i0: u32, i1: u32, i2: u32) -> u32 { //////// import: "body.go" alias BodyVars = i32; //enums:enum const BodyShape: BodyVars = 0; -const BodyWorldIndex: BodyVars = 1; -const BodySizeX: BodyVars = 2; -const BodySizeY: BodyVars = 3; -const BodySizeZ: BodyVars = 4; -const BodyMass: BodyVars = 5; -const BodyInvMass: BodyVars = 6; -const BodyBounce: BodyVars = 7; -const BodyFriction: BodyVars = 8; -const BodyPosX: BodyVars = 9; -const BodyPosY: BodyVars = 10; -const BodyPosZ: BodyVars = 11; -const BodyQuatX: BodyVars = 12; -const BodyQuatY: BodyVars = 13; -const BodyQuatZ: BodyVars = 14; -const BodyQuatW: BodyVars = 15; -const BodyComX: BodyVars = 16; -const BodyComY: BodyVars = 17; -const BodyComZ: BodyVars = 18; -const BodyInertiaXX: BodyVars = 19; -const BodyInertiaYX: BodyVars = 20; -const BodyInertiaZX: BodyVars = 21; -const BodyInertiaXY: BodyVars = 22; -const BodyInertiaYY: BodyVars = 23; -const BodyInertiaZY: BodyVars = 24; -const BodyInertiaXZ: BodyVars = 25; -const BodyInertiaYZ: BodyVars = 26; -const BodyInertiaZZ: BodyVars = 27; -const BodyInvInertiaXX: BodyVars = 28; -const BodyInvInertiaYX: BodyVars = 29; -const BodyInvInertiaZX: BodyVars = 30; -const BodyInvInertiaXY: BodyVars = 31; -const BodyInvInertiaYY: BodyVars = 32; -const BodyInvInertiaZY: BodyVars = 33; -const BodyInvInertiaXZ: BodyVars = 34; -const BodyInvInertiaYZ: BodyVars = 35; -const BodyInvInertiaZZ: BodyVars = 36; +const BodyDynamic: BodyVars = 1; +const BodyWorld: BodyVars = 2; +const BodyGroup: BodyVars = 3; +const BodySizeX: BodyVars = 4; +const BodySizeY: BodyVars = 5; +const BodySizeZ: BodyVars = 6; +const BodyMass: BodyVars = 7; +const BodyInvMass: BodyVars = 8; +const BodyBounce: BodyVars = 9; +const BodyFriction: BodyVars = 10; +const BodyPosX: BodyVars = 11; +const BodyPosY: BodyVars = 12; +const BodyPosZ: BodyVars = 13; +const BodyQuatX: BodyVars = 14; +const BodyQuatY: BodyVars = 15; +const BodyQuatZ: BodyVars = 16; +const BodyQuatW: BodyVars = 17; +const BodyComX: BodyVars = 18; +const BodyComY: BodyVars = 19; +const BodyComZ: BodyVars = 20; +const BodyInertiaXX: BodyVars = 21; +const BodyInertiaYX: BodyVars = 22; +const BodyInertiaZX: BodyVars = 23; +const BodyInertiaXY: BodyVars = 24; +const BodyInertiaYY: BodyVars = 25; +const BodyInertiaZY: BodyVars = 26; +const BodyInertiaXZ: BodyVars = 27; +const BodyInertiaYZ: BodyVars = 28; +const BodyInertiaZZ: BodyVars = 29; +const BodyInvInertiaXX: BodyVars = 30; +const BodyInvInertiaYX: BodyVars = 31; +const BodyInvInertiaZX: BodyVars = 32; +const BodyInvInertiaXY: BodyVars = 33; +const BodyInvInertiaYY: BodyVars = 34; +const BodyInvInertiaZY: BodyVars = 35; +const BodyInvInertiaXZ: BodyVars = 36; +const BodyInvInertiaYZ: BodyVars = 37; +const BodyInvInertiaZZ: BodyVars = 38; //////// import: "contact.go" alias ContactVars = i32; //enums:enum const ContactA: ContactVars = 0; const ContactB: ContactVars = 1; -const ContactNormX: ContactVars = 2; -const ContactNormY: ContactVars = 3; -const ContactNormZ: ContactVars = 4; -const ContactPointX: ContactVars = 5; -const ContactPointY: ContactVars = 6; -const ContactPointZ: ContactVars = 7; -const ContactDist: ContactVars = 8; +const ContactAPointX: ContactVars = 2; +const ContactAPointY: ContactVars = 3; +const ContactAPointZ: ContactVars = 4; +const ContactBPointX: ContactVars = 5; +const ContactBPointY: ContactVars = 6; +const ContactBPointZ: ContactVars = 7; +const ContactADepth: ContactVars = 8; +const ContactBDepth: ContactVars = 9; +const ContactNormX: ContactVars = 10; +const ContactNormY: ContactVars = 11; +const ContactNormZ: ContactVars = 12; +const ContactForceX: ContactVars = 13; +const ContactForceY: ContactVars = 14; +const ContactForceZ: ContactVars = 15; //////// import: "control.go" alias JointControlVars = i32; //enums:enum @@ -93,7 +102,7 @@ const JointTargetVel: JointControlVars = 2; //////// import: "dynamics.go" alias DynamicVars = i32; //enums:enum -const DynIndex: DynamicVars = 0; +const DynBody: DynamicVars = 0; const DynPosX: DynamicVars = 1; const DynPosY: DynamicVars = 2; const DynPosZ: DynamicVars = 3; @@ -125,16 +134,16 @@ const DynDeltaZ: DynamicVars = 28; const DynAngDeltaX: DynamicVars = 29; const DynAngDeltaY: DynamicVars = 30; const DynAngDeltaZ: DynamicVars = 31; -fn DynamicIndex(idx: i32,cni: i32) -> i32 { - return i32(bitcast(Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynIndex))])); +fn DynamicBody(idx: i32) -> i32 { + return i32(bitcast(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(0), u32(DynBody))])); } //////// import: "enumgen.go" -const BodyVarsN: BodyVars = 37; -const ContactVarsN: ContactVars = 9; +const BodyVarsN: BodyVars = 39; +const ContactVarsN: ContactVars = 16; const JointControlVarsN: JointControlVars = 3; const DynamicVarsN: DynamicVars = 32; -const GPUVarsN: GPUVars = 8; +const GPUVarsN: GPUVars = 9; const JointTypesN: JointTypes = 7; const JointVarsN: JointVars = 50; const JointDoFVarsN: JointDoFVars = 7; @@ -223,15 +232,15 @@ struct PhysParams { AngularDamping: f32, ContactWeighting: i32, Restitution: i32, + Cur: i32, + Next: i32, + BodiesN: i32, DynamicsN: i32, JointsN: i32, JointDoFsN: i32, BodyJointsMax: i32, - Cur: i32, - Next: i32, + BodyCollidePairsN: i32, pad: i32, - pad1: i32, - pad2: i32, Gravity: vec4, } @@ -258,16 +267,16 @@ fn InitDynamics(i: u32) { //gosl:kernel return; } for (var cni=0; cni<2; cni++) { - var bi = DynamicIndex(ii, i32(cni)); - Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(ii), u32(cni), u32(DynPosX))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyPosX))]; - Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(ii), u32(cni), u32(DynPosY))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyPosY))]; - Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(ii), u32(cni), u32(DynPosZ))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyPosZ))]; - Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(ii), u32(cni), u32(DynQuatX))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyQuatX))]; - Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(ii), u32(cni), u32(DynQuatY))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyQuatY))]; - Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(ii), u32(cni), u32(DynQuatZ))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyQuatZ))]; - Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(ii), u32(cni), u32(DynQuatW))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyQuatW))]; + var bi = DynamicBody(ii); + Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(ii), u32(cni), u32(DynPosX))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyPosX))]; + Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(ii), u32(cni), u32(DynPosY))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyPosY))]; + Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(ii), u32(cni), u32(DynPosZ))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyPosZ))]; + Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(ii), u32(cni), u32(DynQuatX))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyQuatX))]; + Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(ii), u32(cni), u32(DynQuatY))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyQuatY))]; + Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(ii), u32(cni), u32(DynQuatZ))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyQuatZ))]; + Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(ii), u32(cni), u32(DynQuatW))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyQuatW))]; for (var v = DynVelX; v < DynamicVarsN; v++) { - Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], + Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(ii), u32(cni), u32(v))] = 0.0; } } diff --git a/physics/shaders/StepBodyDeltas.wgsl b/physics/shaders/StepBodyDeltas.wgsl index 1790ee45..dd1885f1 100644 --- a/physics/shaders/StepBodyDeltas.wgsl +++ b/physics/shaders/StepBodyDeltas.wgsl @@ -36,42 +36,44 @@ fn Index3D(s0: u32, s1: u32, s2: u32, i0: u32, i1: u32, i2: u32) -> u32 { //////// import: "body.go" alias BodyVars = i32; //enums:enum const BodyShape: BodyVars = 0; -const BodyWorldIndex: BodyVars = 1; -const BodySizeX: BodyVars = 2; -const BodySizeY: BodyVars = 3; -const BodySizeZ: BodyVars = 4; -const BodyMass: BodyVars = 5; -const BodyInvMass: BodyVars = 6; -const BodyBounce: BodyVars = 7; -const BodyFriction: BodyVars = 8; -const BodyPosX: BodyVars = 9; -const BodyPosY: BodyVars = 10; -const BodyPosZ: BodyVars = 11; -const BodyQuatX: BodyVars = 12; -const BodyQuatY: BodyVars = 13; -const BodyQuatZ: BodyVars = 14; -const BodyQuatW: BodyVars = 15; -const BodyComX: BodyVars = 16; -const BodyComY: BodyVars = 17; -const BodyComZ: BodyVars = 18; -const BodyInertiaXX: BodyVars = 19; -const BodyInertiaYX: BodyVars = 20; -const BodyInertiaZX: BodyVars = 21; -const BodyInertiaXY: BodyVars = 22; -const BodyInertiaYY: BodyVars = 23; -const BodyInertiaZY: BodyVars = 24; -const BodyInertiaXZ: BodyVars = 25; -const BodyInertiaYZ: BodyVars = 26; -const BodyInertiaZZ: BodyVars = 27; -const BodyInvInertiaXX: BodyVars = 28; -const BodyInvInertiaYX: BodyVars = 29; -const BodyInvInertiaZX: BodyVars = 30; -const BodyInvInertiaXY: BodyVars = 31; -const BodyInvInertiaYY: BodyVars = 32; -const BodyInvInertiaZY: BodyVars = 33; -const BodyInvInertiaXZ: BodyVars = 34; -const BodyInvInertiaYZ: BodyVars = 35; -const BodyInvInertiaZZ: BodyVars = 36; +const BodyDynamic: BodyVars = 1; +const BodyWorld: BodyVars = 2; +const BodyGroup: BodyVars = 3; +const BodySizeX: BodyVars = 4; +const BodySizeY: BodyVars = 5; +const BodySizeZ: BodyVars = 6; +const BodyMass: BodyVars = 7; +const BodyInvMass: BodyVars = 8; +const BodyBounce: BodyVars = 9; +const BodyFriction: BodyVars = 10; +const BodyPosX: BodyVars = 11; +const BodyPosY: BodyVars = 12; +const BodyPosZ: BodyVars = 13; +const BodyQuatX: BodyVars = 14; +const BodyQuatY: BodyVars = 15; +const BodyQuatZ: BodyVars = 16; +const BodyQuatW: BodyVars = 17; +const BodyComX: BodyVars = 18; +const BodyComY: BodyVars = 19; +const BodyComZ: BodyVars = 20; +const BodyInertiaXX: BodyVars = 21; +const BodyInertiaYX: BodyVars = 22; +const BodyInertiaZX: BodyVars = 23; +const BodyInertiaXY: BodyVars = 24; +const BodyInertiaYY: BodyVars = 25; +const BodyInertiaZY: BodyVars = 26; +const BodyInertiaXZ: BodyVars = 27; +const BodyInertiaYZ: BodyVars = 28; +const BodyInertiaZZ: BodyVars = 29; +const BodyInvInertiaXX: BodyVars = 30; +const BodyInvInertiaYX: BodyVars = 31; +const BodyInvInertiaZX: BodyVars = 32; +const BodyInvInertiaXY: BodyVars = 33; +const BodyInvInertiaYY: BodyVars = 34; +const BodyInvInertiaZY: BodyVars = 35; +const BodyInvInertiaXZ: BodyVars = 36; +const BodyInvInertiaYZ: BodyVars = 37; +const BodyInvInertiaZZ: BodyVars = 38; fn BodyCom(idx: i32) -> vec3 { return vec3(Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComX))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComY))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComZ))]); } @@ -90,13 +92,20 @@ fn BodyInvInertia(idx: i32) -> mat3x3f { alias ContactVars = i32; //enums:enum const ContactA: ContactVars = 0; const ContactB: ContactVars = 1; -const ContactNormX: ContactVars = 2; -const ContactNormY: ContactVars = 3; -const ContactNormZ: ContactVars = 4; -const ContactPointX: ContactVars = 5; -const ContactPointY: ContactVars = 6; -const ContactPointZ: ContactVars = 7; -const ContactDist: ContactVars = 8; +const ContactAPointX: ContactVars = 2; +const ContactAPointY: ContactVars = 3; +const ContactAPointZ: ContactVars = 4; +const ContactBPointX: ContactVars = 5; +const ContactBPointY: ContactVars = 6; +const ContactBPointZ: ContactVars = 7; +const ContactADepth: ContactVars = 8; +const ContactBDepth: ContactVars = 9; +const ContactNormX: ContactVars = 10; +const ContactNormY: ContactVars = 11; +const ContactNormZ: ContactVars = 12; +const ContactForceX: ContactVars = 13; +const ContactForceY: ContactVars = 14; +const ContactForceZ: ContactVars = 15; //////// import: "control.go" alias JointControlVars = i32; //enums:enum @@ -106,7 +115,7 @@ const JointTargetVel: JointControlVars = 2; //////// import: "dynamics.go" alias DynamicVars = i32; //enums:enum -const DynIndex: DynamicVars = 0; +const DynBody: DynamicVars = 0; const DynPosX: DynamicVars = 1; const DynPosY: DynamicVars = 2; const DynPosZ: DynamicVars = 3; @@ -138,49 +147,49 @@ const DynDeltaZ: DynamicVars = 28; const DynAngDeltaX: DynamicVars = 29; const DynAngDeltaY: DynamicVars = 30; const DynAngDeltaZ: DynamicVars = 31; -fn DynamicIndex(idx: i32,cni: i32) -> i32 { - return i32(bitcast(Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynIndex))])); +fn DynamicBody(idx: i32) -> i32 { + return i32(bitcast(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(0), u32(DynBody))])); } fn DynamicPos(idx: i32,cni: i32) -> vec3 { - return vec3(Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynPosX))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynPosY))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynPosZ))]); + return vec3(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosZ))]); } fn SetDynamicPos(idx: i32,cni: i32, pos: vec3) { - Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynPosX))] = pos.x; - Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynPosY))] = pos.y; - Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynPosZ))] = pos.z; + Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosX))] = pos.x; + Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosY))] = pos.y; + Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosZ))] = pos.z; } fn DynamicQuat(idx: i32,cni: i32) -> vec4 { - return vec4(Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynQuatX))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynQuatY))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynQuatZ))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynQuatW))]); + return vec4(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatZ))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatW))]); } fn SetDynamicQuat(idx: i32,cni: i32, rot: vec4) { - Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynQuatX))] = rot.x; - Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynQuatY))] = rot.y; - Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynQuatZ))] = rot.z; - Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynQuatW))] = rot.w; + Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatX))] = rot.x; + Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatY))] = rot.y; + Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatZ))] = rot.z; + Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatW))] = rot.w; } fn DynamicDelta(idx: i32,cni: i32) -> vec3 { - return vec3(Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynDeltaX))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynDeltaY))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynDeltaZ))]); + return vec3(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaZ))]); } fn SetDynamicDelta(idx: i32,cni: i32, delta: vec3) { - Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynDeltaX))] = delta.x; - Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynDeltaY))] = delta.y; - Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynDeltaZ))] = delta.z; + Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaX))] = delta.x; + Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaY))] = delta.y; + Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaZ))] = delta.z; } fn DynamicAngDelta(idx: i32,cni: i32) -> vec3 { - return vec3(Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynAngDeltaX))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynAngDeltaY))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynAngDeltaZ))]); + return vec3(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaZ))]); } fn SetDynamicAngDelta(idx: i32,cni: i32, angDelta: vec3) { - Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynAngDeltaX))] = angDelta.x; - Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynAngDeltaY))] = angDelta.y; - Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynAngDeltaZ))] = angDelta.z; + Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaX))] = angDelta.x; + Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaY))] = angDelta.y; + Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaZ))] = angDelta.z; } //////// import: "enumgen.go" -const BodyVarsN: BodyVars = 37; -const ContactVarsN: ContactVars = 9; +const BodyVarsN: BodyVars = 39; +const ContactVarsN: ContactVars = 16; const JointControlVarsN: JointControlVars = 3; const DynamicVarsN: DynamicVars = 32; -const GPUVarsN: GPUVars = 8; +const GPUVarsN: GPUVars = 9; const JointTypesN: JointTypes = 7; const JointVarsN: JointVars = 50; const JointDoFVarsN: JointDoFVars = 7; @@ -269,15 +278,15 @@ struct PhysParams { AngularDamping: f32, ContactWeighting: i32, Restitution: i32, + Cur: i32, + Next: i32, + BodiesN: i32, DynamicsN: i32, JointsN: i32, JointDoFsN: i32, BodyJointsMax: i32, - Cur: i32, - Next: i32, + BodyCollidePairsN: i32, pad: i32, - pad1: i32, - pad2: i32, Gravity: vec4, } @@ -295,7 +304,7 @@ fn QuatLength(q: vec4) -> f32 { return sqrt(q.x*q.x + q.y*q.y + q.z*q.z + q.w*q.w); } fn QuatNormalize(q: vec4) -> vec4 { - var nq: vec4; + var nq = q; var l = QuatLength(q); if (l == 0) { nq.x = f32(0); @@ -346,14 +355,14 @@ fn StepBodyDeltas(i: u32) { //gosl:kernel if (di >= params.DynamicsN) { return; } - var bi = DynamicIndex(di, params.Cur); + var bi = DynamicBody(di); var invMass = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyInvMass))]; if (invMass == 0) { return; // no updates } var inertia = BodyInertia(bi); var invInertia = BodyInvInertia(bi); - var p0 = DynamicPos(di, params.Next); + var r0 = DynamicPos(di, params.Next); var q0 = DynamicQuat(di, params.Next); var v0 = DynamicDelta(di, params.Next); var w0 = DynamicAngDelta(di, params.Next); @@ -367,7 +376,7 @@ fn StepBodyDeltas(i: u32) { //gosl:kernel var q1 = q0+(MulQuats(vec4(dw1.x, dw1.y, dw1.z, 0), q0)*(0.5 * params.Dt)); q1 = QuatNormalize(q1); var com = BodyCom(bi); - var pcom = MulQuatVector(q0, com)+(p0); + var pcom = MulQuatVector(q0, com)+(r0); var p1 = pcom+(dp*(params.Dt)); p1 = p1-(MulQuatVector(q1, com)); var v1 = v0+(dp); diff --git a/physics/shaders/StepIntegrateBodies.wgsl b/physics/shaders/StepIntegrateBodies.wgsl index 280220b1..d5403cf7 100644 --- a/physics/shaders/StepIntegrateBodies.wgsl +++ b/physics/shaders/StepIntegrateBodies.wgsl @@ -36,42 +36,44 @@ fn Index3D(s0: u32, s1: u32, s2: u32, i0: u32, i1: u32, i2: u32) -> u32 { //////// import: "body.go" alias BodyVars = i32; //enums:enum const BodyShape: BodyVars = 0; -const BodyWorldIndex: BodyVars = 1; -const BodySizeX: BodyVars = 2; -const BodySizeY: BodyVars = 3; -const BodySizeZ: BodyVars = 4; -const BodyMass: BodyVars = 5; -const BodyInvMass: BodyVars = 6; -const BodyBounce: BodyVars = 7; -const BodyFriction: BodyVars = 8; -const BodyPosX: BodyVars = 9; -const BodyPosY: BodyVars = 10; -const BodyPosZ: BodyVars = 11; -const BodyQuatX: BodyVars = 12; -const BodyQuatY: BodyVars = 13; -const BodyQuatZ: BodyVars = 14; -const BodyQuatW: BodyVars = 15; -const BodyComX: BodyVars = 16; -const BodyComY: BodyVars = 17; -const BodyComZ: BodyVars = 18; -const BodyInertiaXX: BodyVars = 19; -const BodyInertiaYX: BodyVars = 20; -const BodyInertiaZX: BodyVars = 21; -const BodyInertiaXY: BodyVars = 22; -const BodyInertiaYY: BodyVars = 23; -const BodyInertiaZY: BodyVars = 24; -const BodyInertiaXZ: BodyVars = 25; -const BodyInertiaYZ: BodyVars = 26; -const BodyInertiaZZ: BodyVars = 27; -const BodyInvInertiaXX: BodyVars = 28; -const BodyInvInertiaYX: BodyVars = 29; -const BodyInvInertiaZX: BodyVars = 30; -const BodyInvInertiaXY: BodyVars = 31; -const BodyInvInertiaYY: BodyVars = 32; -const BodyInvInertiaZY: BodyVars = 33; -const BodyInvInertiaXZ: BodyVars = 34; -const BodyInvInertiaYZ: BodyVars = 35; -const BodyInvInertiaZZ: BodyVars = 36; +const BodyDynamic: BodyVars = 1; +const BodyWorld: BodyVars = 2; +const BodyGroup: BodyVars = 3; +const BodySizeX: BodyVars = 4; +const BodySizeY: BodyVars = 5; +const BodySizeZ: BodyVars = 6; +const BodyMass: BodyVars = 7; +const BodyInvMass: BodyVars = 8; +const BodyBounce: BodyVars = 9; +const BodyFriction: BodyVars = 10; +const BodyPosX: BodyVars = 11; +const BodyPosY: BodyVars = 12; +const BodyPosZ: BodyVars = 13; +const BodyQuatX: BodyVars = 14; +const BodyQuatY: BodyVars = 15; +const BodyQuatZ: BodyVars = 16; +const BodyQuatW: BodyVars = 17; +const BodyComX: BodyVars = 18; +const BodyComY: BodyVars = 19; +const BodyComZ: BodyVars = 20; +const BodyInertiaXX: BodyVars = 21; +const BodyInertiaYX: BodyVars = 22; +const BodyInertiaZX: BodyVars = 23; +const BodyInertiaXY: BodyVars = 24; +const BodyInertiaYY: BodyVars = 25; +const BodyInertiaZY: BodyVars = 26; +const BodyInertiaXZ: BodyVars = 27; +const BodyInertiaYZ: BodyVars = 28; +const BodyInertiaZZ: BodyVars = 29; +const BodyInvInertiaXX: BodyVars = 30; +const BodyInvInertiaYX: BodyVars = 31; +const BodyInvInertiaZX: BodyVars = 32; +const BodyInvInertiaXY: BodyVars = 33; +const BodyInvInertiaYY: BodyVars = 34; +const BodyInvInertiaZY: BodyVars = 35; +const BodyInvInertiaXZ: BodyVars = 36; +const BodyInvInertiaYZ: BodyVars = 37; +const BodyInvInertiaZZ: BodyVars = 38; fn BodyCom(idx: i32) -> vec3 { return vec3(Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComX))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComY))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComZ))]); } @@ -90,13 +92,20 @@ fn BodyInvInertia(idx: i32) -> mat3x3f { alias ContactVars = i32; //enums:enum const ContactA: ContactVars = 0; const ContactB: ContactVars = 1; -const ContactNormX: ContactVars = 2; -const ContactNormY: ContactVars = 3; -const ContactNormZ: ContactVars = 4; -const ContactPointX: ContactVars = 5; -const ContactPointY: ContactVars = 6; -const ContactPointZ: ContactVars = 7; -const ContactDist: ContactVars = 8; +const ContactAPointX: ContactVars = 2; +const ContactAPointY: ContactVars = 3; +const ContactAPointZ: ContactVars = 4; +const ContactBPointX: ContactVars = 5; +const ContactBPointY: ContactVars = 6; +const ContactBPointZ: ContactVars = 7; +const ContactADepth: ContactVars = 8; +const ContactBDepth: ContactVars = 9; +const ContactNormX: ContactVars = 10; +const ContactNormY: ContactVars = 11; +const ContactNormZ: ContactVars = 12; +const ContactForceX: ContactVars = 13; +const ContactForceY: ContactVars = 14; +const ContactForceZ: ContactVars = 15; //////// import: "control.go" alias JointControlVars = i32; //enums:enum @@ -106,7 +115,7 @@ const JointTargetVel: JointControlVars = 2; //////// import: "dynamics.go" alias DynamicVars = i32; //enums:enum -const DynIndex: DynamicVars = 0; +const DynBody: DynamicVars = 0; const DynPosX: DynamicVars = 1; const DynPosY: DynamicVars = 2; const DynPosZ: DynamicVars = 3; @@ -138,55 +147,55 @@ const DynDeltaZ: DynamicVars = 28; const DynAngDeltaX: DynamicVars = 29; const DynAngDeltaY: DynamicVars = 30; const DynAngDeltaZ: DynamicVars = 31; -fn DynamicIndex(idx: i32,cni: i32) -> i32 { - return i32(bitcast(Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynIndex))])); +fn DynamicBody(idx: i32) -> i32 { + return i32(bitcast(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(0), u32(DynBody))])); } fn DynamicPos(idx: i32,cni: i32) -> vec3 { - return vec3(Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynPosX))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynPosY))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynPosZ))]); + return vec3(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosZ))]); } fn SetDynamicPos(idx: i32,cni: i32, pos: vec3) { - Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynPosX))] = pos.x; - Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynPosY))] = pos.y; - Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynPosZ))] = pos.z; + Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosX))] = pos.x; + Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosY))] = pos.y; + Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosZ))] = pos.z; } fn DynamicQuat(idx: i32,cni: i32) -> vec4 { - return vec4(Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynQuatX))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynQuatY))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynQuatZ))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynQuatW))]); + return vec4(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatZ))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatW))]); } fn SetDynamicQuat(idx: i32,cni: i32, rot: vec4) { - Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynQuatX))] = rot.x; - Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynQuatY))] = rot.y; - Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynQuatZ))] = rot.z; - Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynQuatW))] = rot.w; + Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatX))] = rot.x; + Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatY))] = rot.y; + Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatZ))] = rot.z; + Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatW))] = rot.w; } fn DynamicForce(idx: i32,cni: i32) -> vec3 { - return vec3(Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynForceX))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynForceY))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynForceZ))]); + return vec3(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynForceX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynForceY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynForceZ))]); } fn DynamicTorque(idx: i32,cni: i32) -> vec3 { - return vec3(Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynTorqueX))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynTorqueY))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynTorqueZ))]); + return vec3(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynTorqueX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynTorqueY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynTorqueZ))]); } fn DynamicDelta(idx: i32,cni: i32) -> vec3 { - return vec3(Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynDeltaX))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynDeltaY))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynDeltaZ))]); + return vec3(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaZ))]); } fn SetDynamicDelta(idx: i32,cni: i32, delta: vec3) { - Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynDeltaX))] = delta.x; - Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynDeltaY))] = delta.y; - Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynDeltaZ))] = delta.z; + Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaX))] = delta.x; + Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaY))] = delta.y; + Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaZ))] = delta.z; } fn DynamicAngDelta(idx: i32,cni: i32) -> vec3 { - return vec3(Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynAngDeltaX))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynAngDeltaY))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynAngDeltaZ))]); + return vec3(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaZ))]); } fn SetDynamicAngDelta(idx: i32,cni: i32, angDelta: vec3) { - Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynAngDeltaX))] = angDelta.x; - Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynAngDeltaY))] = angDelta.y; - Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynAngDeltaZ))] = angDelta.z; + Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaX))] = angDelta.x; + Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaY))] = angDelta.y; + Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaZ))] = angDelta.z; } //////// import: "enumgen.go" -const BodyVarsN: BodyVars = 37; -const ContactVarsN: ContactVars = 9; +const BodyVarsN: BodyVars = 39; +const ContactVarsN: ContactVars = 16; const JointControlVarsN: JointControlVars = 3; const DynamicVarsN: DynamicVars = 32; -const GPUVarsN: GPUVars = 8; +const GPUVarsN: GPUVars = 9; const JointTypesN: JointTypes = 7; const JointVarsN: JointVars = 50; const JointDoFVarsN: JointDoFVars = 7; @@ -275,15 +284,15 @@ struct PhysParams { AngularDamping: f32, ContactWeighting: i32, Restitution: i32, + Cur: i32, + Next: i32, + BodiesN: i32, DynamicsN: i32, JointsN: i32, JointDoFsN: i32, BodyJointsMax: i32, - Cur: i32, - Next: i32, + BodyCollidePairsN: i32, pad: i32, - pad1: i32, - pad2: i32, Gravity: vec4, } @@ -301,7 +310,7 @@ fn QuatLength(q: vec4) -> f32 { return sqrt(q.x*q.x + q.y*q.y + q.z*q.z + q.w*q.w); } fn QuatNormalize(q: vec4) -> vec4 { - var nq: vec4; + var nq = q; var l = QuatLength(q); if (l == 0) { nq.x = f32(0); @@ -331,6 +340,13 @@ fn MulQuats(a: vec4,b: vec4) -> vec4 { q.z = a.z*b.w + a.w*b.z + a.x*b.y - a.y*b.x; q.w = a.w*b.w - a.x*b.x - a.y*b.y - a.z*b.z;return q; } +fn QuatAdd(q: vec4, o: vec4) -> vec4 { + var nq = q; + nq.x += o.x; + nq.y += o.y; + nq.z += o.z; + nq.w += o.w;return nq; +} //////// import: "slmath-vector3.go" fn MulScalar3(v: vec3, s: f32) -> vec3 { @@ -354,24 +370,24 @@ fn StepIntegrateBodies(i: u32) { //gosl:kernel if (di >= params.DynamicsN) { return; } - var bi = DynamicIndex(di, params.Cur); + var bi = DynamicBody(di); var invMass = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyInvMass))]; var inertia = BodyInertia(bi); var invInertia = BodyInvInertia(bi); var com = BodyCom(bi); - var p0 = DynamicPos(di, params.Cur); + var r0 = DynamicPos(di, params.Cur); var q0 = DynamicQuat(di, params.Cur); var v0 = DynamicDelta(di, params.Cur); var w0 = DynamicAngDelta(di, params.Cur); var f0 = DynamicForce(di, params.Next); var t0 = DynamicTorque(di, params.Next); - var pcom = MulQuatVector(q0, com)+(p0); + var pcom = MulQuatVector(q0, com)+(r0); var v1 = v0+(f0*(invMass)+(vec3(params.Gravity.x,params.Gravity.y,params.Gravity.z)*(OneIfNonzero(invMass)))*(params.Dt)); var p1 = pcom+(v1*(params.Dt)); var wb = MulQuatVectorInverse(q0, w0); var tb = MulQuatVectorInverse(q0, t0)-(Cross3(wb, inertia*(wb))); // coriolis forces var w1 = MulQuatVector(q0, wb+(invInertia*(tb)*(params.Dt))); - var q1 = MulQuats(vec4(w1.x, w1.y, w1.z, 0), q0)*(0.5 * params.Dt); + var q1 = QuatAdd(q0, MulQuats(vec4(w1.x, w1.y, w1.z, 0), q0)*(0.5*params.Dt)); q1 = QuatNormalize(q1); w1 = w1*(1.0 - params.AngularDamping*params.Dt); var p1a = p1-(MulQuatVector(q1, com)); // pos corrected to nominal center. diff --git a/physics/shaders/StepJointForces.wgsl b/physics/shaders/StepJointForces.wgsl index 41f148ee..1b560b30 100644 --- a/physics/shaders/StepJointForces.wgsl +++ b/physics/shaders/StepJointForces.wgsl @@ -42,42 +42,44 @@ fn Index3D(s0: u32, s1: u32, s2: u32, i0: u32, i1: u32, i2: u32) -> u32 { //////// import: "body.go" alias BodyVars = i32; //enums:enum const BodyShape: BodyVars = 0; -const BodyWorldIndex: BodyVars = 1; -const BodySizeX: BodyVars = 2; -const BodySizeY: BodyVars = 3; -const BodySizeZ: BodyVars = 4; -const BodyMass: BodyVars = 5; -const BodyInvMass: BodyVars = 6; -const BodyBounce: BodyVars = 7; -const BodyFriction: BodyVars = 8; -const BodyPosX: BodyVars = 9; -const BodyPosY: BodyVars = 10; -const BodyPosZ: BodyVars = 11; -const BodyQuatX: BodyVars = 12; -const BodyQuatY: BodyVars = 13; -const BodyQuatZ: BodyVars = 14; -const BodyQuatW: BodyVars = 15; -const BodyComX: BodyVars = 16; -const BodyComY: BodyVars = 17; -const BodyComZ: BodyVars = 18; -const BodyInertiaXX: BodyVars = 19; -const BodyInertiaYX: BodyVars = 20; -const BodyInertiaZX: BodyVars = 21; -const BodyInertiaXY: BodyVars = 22; -const BodyInertiaYY: BodyVars = 23; -const BodyInertiaZY: BodyVars = 24; -const BodyInertiaXZ: BodyVars = 25; -const BodyInertiaYZ: BodyVars = 26; -const BodyInertiaZZ: BodyVars = 27; -const BodyInvInertiaXX: BodyVars = 28; -const BodyInvInertiaYX: BodyVars = 29; -const BodyInvInertiaZX: BodyVars = 30; -const BodyInvInertiaXY: BodyVars = 31; -const BodyInvInertiaYY: BodyVars = 32; -const BodyInvInertiaZY: BodyVars = 33; -const BodyInvInertiaXZ: BodyVars = 34; -const BodyInvInertiaYZ: BodyVars = 35; -const BodyInvInertiaZZ: BodyVars = 36; +const BodyDynamic: BodyVars = 1; +const BodyWorld: BodyVars = 2; +const BodyGroup: BodyVars = 3; +const BodySizeX: BodyVars = 4; +const BodySizeY: BodyVars = 5; +const BodySizeZ: BodyVars = 6; +const BodyMass: BodyVars = 7; +const BodyInvMass: BodyVars = 8; +const BodyBounce: BodyVars = 9; +const BodyFriction: BodyVars = 10; +const BodyPosX: BodyVars = 11; +const BodyPosY: BodyVars = 12; +const BodyPosZ: BodyVars = 13; +const BodyQuatX: BodyVars = 14; +const BodyQuatY: BodyVars = 15; +const BodyQuatZ: BodyVars = 16; +const BodyQuatW: BodyVars = 17; +const BodyComX: BodyVars = 18; +const BodyComY: BodyVars = 19; +const BodyComZ: BodyVars = 20; +const BodyInertiaXX: BodyVars = 21; +const BodyInertiaYX: BodyVars = 22; +const BodyInertiaZX: BodyVars = 23; +const BodyInertiaXY: BodyVars = 24; +const BodyInertiaYY: BodyVars = 25; +const BodyInertiaZY: BodyVars = 26; +const BodyInertiaXZ: BodyVars = 27; +const BodyInertiaYZ: BodyVars = 28; +const BodyInertiaZZ: BodyVars = 29; +const BodyInvInertiaXX: BodyVars = 30; +const BodyInvInertiaYX: BodyVars = 31; +const BodyInvInertiaZX: BodyVars = 32; +const BodyInvInertiaXY: BodyVars = 33; +const BodyInvInertiaYY: BodyVars = 34; +const BodyInvInertiaZY: BodyVars = 35; +const BodyInvInertiaXZ: BodyVars = 36; +const BodyInvInertiaYZ: BodyVars = 37; +const BodyInvInertiaZZ: BodyVars = 38; fn BodyCom(idx: i32) -> vec3 { return vec3(Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComX))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComY))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComZ))]); } @@ -86,13 +88,20 @@ fn BodyCom(idx: i32) -> vec3 { alias ContactVars = i32; //enums:enum const ContactA: ContactVars = 0; const ContactB: ContactVars = 1; -const ContactNormX: ContactVars = 2; -const ContactNormY: ContactVars = 3; -const ContactNormZ: ContactVars = 4; -const ContactPointX: ContactVars = 5; -const ContactPointY: ContactVars = 6; -const ContactPointZ: ContactVars = 7; -const ContactDist: ContactVars = 8; +const ContactAPointX: ContactVars = 2; +const ContactAPointY: ContactVars = 3; +const ContactAPointZ: ContactVars = 4; +const ContactBPointX: ContactVars = 5; +const ContactBPointY: ContactVars = 6; +const ContactBPointZ: ContactVars = 7; +const ContactADepth: ContactVars = 8; +const ContactBDepth: ContactVars = 9; +const ContactNormX: ContactVars = 10; +const ContactNormY: ContactVars = 11; +const ContactNormZ: ContactVars = 12; +const ContactForceX: ContactVars = 13; +const ContactForceY: ContactVars = 14; +const ContactForceZ: ContactVars = 15; //////// import: "control.go" alias JointControlVars = i32; //enums:enum @@ -100,12 +109,12 @@ const JointControlForce: JointControlVars = 0; const JointTargetPos: JointControlVars = 1; const JointTargetVel: JointControlVars = 2; fn JointControl(idx: i32,dof: i32, vr: JointControlVars) -> f32 { - return JointControls[Index2D(TensorStrides[60], TensorStrides[61], u32(JointDoFIndex(idx, dof)), u32(vr))]; + return JointControls[Index2D(TensorStrides[70], TensorStrides[71], u32(JointDoFIndex(idx, dof)), u32(vr))]; } //////// import: "dynamics.go" alias DynamicVars = i32; //enums:enum -const DynIndex: DynamicVars = 0; +const DynBody: DynamicVars = 0; const DynPosX: DynamicVars = 1; const DynPosY: DynamicVars = 2; const DynPosZ: DynamicVars = 3; @@ -137,22 +146,22 @@ const DynDeltaZ: DynamicVars = 28; const DynAngDeltaX: DynamicVars = 29; const DynAngDeltaY: DynamicVars = 30; const DynAngDeltaZ: DynamicVars = 31; -fn DynamicIndex(idx: i32,cni: i32) -> i32 { - return i32(bitcast(Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynIndex))])); +fn DynamicBody(idx: i32) -> i32 { + return i32(bitcast(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(0), u32(DynBody))])); } fn DynamicPos(idx: i32,cni: i32) -> vec3 { - return vec3(Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynPosX))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynPosY))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynPosZ))]); + return vec3(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosZ))]); } fn DynamicQuat(idx: i32,cni: i32) -> vec4 { - return vec4(Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynQuatX))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynQuatY))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynQuatZ))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynQuatW))]); + return vec4(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatZ))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatW))]); } //////// import: "enumgen.go" -const BodyVarsN: BodyVars = 37; -const ContactVarsN: ContactVars = 9; +const BodyVarsN: BodyVars = 39; +const ContactVarsN: ContactVars = 16; const JointControlVarsN: JointControlVars = 3; const DynamicVarsN: DynamicVars = 32; -const GPUVarsN: GPUVars = 8; +const GPUVarsN: GPUVars = 9; const JointTypesN: JointTypes = 7; const JointVarsN: JointVars = 50; const JointDoFVarsN: JointDoFVars = 7; @@ -294,15 +303,15 @@ struct PhysParams { AngularDamping: f32, ContactWeighting: i32, Restitution: i32, + Cur: i32, + Next: i32, + BodiesN: i32, DynamicsN: i32, JointsN: i32, JointDoFsN: i32, BodyJointsMax: i32, - Cur: i32, - Next: i32, + BodyCollidePairsN: i32, pad: i32, - pad1: i32, - pad2: i32, Gravity: vec4, } @@ -361,10 +370,10 @@ fn StepJointForces(i: u32) { //gosl:kernel var jPi = JointParentIndex(ji); var jPbi = i32(-1); if (jPi >= 0) { - jPbi = DynamicIndex(jPi, params.Cur); + jPbi = DynamicBody(jPi); } var jCi = JointChildIndex(ji); - var jCbi = DynamicIndex(jCi, params.Cur); + var jCbi = DynamicBody(jCi); var jLinearN = GetJointLinearDoFN(ji); var jAngularN = GetJointAngularDoFN(ji); var jPR = JointPPos(ji); diff --git a/physics/shaders/StepSolveJoints.wgsl b/physics/shaders/StepSolveJoints.wgsl index 269cdc1c..6383b6ea 100644 --- a/physics/shaders/StepSolveJoints.wgsl +++ b/physics/shaders/StepSolveJoints.wgsl @@ -42,42 +42,44 @@ fn Index3D(s0: u32, s1: u32, s2: u32, i0: u32, i1: u32, i2: u32) -> u32 { //////// import: "body.go" alias BodyVars = i32; //enums:enum const BodyShape: BodyVars = 0; -const BodyWorldIndex: BodyVars = 1; -const BodySizeX: BodyVars = 2; -const BodySizeY: BodyVars = 3; -const BodySizeZ: BodyVars = 4; -const BodyMass: BodyVars = 5; -const BodyInvMass: BodyVars = 6; -const BodyBounce: BodyVars = 7; -const BodyFriction: BodyVars = 8; -const BodyPosX: BodyVars = 9; -const BodyPosY: BodyVars = 10; -const BodyPosZ: BodyVars = 11; -const BodyQuatX: BodyVars = 12; -const BodyQuatY: BodyVars = 13; -const BodyQuatZ: BodyVars = 14; -const BodyQuatW: BodyVars = 15; -const BodyComX: BodyVars = 16; -const BodyComY: BodyVars = 17; -const BodyComZ: BodyVars = 18; -const BodyInertiaXX: BodyVars = 19; -const BodyInertiaYX: BodyVars = 20; -const BodyInertiaZX: BodyVars = 21; -const BodyInertiaXY: BodyVars = 22; -const BodyInertiaYY: BodyVars = 23; -const BodyInertiaZY: BodyVars = 24; -const BodyInertiaXZ: BodyVars = 25; -const BodyInertiaYZ: BodyVars = 26; -const BodyInertiaZZ: BodyVars = 27; -const BodyInvInertiaXX: BodyVars = 28; -const BodyInvInertiaYX: BodyVars = 29; -const BodyInvInertiaZX: BodyVars = 30; -const BodyInvInertiaXY: BodyVars = 31; -const BodyInvInertiaYY: BodyVars = 32; -const BodyInvInertiaZY: BodyVars = 33; -const BodyInvInertiaXZ: BodyVars = 34; -const BodyInvInertiaYZ: BodyVars = 35; -const BodyInvInertiaZZ: BodyVars = 36; +const BodyDynamic: BodyVars = 1; +const BodyWorld: BodyVars = 2; +const BodyGroup: BodyVars = 3; +const BodySizeX: BodyVars = 4; +const BodySizeY: BodyVars = 5; +const BodySizeZ: BodyVars = 6; +const BodyMass: BodyVars = 7; +const BodyInvMass: BodyVars = 8; +const BodyBounce: BodyVars = 9; +const BodyFriction: BodyVars = 10; +const BodyPosX: BodyVars = 11; +const BodyPosY: BodyVars = 12; +const BodyPosZ: BodyVars = 13; +const BodyQuatX: BodyVars = 14; +const BodyQuatY: BodyVars = 15; +const BodyQuatZ: BodyVars = 16; +const BodyQuatW: BodyVars = 17; +const BodyComX: BodyVars = 18; +const BodyComY: BodyVars = 19; +const BodyComZ: BodyVars = 20; +const BodyInertiaXX: BodyVars = 21; +const BodyInertiaYX: BodyVars = 22; +const BodyInertiaZX: BodyVars = 23; +const BodyInertiaXY: BodyVars = 24; +const BodyInertiaYY: BodyVars = 25; +const BodyInertiaZY: BodyVars = 26; +const BodyInertiaXZ: BodyVars = 27; +const BodyInertiaYZ: BodyVars = 28; +const BodyInertiaZZ: BodyVars = 29; +const BodyInvInertiaXX: BodyVars = 30; +const BodyInvInertiaYX: BodyVars = 31; +const BodyInvInertiaZX: BodyVars = 32; +const BodyInvInertiaXY: BodyVars = 33; +const BodyInvInertiaYY: BodyVars = 34; +const BodyInvInertiaZY: BodyVars = 35; +const BodyInvInertiaXZ: BodyVars = 36; +const BodyInvInertiaYZ: BodyVars = 37; +const BodyInvInertiaZZ: BodyVars = 38; fn BodyCom(idx: i32) -> vec3 { return vec3(Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComX))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComY))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComZ))]); } @@ -91,13 +93,20 @@ fn BodyInvInertia(idx: i32) -> mat3x3f { alias ContactVars = i32; //enums:enum const ContactA: ContactVars = 0; const ContactB: ContactVars = 1; -const ContactNormX: ContactVars = 2; -const ContactNormY: ContactVars = 3; -const ContactNormZ: ContactVars = 4; -const ContactPointX: ContactVars = 5; -const ContactPointY: ContactVars = 6; -const ContactPointZ: ContactVars = 7; -const ContactDist: ContactVars = 8; +const ContactAPointX: ContactVars = 2; +const ContactAPointY: ContactVars = 3; +const ContactAPointZ: ContactVars = 4; +const ContactBPointX: ContactVars = 5; +const ContactBPointY: ContactVars = 6; +const ContactBPointZ: ContactVars = 7; +const ContactADepth: ContactVars = 8; +const ContactBDepth: ContactVars = 9; +const ContactNormX: ContactVars = 10; +const ContactNormY: ContactVars = 11; +const ContactNormZ: ContactVars = 12; +const ContactForceX: ContactVars = 13; +const ContactForceY: ContactVars = 14; +const ContactForceZ: ContactVars = 15; //////// import: "control.go" alias JointControlVars = i32; //enums:enum @@ -105,12 +114,12 @@ const JointControlForce: JointControlVars = 0; const JointTargetPos: JointControlVars = 1; const JointTargetVel: JointControlVars = 2; fn JointControl(idx: i32,dof: i32, vr: JointControlVars) -> f32 { - return JointControls[Index2D(TensorStrides[60], TensorStrides[61], u32(JointDoFIndex(idx, dof)), u32(vr))]; + return JointControls[Index2D(TensorStrides[70], TensorStrides[71], u32(JointDoFIndex(idx, dof)), u32(vr))]; } //////// import: "dynamics.go" alias DynamicVars = i32; //enums:enum -const DynIndex: DynamicVars = 0; +const DynBody: DynamicVars = 0; const DynPosX: DynamicVars = 1; const DynPosY: DynamicVars = 2; const DynPosZ: DynamicVars = 3; @@ -142,28 +151,28 @@ const DynDeltaZ: DynamicVars = 28; const DynAngDeltaX: DynamicVars = 29; const DynAngDeltaY: DynamicVars = 30; const DynAngDeltaZ: DynamicVars = 31; -fn DynamicIndex(idx: i32,cni: i32) -> i32 { - return i32(bitcast(Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynIndex))])); +fn DynamicBody(idx: i32) -> i32 { + return i32(bitcast(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(0), u32(DynBody))])); } fn DynamicPos(idx: i32,cni: i32) -> vec3 { - return vec3(Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynPosX))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynPosY))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynPosZ))]); + return vec3(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosZ))]); } fn DynamicQuat(idx: i32,cni: i32) -> vec4 { - return vec4(Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynQuatX))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynQuatY))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynQuatZ))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynQuatW))]); + return vec4(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatZ))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatW))]); } fn DynamicDelta(idx: i32,cni: i32) -> vec3 { - return vec3(Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynDeltaX))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynDeltaY))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynDeltaZ))]); + return vec3(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaZ))]); } fn DynamicAngDelta(idx: i32,cni: i32) -> vec3 { - return vec3(Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynAngDeltaX))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynAngDeltaY))], Dynamics[Index3D(TensorStrides[40], TensorStrides[41], TensorStrides[42], u32(idx), u32(cni), u32(DynAngDeltaZ))]); + return vec3(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaZ))]); } //////// import: "enumgen.go" -const BodyVarsN: BodyVars = 37; -const ContactVarsN: ContactVars = 9; +const BodyVarsN: BodyVars = 39; +const ContactVarsN: ContactVars = 16; const JointControlVarsN: JointControlVars = 3; const DynamicVarsN: DynamicVars = 32; -const GPUVarsN: GPUVars = 8; +const GPUVarsN: GPUVars = 9; const JointTypesN: JointTypes = 7; const JointVarsN: JointVars = 50; const JointDoFVarsN: JointDoFVars = 7; @@ -314,15 +323,15 @@ struct PhysParams { AngularDamping: f32, ContactWeighting: i32, Restitution: i32, + Cur: i32, + Next: i32, + BodiesN: i32, DynamicsN: i32, JointsN: i32, JointDoFsN: i32, BodyJointsMax: i32, - Cur: i32, - Next: i32, + BodyCollidePairsN: i32, pad: i32, - pad1: i32, - pad2: i32, Gravity: vec4, } @@ -340,7 +349,7 @@ fn QuatLength(q: vec4) -> f32 { return sqrt(q.x*q.x + q.y*q.y + q.z*q.z + q.w*q.w); } fn QuatNormalize(q: vec4) -> vec4 { - var nq: vec4; + var nq = q; var l = QuatLength(q); if (l == 0) { nq.x = f32(0); @@ -484,10 +493,10 @@ fn StepSolveJoints(i: u32) { //gosl:kernel var jPi = JointParentIndex(ji); var jPbi = i32(-1); if (jPi >= 0) { - jPbi = DynamicIndex(jPi, params.Cur); + jPbi = DynamicBody(jPi); } var jCi = JointChildIndex(ji); - var jCbi = DynamicIndex(jCi, params.Cur); + var jCbi = DynamicBody(jCi); var jLinearN = GetJointLinearDoFN(ji); var jAngularN = GetJointAngularDoFN(ji); var jPR = JointPPos(ji); @@ -574,7 +583,7 @@ fn StepSolveJoints(i: u32) { //gosl:kernel linDeltaC = linDeltaC+(linearC*(dLambda * params.JointLinearRelax)); angDeltaC = angDeltaC+(angularC*(dLambda * params.JointAngularRelax)); } - } else { // compute joint target, stiffness, damping + } else if (jLinearN > 0) { // compute joint target, stiffness, damping var axisLimitsD: vec3; var axisLimitsA: vec3; var axisTargetPosKeD: vec3; @@ -647,7 +656,7 @@ fn StepSolveJoints(i: u32) { //gosl:kernel } } } - if (jt == Fixed || jt == Prismatic || jt == Revolute || jt == D6) { // angular + if (jAngularN > 0) { // angular var qP = xwPQ; var qC = xwCQ; if (QuatDot(qP, qC) < 0) { @@ -728,7 +737,8 @@ fn StepSolveJoints(i: u32) { //gosl:kernel var compliance = params.JointLinearComply; var damping = f32(0.0); var targetVel = Dim3(axisTargetVelKdD, dim); - var derrRel = derr - targetVel; + var angularClen = Length3(angularC); + var derrRel = derr - targetVel*angularClen; var lower = Dim3(axisLimitsLower, dim); var upper = Dim3(axisLimitsUpper, dim); if (e < lower) { diff --git a/physics/step_body.go b/physics/step_body.go index 8084116a..7faabd46 100644 --- a/physics/step_body.go +++ b/physics/step_body.go @@ -26,7 +26,7 @@ func InitDynamics(i uint32) { //gosl:kernel return } for cni := range 2 { - bi := DynamicIndex(ii, int32(cni)) + bi := DynamicBody(ii) Dynamics.Set(Bodies.Value(int(bi), int(BodyPosX)), int(ii), int(cni), int(DynPosX)) Dynamics.Set(Bodies.Value(int(bi), int(BodyPosY)), int(ii), int(cni), int(DynPosY)) Dynamics.Set(Bodies.Value(int(bi), int(BodyPosZ)), int(ii), int(cni), int(DynPosZ)) @@ -49,7 +49,7 @@ func DynamicsCurToNext(i uint32) { //gosl:kernel if ii >= params.DynamicsN { return } - for di := DynIndex; di < DynamicVarsN; di++ { + for di := DynBody; di < DynamicVarsN; di++ { Dynamics.Set(Dynamics.Value(int(ii), int(params.Cur), int(di)), int(ii), int(params.Next), int(di)) } } @@ -121,7 +121,7 @@ func StepIntegrateBodies(i uint32) { //gosl:kernel if di >= params.DynamicsN { return } - bi := DynamicIndex(di, params.Cur) + bi := DynamicBody(di) invMass := Bodies.Value(int(bi), int(BodyInvMass)) inertia := BodyInertia(bi) @@ -130,7 +130,7 @@ func StepIntegrateBodies(i uint32) { //gosl:kernel com := BodyCom(bi) // current pos - p0 := DynamicPos(di, params.Cur) + r0 := DynamicPos(di, params.Cur) q0 := DynamicQuat(di, params.Cur) // current deltas @@ -141,7 +141,7 @@ func StepIntegrateBodies(i uint32) { //gosl:kernel f0 := DynamicForce(di, params.Next) t0 := DynamicTorque(di, params.Next) - pcom := slmath.MulQuatVector(q0, com).Add(p0) + pcom := slmath.MulQuatVector(q0, com).Add(r0) // linear part v1 := v0.Add(f0.MulScalar(invMass).Add(params.Gravity.V().MulScalar(OneIfNonzero(invMass))).MulScalar(params.Dt)) @@ -173,7 +173,7 @@ func StepBodyDeltas(i uint32) { //gosl:kernel if di >= params.DynamicsN { return } - bi := DynamicIndex(di, params.Cur) + bi := DynamicBody(di) invMass := Bodies.Value(int(bi), int(BodyInvMass)) if invMass == 0 { @@ -183,7 +183,7 @@ func StepBodyDeltas(i uint32) { //gosl:kernel invInertia := BodyInvInertia(bi) // starting pos (from force integration) - p0 := DynamicPos(di, params.Next) + r0 := DynamicPos(di, params.Next) q0 := DynamicQuat(di, params.Next) // starting deltas @@ -213,7 +213,7 @@ func StepBodyDeltas(i uint32) { //gosl:kernel // update position com := BodyCom(bi) - pcom := slmath.MulQuatVector(q0, com).Add(p0) + pcom := slmath.MulQuatVector(q0, com).Add(r0) p1 := pcom.Add(dp.MulScalar(params.Dt)) p1 = p1.Sub(slmath.MulQuatVector(q1, com)) diff --git a/physics/step_body.goal b/physics/step_body.goal index ec6f95b7..1dfe7fe7 100644 --- a/physics/step_body.goal +++ b/physics/step_body.goal @@ -24,7 +24,7 @@ func InitDynamics(i uint32) { //gosl:kernel return } for cni := range 2 { - bi := DynamicIndex(ii, int32(cni)) + bi := DynamicBody(ii) Dynamics[ii, cni, DynPosX] = Bodies[bi, BodyPosX] Dynamics[ii, cni, DynPosY] = Bodies[bi, BodyPosY] Dynamics[ii, cni, DynPosZ] = Bodies[bi, BodyPosZ] @@ -47,7 +47,7 @@ func DynamicsCurToNext(i uint32) { //gosl:kernel if ii >= params.DynamicsN { return } - for di := DynIndex; di < DynamicVarsN; di++ { + for di := DynBody; di < DynamicVarsN; di++ { Dynamics[ii, params.Next, di] = Dynamics[ii, params.Cur, di] } } @@ -119,7 +119,7 @@ func StepIntegrateBodies(i uint32) { //gosl:kernel if di >= params.DynamicsN { return } - bi := DynamicIndex(di, params.Cur) + bi := DynamicBody(di) invMass := Bodies[bi, BodyInvMass] inertia := BodyInertia(bi) @@ -128,7 +128,7 @@ func StepIntegrateBodies(i uint32) { //gosl:kernel com := BodyCom(bi) // current pos - p0 := DynamicPos(di, params.Cur) + r0 := DynamicPos(di, params.Cur) q0 := DynamicQuat(di, params.Cur) // current deltas @@ -139,7 +139,7 @@ func StepIntegrateBodies(i uint32) { //gosl:kernel f0 := DynamicForce(di, params.Next) t0 := DynamicTorque(di, params.Next) - pcom := slmath.MulQuatVector(q0, com).Add(p0) + pcom := slmath.MulQuatVector(q0, com).Add(r0) // linear part v1 := v0.Add(f0.MulScalar(invMass).Add(params.Gravity.V().MulScalar(OneIfNonzero(invMass))).MulScalar(params.Dt)) @@ -171,7 +171,7 @@ func StepBodyDeltas(i uint32) { //gosl:kernel if di >= params.DynamicsN { return } - bi := DynamicIndex(di, params.Cur) + bi := DynamicBody(di) invMass := Bodies[bi, BodyInvMass] if invMass == 0 { @@ -181,7 +181,7 @@ func StepBodyDeltas(i uint32) { //gosl:kernel invInertia := BodyInvInertia(bi) // starting pos (from force integration) - p0 := DynamicPos(di, params.Next) + r0 := DynamicPos(di, params.Next) q0 := DynamicQuat(di, params.Next) // starting deltas @@ -211,7 +211,7 @@ func StepBodyDeltas(i uint32) { //gosl:kernel // update position com := BodyCom(bi) - pcom := slmath.MulQuatVector(q0, com).Add(p0) + pcom := slmath.MulQuatVector(q0, com).Add(r0) p1 := pcom.Add(dp.MulScalar(params.Dt)) p1 = p1.Sub(slmath.MulQuatVector(q1, com)) diff --git a/physics/step_joint.go b/physics/step_joint.go index 60737227..2c59c2ef 100644 --- a/physics/step_joint.go +++ b/physics/step_joint.go @@ -14,6 +14,10 @@ import ( "cogentcore.org/lab/gosl/slmath" ) +// todo: joint types in solve -- seems wrong +// see if does anything when dofs are empty +// limits, damping, etc in test. + // notation convention: // spatial transform: R = position, Q = quat rotation // P = parent, C = child @@ -38,10 +42,10 @@ func StepJointForces(i uint32) { //gosl:kernel jPi := JointParentIndex(ji) jPbi := int32(-1) if jPi >= 0 { - jPbi = DynamicIndex(jPi, params.Cur) + jPbi = DynamicBody(jPi) } jCi := JointChildIndex(ji) - jCbi := DynamicIndex(jCi, params.Cur) + jCbi := DynamicBody(jCi) jLinearN := GetJointLinearDoFN(ji) jAngularN := GetJointAngularDoFN(ji) @@ -118,10 +122,10 @@ func StepSolveJoints(i uint32) { //gosl:kernel jPi := JointParentIndex(ji) jPbi := int32(-1) if jPi >= 0 { - jPbi = DynamicIndex(jPi, params.Cur) + jPbi = DynamicBody(jPi) } jCi := JointChildIndex(ji) - jCbi := DynamicIndex(jCi, params.Cur) + jCbi := DynamicBody(jCi) jLinearN := GetJointLinearDoFN(ji) jAngularN := GetJointAngularDoFN(ji) @@ -220,7 +224,7 @@ func StepSolveJoints(i uint32) { //gosl:kernel linDeltaC = linDeltaC.Add(linearC.MulScalar(dLambda * params.JointLinearRelax)) angDeltaC = angDeltaC.Add(angularC.MulScalar(dLambda * params.JointAngularRelax)) } - } else { // compute joint target, stiffness, damping + } else if jLinearN > 0 { // compute joint target, stiffness, damping var axisLimitsD, axisLimitsA math32.Vector3 var axisTargetPosKeD, axisTargetPosKeA math32.Vector3 var axisTargetVelKdD, axisTargetVelKdA math32.Vector3 @@ -304,8 +308,7 @@ func StepSolveJoints(i uint32) { //gosl:kernel } } } - // todo: does it make sense to have prismatic, fixed here? - if jt == Fixed || jt == Prismatic || jt == Revolute || jt == D6 { // angular + if jAngularN > 0 { // angular qP := xwPQ qC := xwCQ // make quats lie in same hemisphere @@ -407,7 +410,8 @@ func StepSolveJoints(i uint32) { //gosl:kernel damping := float32(0.0) targetVel := slmath.Dim3(axisTargetVelKdD, dim) - derrRel := derr - targetVel + angularClen := slmath.Length3(angularC) + derrRel := derr - targetVel*angularClen // consider joint limits irrespective of axis mode lower := slmath.Dim3(axisLimitsLower, dim) diff --git a/physics/step_joint.goal b/physics/step_joint.goal index 547cf47c..10e92543 100644 --- a/physics/step_joint.goal +++ b/physics/step_joint.goal @@ -12,6 +12,10 @@ import ( "cogentcore.org/lab/gosl/slmath" ) +// todo: joint types in solve -- seems wrong +// see if does anything when dofs are empty +// limits, damping, etc in test. + // notation convention: // spatial transform: R = position, Q = quat rotation // P = parent, C = child @@ -36,10 +40,10 @@ func StepJointForces(i uint32) { //gosl:kernel jPi := JointParentIndex(ji) jPbi := int32(-1) if jPi >= 0 { - jPbi = DynamicIndex(jPi, params.Cur) + jPbi = DynamicBody(jPi) } jCi := JointChildIndex(ji) - jCbi := DynamicIndex(jCi, params.Cur) + jCbi := DynamicBody(jCi) jLinearN := GetJointLinearDoFN(ji) jAngularN := GetJointAngularDoFN(ji) @@ -116,10 +120,10 @@ func StepSolveJoints(i uint32) { //gosl:kernel jPi := JointParentIndex(ji) jPbi := int32(-1) if jPi >= 0 { - jPbi = DynamicIndex(jPi, params.Cur) + jPbi = DynamicBody(jPi) } jCi := JointChildIndex(ji) - jCbi := DynamicIndex(jCi, params.Cur) + jCbi := DynamicBody(jCi) jLinearN := GetJointLinearDoFN(ji) jAngularN := GetJointAngularDoFN(ji) @@ -218,7 +222,7 @@ func StepSolveJoints(i uint32) { //gosl:kernel linDeltaC = linDeltaC.Add(linearC.MulScalar(dLambda * params.JointLinearRelax)) angDeltaC = angDeltaC.Add(angularC.MulScalar(dLambda * params.JointAngularRelax)) } - } else { // compute joint target, stiffness, damping + } else if jLinearN > 0 { // compute joint target, stiffness, damping var axisLimitsD, axisLimitsA math32.Vector3 var axisTargetPosKeD, axisTargetPosKeA math32.Vector3 var axisTargetVelKdD, axisTargetVelKdA math32.Vector3 @@ -302,8 +306,7 @@ func StepSolveJoints(i uint32) { //gosl:kernel } } } - // todo: does it make sense to have prismatic, fixed here? - if jt == Fixed || jt == Prismatic || jt == Revolute || jt == D6 { // angular + if jAngularN > 0 { // angular qP := xwPQ qC := xwCQ // make quats lie in same hemisphere @@ -405,7 +408,8 @@ func StepSolveJoints(i uint32) { //gosl:kernel damping := float32(0.0) targetVel := slmath.Dim3(axisTargetVelKdD, dim) - derrRel := derr - targetVel + angularClen := slmath.Length3(angularC) + derrRel := derr - targetVel*angularClen // consider joint limits irrespective of axis mode lower := slmath.Dim3(axisLimitsLower, dim) diff --git a/physics/typegen.go b/physics/typegen.go index a1eab1d6..2acb7228 100644 --- a/physics/typegen.go +++ b/physics/typegen.go @@ -24,10 +24,10 @@ var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.JointVars", var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.JointDoFVars", IDName: "joint-do-f-vars", Doc: "JointDoFVars are joint DoF state variables stored in tensor.Float32,\none for each DoF."}) -var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.PhysParams", IDName: "phys-params", Doc: "PhysParams are the physics parameters", Directives: []types.Directive{{Tool: "gosl", Directive: "start"}}, Fields: []types.Field{{Name: "Iterations", Doc: "Iterations is the number of iterations to perform."}, {Name: "Dt", Doc: "Dt is the integration stepsize."}, {Name: "SoftRelax", Doc: "SoftRelax is soft-body relaxation constant."}, {Name: "JointLinearRelax", Doc: "JointLinearRelax is joint linear relaxation constant."}, {Name: "JointAngularRelax", Doc: "JointAngularRelax is joint angular relaxation constant."}, {Name: "JointLinearComply", Doc: "JointLinearComply is joint linear compliance constant."}, {Name: "JointAngularComply", Doc: "JointAngularComply is joint angular compliance constant."}, {Name: "ContactRelax", Doc: "ContactRelax is rigid contact relaxation constant."}, {Name: "AngularDamping", Doc: "AngularDamping is damping of angular motion."}, {Name: "ContactWeighting", Doc: "Contact weighting"}, {Name: "Restitution", Doc: "Restitution"}, {Name: "DynamicsN", Doc: "DynamicsN is number of dynamics bodies."}, {Name: "JointsN", Doc: "JointsN is number of joints."}, {Name: "JointDoFsN", Doc: "JointDoFsN is number of joint DoFs."}, {Name: "BodyJointsMax", Doc: "BodyJointsMax is max number of joints per body + 1 for actual n."}, {Name: "Cur", Doc: "Index for the current state (0 or 1, alternates with Next)."}, {Name: "Next", Doc: "Index for the next state (1 or 0, alternates with Cur)."}, {Name: "pad"}, {Name: "pad1"}, {Name: "pad2"}, {Name: "Gravity", Doc: "Gravity is the gravity acceleration function"}}}) +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.PhysParams", IDName: "phys-params", Doc: "PhysParams are the physics parameters", Directives: []types.Directive{{Tool: "gosl", Directive: "start"}}, Fields: []types.Field{{Name: "Iterations", Doc: "Iterations is the number of iterations to perform."}, {Name: "Dt", Doc: "Dt is the integration stepsize."}, {Name: "SoftRelax", Doc: "SoftRelax is soft-body relaxation constant."}, {Name: "JointLinearRelax", Doc: "JointLinearRelax is joint linear relaxation constant."}, {Name: "JointAngularRelax", Doc: "JointAngularRelax is joint angular relaxation constant."}, {Name: "JointLinearComply", Doc: "JointLinearComply is joint linear compliance constant."}, {Name: "JointAngularComply", Doc: "JointAngularComply is joint angular compliance constant."}, {Name: "ContactRelax", Doc: "ContactRelax is rigid contact relaxation constant."}, {Name: "AngularDamping", Doc: "AngularDamping is damping of angular motion."}, {Name: "ContactWeighting", Doc: "Contact weighting"}, {Name: "Restitution", Doc: "Restitution"}, {Name: "Cur", Doc: "Index for the current state (0 or 1, alternates with Next)."}, {Name: "Next", Doc: "Index for the next state (1 or 0, alternates with Cur)."}, {Name: "BodiesN", Doc: "BodiesN is number of rigid bodies."}, {Name: "DynamicsN", Doc: "DynamicsN is number of dynamics bodies."}, {Name: "JointsN", Doc: "JointsN is number of joints."}, {Name: "JointDoFsN", Doc: "JointDoFsN is number of joint DoFs."}, {Name: "BodyJointsMax", Doc: "BodyJointsMax is max number of joints per body + 1 for actual n."}, {Name: "BodyCollidePairsN", Doc: "BodyCollidePairsN is the total number of pre-compiled collision pairs\nto examine."}, {Name: "pad"}, {Name: "Gravity", Doc: "Gravity is the gravity acceleration function"}}}) var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.Shapes", IDName: "shapes", Doc: "Shapes are elemental shapes for rigid bodies."}) var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.State", IDName: "state", Doc: "State contains the basic physical state including position, orientation, velocity.\nThese are only the values that can be either relative or absolute -- other physical\nstate values such as Mass should go in Rigid.", Methods: []types.Method{{Name: "MoveOnAxis", Doc: "MoveOnAxis moves (translates) the specified distance on the specified local axis,\nrelative to the current rotation orientation.\nThe axis is normalized prior to aplying the distance factor.\nSets the LinVel to motion vector.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z", "dist"}}, {Name: "MoveOnAxisAbs", Doc: "MoveOnAxisAbs moves (translates) the specified distance on the specified local axis,\nin absolute X,Y,Z coordinates (does not apply the Quat rotation factor.\nThe axis is normalized prior to aplying the distance factor.\nSets the LinVel to motion vector.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z", "dist"}}, {Name: "SetEulerRotation", Doc: "SetEulerRotation sets the rotation in Euler angles (degrees).", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z"}}, {Name: "EulerRotation", Doc: "EulerRotation returns the current rotation in Euler angles (degrees).", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Returns: []string{"Vector3"}}, {Name: "SetAxisRotation", Doc: "SetAxisRotation sets rotation from local axis and angle in degrees.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z", "angle"}}, {Name: "RotateOnAxis", Doc: "RotateOnAxis rotates around the specified local axis the specified angle in degrees.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z", "angle"}}, {Name: "RotateEuler", Doc: "RotateEuler rotates by given Euler angles (in degrees) relative to existing rotation.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z"}}}, Fields: []types.Field{{Name: "Pos", Doc: "Pos is the position of center of mass of object."}, {Name: "Quat", Doc: "Quat is the rotation specified as a quaternion."}, {Name: "LinVel", Doc: "LinVel is the linear velocity."}, {Name: "AngVel", Doc: "AngVel is the angular velocity."}}}) -var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.World", IDName: "world", Doc: "World contains and manages all of the physics elements.", Directives: []types.Directive{{Tool: "go", Directive: "generate", Args: []string{"core", "generate", "-add-types", "-gosl"}}}, Fields: []types.Field{{Name: "GPU", Doc: "GPU determines whether to use GPU (else CPU)."}, {Name: "Params", Doc: "Params are global parameters."}, {Name: "Bodies", Doc: "Bodies are the rigid body elements (dynamic and static),\nspecifying the constant, non-dynamic properties,\nwhich is initial state for dynamics.\n[body][BodyVarsN]"}, {Name: "Joints", Doc: "Joints is a list of permanent joints connecting bodies,\nwhich do not change (no dynamic variables).\n[joint][JointVarsN]"}, {Name: "JointDoFs", Doc: "JointDoFs is a list of joint DoF parameters, allocated per joint.\n[dof][JointDoFVars]"}, {Name: "BodyJoints", Doc: "BodyJoints is a list of joint indexes for each dynamic body, for aggregating.\n[dyn body][parent, child][Params.BodyJointsMax]"}, {Name: "Dynamics", Doc: "Dynamics are the dynamic rigid body elements: these actually move.\nThe first set of variables are for initial values, and the second current.\n[body][cur/next][DynamicVarsN]"}, {Name: "Contacts", Doc: "Contacts are points of contact between bodies.\n[contact][ContactVarsN]"}, {Name: "JointControls", Doc: "JointControls are dynamic joint control inputs, per joint DoF\n(in correspondence with [JointDoFs]). This can be uploaded to the\nGPU at every step.\n[dof][JointControlVarsN]"}}}) +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.World", IDName: "world", Doc: "World contains and manages all of the physics elements.", Directives: []types.Directive{{Tool: "go", Directive: "generate", Args: []string{"core", "generate", "-add-types", "-gosl"}}}, Fields: []types.Field{{Name: "GPU", Doc: "GPU determines whether to use GPU (else CPU)."}, {Name: "Params", Doc: "Params are global parameters."}, {Name: "Bodies", Doc: "Bodies are the rigid body elements (dynamic and static),\nspecifying the constant, non-dynamic properties,\nwhich is initial state for dynamics.\n[body][BodyVarsN]"}, {Name: "Joints", Doc: "Joints is a list of permanent joints connecting bodies,\nwhich do not change (no dynamic variables).\n[joint][JointVarsN]"}, {Name: "JointDoFs", Doc: "JointDoFs is a list of joint DoF parameters, allocated per joint.\n[dof][JointDoFVars]"}, {Name: "BodyJoints", Doc: "BodyJoints is a list of joint indexes for each dynamic body, for aggregating.\n[dyn body][parent, child][Params.BodyJointsMax]"}, {Name: "BodyCollidePairs", Doc: "BodyCollidePairs are pairs of Body indexes that could potentially collide\nbased on precomputed collision logic, using World, Group, and Joint indexes.\n[BodyCollidePairsN][2]"}, {Name: "Dynamics", Doc: "Dynamics are the dynamic rigid body elements: these actually move.\nThe first set of variables are for initial values, and the second current.\n[body][cur/next][DynamicVarsN]"}, {Name: "Contacts", Doc: "Contacts are points of contact between bodies.\n[contact][ContactVarsN]"}, {Name: "JointControls", Doc: "JointControls are dynamic joint control inputs, per joint DoF\n(in correspondence with [JointDoFs]). This can be uploaded to the\nGPU at every step.\n[dof][JointControlVarsN]"}}}) diff --git a/physics/vars.go b/physics/vars.go index 428feba8..2fc2ff27 100644 --- a/physics/vars.go +++ b/physics/vars.go @@ -43,10 +43,16 @@ var ( //gosl:dims 3 BodyJoints *tensor.Int32 + // BodyCollidePairs are pairs of Body indexes that could potentially collide + // based on precomputed collision logic, using World, Group, and Joint indexes. + // [BodyCollidePairsN][2] + //gosl:dims 2 + BodyCollidePairs *tensor.Int32 + // Dynamics are the dynamic rigid body elements: these actually move. // Two alternating states are used: Params.Cur and Params.Next. // [dyn body][cur/next][DynamicVarsN] - //gosl:group Bodies + //gosl:group Dynamics //gosl:dims 3 Dynamics *tensor.Float32 diff --git a/physics/world.go b/physics/world.go index 059ebf1e..b8e71370 100644 --- a/physics/world.go +++ b/physics/world.go @@ -40,6 +40,11 @@ type World struct { // [dyn body][parent, child][Params.BodyJointsMax] BodyJoints *tensor.Int32 `display:"no-inline"` + // BodyCollidePairs are pairs of Body indexes that could potentially collide + // based on precomputed collision logic, using World, Group, and Joint indexes. + // [BodyCollidePairsN][2] + BodyCollidePairs *tensor.Int32 + // Dynamics are the dynamic rigid body elements: these actually move. // The first set of variables are for initial values, and the second current. // [body][cur/next][DynamicVarsN] @@ -82,7 +87,9 @@ func (wl *World) NewBody(shape Shapes, size, pos math32.Vector3, rot math32.Quat sizes := wl.Bodies.ShapeSizes() n := int32(sizes[0]) wl.Bodies.SetShapeSizes(int(n+1), int(BodyVarsN)) + wl.Params[0].BodiesN = n + 1 SetBodyShape(n, shape) + SetBodyDynamic(n, -1) SetBodySize(n, size) SetBodyPos(n, pos) SetBodyQuat(n, rot) @@ -95,10 +102,10 @@ func (wl *World) NewDynamic(shape Shapes, mass float32, size, pos math32.Vector3 sizes := wl.Dynamics.ShapeSizes() dynIdx = int32(sizes[0]) wl.Dynamics.SetShapeSizes(int(dynIdx+1), 2, int(DynamicVarsN)) - SetDynamicIndex(dynIdx, 0, bodyIdx) - SetDynamicIndex(dynIdx, 1, bodyIdx) - wl.SetMass(bodyIdx, shape, size, mass) wl.Params[0].DynamicsN = dynIdx + 1 + SetDynamicBody(dynIdx, bodyIdx) + SetBodyDynamic(bodyIdx, dynIdx) + wl.SetMass(bodyIdx, shape, size, mass) return } diff --git a/physics/world.goal b/physics/world.goal index 30a399b0..ae755e2d 100644 --- a/physics/world.goal +++ b/physics/world.goal @@ -38,6 +38,11 @@ type World struct { // [dyn body][parent, child][Params.BodyJointsMax] BodyJoints *tensor.Int32 `display:"no-inline"` + // BodyCollidePairs are pairs of Body indexes that could potentially collide + // based on precomputed collision logic, using World, Group, and Joint indexes. + // [BodyCollidePairsN][2] + BodyCollidePairs *tensor.Int32 + // Dynamics are the dynamic rigid body elements: these actually move. // The first set of variables are for initial values, and the second current. // [body][cur/next][DynamicVarsN] @@ -80,7 +85,9 @@ func (wl *World) NewBody(shape Shapes, size, pos math32.Vector3, rot math32.Quat sizes := wl.Bodies.ShapeSizes() n := int32(sizes[0]) wl.Bodies.SetShapeSizes(int(n+1), int(BodyVarsN)) + wl.Params[0].BodiesN = n + 1 SetBodyShape(n, shape) + SetBodyDynamic(n, -1) SetBodySize(n, size) SetBodyPos(n, pos) SetBodyQuat(n, rot) @@ -93,10 +100,10 @@ func (wl *World) NewDynamic(shape Shapes, mass float32, size, pos math32.Vector3 sizes := wl.Dynamics.ShapeSizes() dynIdx = int32(sizes[0]) wl.Dynamics.SetShapeSizes(int(dynIdx+1), 2, int(DynamicVarsN)) - SetDynamicIndex(dynIdx, 0, bodyIdx) - SetDynamicIndex(dynIdx, 1, bodyIdx) - wl.SetMass(bodyIdx, shape, size, mass) wl.Params[0].DynamicsN = dynIdx + 1 + SetDynamicBody(dynIdx, bodyIdx) + SetBodyDynamic(bodyIdx, dynIdx) + wl.SetMass(bodyIdx, shape, size, mass) return } From 3f5e16ef0103a867b65e7c7a00c4d5a797c4a943 Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Fri, 19 Dec 2025 08:39:07 +0100 Subject: [PATCH 23/97] physics: fix jointdof make and update virtroom --- physics/examples/test1/test1.go | 4 +-- physics/examples/virtroom/virtroom.go | 25 +++++++------- physics/joint.go | 32 ++++++++---------- physics/joint.goal | 32 ++++++++---------- physics/vars.go | 2 +- physics/world/world.go | 4 ++- .../cogentcore_org-lab-stats-cluster.go | 33 ++++++++++--------- .../cogentcore_org-lab-tensor.go | 1 + 8 files changed, 65 insertions(+), 68 deletions(-) diff --git a/physics/examples/test1/test1.go b/physics/examples/test1/test1.go index 8aec4214..96f3da49 100644 --- a/physics/examples/test1/test1.go +++ b/physics/examples/test1/test1.go @@ -65,12 +65,12 @@ func main() { // physics.SetJointDoF(bj, 0, physics.JointDamp, 0.01) // physics.SetJointDoF(bj, 0, physics.JointStiff, 1) // this makes a big difference - wl.Config() + wr.Init(wl) + params := physics.GetParams(0) params.Dt = 0.05 params.Gravity.Y = 0 - wr.Init(wl) wr.Update() sc.Camera.Pose.Pos = math32.Vec3(0, 40, 3.5) diff --git a/physics/examples/virtroom/virtroom.go b/physics/examples/virtroom/virtroom.go index 0006c729..2aedcba3 100644 --- a/physics/examples/virtroom/virtroom.go +++ b/physics/examples/virtroom/virtroom.go @@ -268,38 +268,39 @@ func (ev *Env) MakeEmer(name string, height float32) { headsz := depth * 1.5 eyesz := headsz * .2 hhsz := .5 * headsz + mass := float32(50) // kg rot := math32.NewQuat(0, 0, 0, 1) - ev.Emer = wr.NewDynamic(wl, name+"_body", physics.Box, "purple", math32.Vec3(width, height, depth), + ev.Emer = wr.NewDynamic(wl, name+"_body", physics.Box, "purple", mass, math32.Vec3(width, height, depth), math32.Vec3(0, height/2, 0), rot) // body := physics.NewCapsule(emr, "body", math32.Vec3(0, height / 2, 0), height, width/2) // body := physics.NewCylinder(emr, "body", math32.Vec3(0, height / 2, 0), height, width/2) headPos := math32.Vec3(0, height+hhsz, 0) - vw := wr.NewDynamic(wl, name+"_head", physics.Box, "tan", math32.Vec3(headsz, headsz, headsz), + head := wr.NewDynamic(wl, name+"_head", physics.Box, "tan", mass*.1, math32.Vec3(headsz, headsz, headsz), headPos, rot) - vw.InitView = func(sld *xyz.Solid) { - vw.BoxInit(sld) + head.InitView = func(sld *xyz.Solid) { + head.BoxInit(sld) sld.Updater(func() { - clr := vw.Color + clr := head.Color if ev.EmerAngry { clr = "pink" } - vw.UpdateColor(clr, sld) + head.UpdateColor(clr, sld) }) } - wl.NewJoint(physics.Fixed, ev.Emer.Index, vw.Index, vw.Pos) - vw = wr.NewDynamic(wl, name+"_eye-l", physics.Box, "green", math32.Vec3(eyesz, eyesz*.5, eyesz*.2), + wl.NewJointBall(ev.Emer.DynamicIndex, head.DynamicIndex, head.Pos, math32.Vec3(0, 0, 0)) + vw := wr.NewDynamic(wl, name+"_eye-l", physics.Box, "green", mass*.001, math32.Vec3(eyesz, eyesz*.5, eyesz*.2), headPos.Add(math32.Vec3(-hhsz*.6, headsz*.1, -(hhsz+eyesz*.3))), rot) - wl.NewJoint(physics.Glue, ev.Emer.Index, vw.Index, vw.Pos) - ev.EyeR = wr.NewDynamic(wl, name+"_eye-r", physics.Box, "green", math32.Vec3(eyesz, eyesz*.5, eyesz*.2), + wl.NewJointBall(ev.Emer.DynamicIndex, vw.DynamicIndex, vw.Pos, math32.Vec3(0, 0, 0)) + ev.EyeR = wr.NewDynamic(wl, name+"_eye-r", physics.Box, "green", mass*.001, math32.Vec3(eyesz, eyesz*.5, eyesz*.2), headPos.Add(math32.Vec3(hhsz*.6, headsz*.1, -(hhsz+eyesz*.3))), rot) - wl.NewJoint(physics.Glue, ev.Emer.Index, ev.EyeR.Index, ev.EyeR.Pos) + wl.NewJointBall(ev.Emer.DynamicIndex, ev.EyeR.DynamicIndex, ev.EyeR.Pos, math32.Vec3(0, 0, 0)) } func (ev *Env) ConfigGUI() *core.Body { // vgpu.Debug = true - b := core.NewBody("virtroom").SetTitle("Emergent Virtual Engine") + b := core.NewBody("virtroom").SetTitle("Physics Virtual Room") split := core.NewSplits(b) tv := core.NewTree(core.NewFrame(split)) diff --git a/physics/joint.go b/physics/joint.go index eb5d8709..9b9b9069 100644 --- a/physics/joint.go +++ b/physics/joint.go @@ -396,8 +396,7 @@ func (wl *World) JointDoFDefaults(didx int32) { func (wl *World) NewJointPrismatic(parent, child int32, ppos, cpos, axis math32.Vector3) int32 { idx := wl.newJoint(Prismatic, parent, child, ppos, cpos) SetJointLinearDoFN(idx, 1) - didx := wl.newJointDoF(0, axis) - SetJointDoFIndex(idx, 0, didx) + wl.newJointDoF(idx, 0, axis) return idx } @@ -411,8 +410,7 @@ func (wl *World) NewJointPrismatic(parent, child int32, ppos, cpos, axis math32. func (wl *World) NewJointRevolute(parent, child int32, ppos, cpos, axis math32.Vector3) int32 { idx := wl.newJoint(Revolute, parent, child, ppos, cpos) SetJointAngularDoFN(idx, 1) - didx := wl.newJointDoF(0, axis) - SetJointDoFIndex(idx, 0, didx) + wl.newJointDoF(idx, 0, axis) return idx } @@ -428,8 +426,7 @@ func (wl *World) NewJointBall(parent, child int32, ppos, cpos math32.Vector3) in for d := range math32.W { axis := math32.Vector3{} axis.SetDim(d, 1) - didx := wl.newJointDoF(int32(d), axis) - SetJointDoFIndex(idx, int32(d), didx) + wl.newJointDoF(idx, int32(d), axis) } return idx } @@ -448,14 +445,12 @@ func (wl *World) NewJointDistance(parent, child int32, ppos, cpos math32.Vector3 for d := range math32.W { axis := math32.Vector3{} axis.SetDim(d, 1) - didx := wl.newJointDoF(int32(d), axis) - SetJointDoFIndex(idx, int32(d), didx) + wl.newJointDoF(idx, int32(d), axis) } for d := range math32.W { axis := math32.Vector3{} axis.SetDim(d, 1) - didx := wl.newJointDoF(int32(d), axis) - SetJointDoFIndex(idx, int32(d), didx) + wl.newJointDoF(idx, int32(d), axis) } // only on the X linear axis SetJointDoF(idx, 0, JointLimitLower, minDist) @@ -485,13 +480,14 @@ func (wl *World) newJoint(joint JointTypes, parent, child int32, ppos, cpos math // newJointDoF adds new JointDoFs and JointControls entries // initialized to detfaults. Returns index. -func (wl *World) newJointDoF(dof int32, axis math32.Vector3) int32 { +func (wl *World) newJointDoF(jidx, dof int32, axis math32.Vector3) int32 { sizes := wl.JointDoFs.ShapeSizes() - idx := int32(sizes[0]) - wl.JointDoFs.SetShapeSizes(int(idx+1), int(JointDoFVarsN)) - wl.JointControls.SetShapeSizes(int(idx+1), int(JointControlVarsN)) - wl.JointDoFDefaults(idx) - SetJointAxis(idx, 0, axis) - wl.Params[0].JointDoFsN = idx + 1 - return idx + didx := int32(sizes[0]) + wl.JointDoFs.SetShapeSizes(int(didx+1), int(JointDoFVarsN)) + wl.JointControls.SetShapeSizes(int(didx+1), int(JointControlVarsN)) + wl.Params[0].JointDoFsN = didx + 1 + wl.JointDoFDefaults(didx) + SetJointDoFIndex(jidx, dof, didx) + SetJointAxis(jidx, dof, axis) + return didx } diff --git a/physics/joint.goal b/physics/joint.goal index 9e62ce3b..10888f09 100644 --- a/physics/joint.goal +++ b/physics/joint.goal @@ -396,8 +396,7 @@ func (wl *World) JointDoFDefaults(didx int32) { func (wl *World) NewJointPrismatic(parent, child int32, ppos, cpos, axis math32.Vector3) int32 { idx := wl.newJoint(Prismatic, parent, child, ppos, cpos) SetJointLinearDoFN(idx, 1) - didx := wl.newJointDoF(0, axis) - SetJointDoFIndex(idx, 0, didx) + wl.newJointDoF(idx, 0, axis) return idx } @@ -411,8 +410,7 @@ func (wl *World) NewJointPrismatic(parent, child int32, ppos, cpos, axis math32. func (wl *World) NewJointRevolute(parent, child int32, ppos, cpos, axis math32.Vector3) int32 { idx := wl.newJoint(Revolute, parent, child, ppos, cpos) SetJointAngularDoFN(idx, 1) - didx := wl.newJointDoF(0, axis) - SetJointDoFIndex(idx, 0, didx) + wl.newJointDoF(idx, 0, axis) return idx } @@ -428,8 +426,7 @@ func (wl *World) NewJointBall(parent, child int32, ppos, cpos math32.Vector3) in for d := range math32.W { axis := math32.Vector3{} axis.SetDim(d, 1) - didx := wl.newJointDoF(int32(d), axis) - SetJointDoFIndex(idx, int32(d), didx) + wl.newJointDoF(idx, int32(d), axis) } return idx } @@ -448,14 +445,12 @@ func (wl *World) NewJointDistance(parent, child int32, ppos, cpos math32.Vector3 for d := range math32.W { axis := math32.Vector3{} axis.SetDim(d, 1) - didx := wl.newJointDoF(int32(d), axis) - SetJointDoFIndex(idx, int32(d), didx) + wl.newJointDoF(idx, int32(d), axis) } for d := range math32.W { axis := math32.Vector3{} axis.SetDim(d, 1) - didx := wl.newJointDoF(int32(d), axis) - SetJointDoFIndex(idx, int32(d), didx) + wl.newJointDoF(idx, int32(d), axis) } // only on the X linear axis SetJointDoF(idx, 0, JointLimitLower, minDist) @@ -485,14 +480,15 @@ func (wl *World) newJoint(joint JointTypes, parent, child int32, ppos, cpos math // newJointDoF adds new JointDoFs and JointControls entries // initialized to detfaults. Returns index. -func (wl *World) newJointDoF(dof int32, axis math32.Vector3) int32 { +func (wl *World) newJointDoF(jidx, dof int32, axis math32.Vector3) int32 { sizes := wl.JointDoFs.ShapeSizes() - idx := int32(sizes[0]) - wl.JointDoFs.SetShapeSizes(int(idx+1), int(JointDoFVarsN)) - wl.JointControls.SetShapeSizes(int(idx+1), int(JointControlVarsN)) - wl.JointDoFDefaults(idx) - SetJointAxis(idx, 0, axis) - wl.Params[0].JointDoFsN = idx + 1 - return idx + didx := int32(sizes[0]) + wl.JointDoFs.SetShapeSizes(int(didx+1), int(JointDoFVarsN)) + wl.JointControls.SetShapeSizes(int(didx+1), int(JointControlVarsN)) + wl.Params[0].JointDoFsN = didx + 1 + wl.JointDoFDefaults(didx) + SetJointDoFIndex(jidx, dof, didx) + SetJointAxis(jidx, dof, axis) + return didx } diff --git a/physics/vars.go b/physics/vars.go index 2fc2ff27..233c546f 100644 --- a/physics/vars.go +++ b/physics/vars.go @@ -6,7 +6,7 @@ package physics import "cogentcore.org/lab/tensor" -//go:generate gosl -keep -exclude=Update,Defaults,ShouldDisplay -max-buffer-size=2147483616 +//go:generate gosl -exclude=Update,Defaults,ShouldDisplay -max-buffer-size=2147483616 //gosl:start diff --git a/physics/world/world.go b/physics/world/world.go index 8251de87..53ca2e4a 100644 --- a/physics/world/world.go +++ b/physics/world/world.go @@ -42,10 +42,12 @@ func NewWorld(sc *xyz.Scene) *World { return wr } -// Init configures the visual world based on Views. +// Init configures the visual world based on Views, +// and calls Config on [physics.World]. // Call this _once_ after making all the new Views and Bodies. // (will return if already called). func (wr *World) Init(wl *physics.World) { + wl.Config() if len(wr.Root.Makers.Normal) > 0 { return } diff --git a/yaegilab/tensorsymbols/cogentcore_org-lab-stats-cluster.go b/yaegilab/tensorsymbols/cogentcore_org-lab-stats-cluster.go index 55713542..78fdf44b 100644 --- a/yaegilab/tensorsymbols/cogentcore_org-lab-stats-cluster.go +++ b/yaegilab/tensorsymbols/cogentcore_org-lab-stats-cluster.go @@ -10,22 +10,23 @@ import ( func init() { Symbols["cogentcore.org/lab/stats/cluster/cluster"] = map[string]reflect.Value{ // function, constant and variable definitions - "Avg": reflect.ValueOf(cluster.Avg), - "AvgFunc": reflect.ValueOf(cluster.AvgFunc), - "Cluster": reflect.ValueOf(cluster.Cluster), - "Contrast": reflect.ValueOf(cluster.Contrast), - "ContrastFunc": reflect.ValueOf(cluster.ContrastFunc), - "Glom": reflect.ValueOf(cluster.Glom), - "InitAllLeaves": reflect.ValueOf(cluster.InitAllLeaves), - "Max": reflect.ValueOf(cluster.Max), - "MaxFunc": reflect.ValueOf(cluster.MaxFunc), - "MetricsN": reflect.ValueOf(cluster.MetricsN), - "MetricsValues": reflect.ValueOf(cluster.MetricsValues), - "Min": reflect.ValueOf(cluster.Min), - "MinFunc": reflect.ValueOf(cluster.MinFunc), - "NewNode": reflect.ValueOf(cluster.NewNode), - "Plot": reflect.ValueOf(cluster.Plot), - "PlotFromTable": reflect.ValueOf(cluster.PlotFromTable), + "Avg": reflect.ValueOf(cluster.Avg), + "AvgFunc": reflect.ValueOf(cluster.AvgFunc), + "Cluster": reflect.ValueOf(cluster.Cluster), + "Contrast": reflect.ValueOf(cluster.Contrast), + "ContrastFunc": reflect.ValueOf(cluster.ContrastFunc), + "Glom": reflect.ValueOf(cluster.Glom), + "InitAllLeaves": reflect.ValueOf(cluster.InitAllLeaves), + "Max": reflect.ValueOf(cluster.Max), + "MaxFunc": reflect.ValueOf(cluster.MaxFunc), + "MetricsN": reflect.ValueOf(cluster.MetricsN), + "MetricsValues": reflect.ValueOf(cluster.MetricsValues), + "Min": reflect.ValueOf(cluster.Min), + "MinFunc": reflect.ValueOf(cluster.MinFunc), + "NewNode": reflect.ValueOf(cluster.NewNode), + "Plot": reflect.ValueOf(cluster.Plot), + "PlotFromTable": reflect.ValueOf(cluster.PlotFromTable), + "PlotFromTableToTable": reflect.ValueOf(cluster.PlotFromTableToTable), // type definitions "MetricFunc": reflect.ValueOf((*cluster.MetricFunc)(nil)), diff --git a/yaegilab/tensorsymbols/cogentcore_org-lab-tensor.go b/yaegilab/tensorsymbols/cogentcore_org-lab-tensor.go index 989c6fe3..e48f83c7 100644 --- a/yaegilab/tensorsymbols/cogentcore_org-lab-tensor.go +++ b/yaegilab/tensorsymbols/cogentcore_org-lab-tensor.go @@ -56,6 +56,7 @@ func init() { "ContainsFloat": reflect.ValueOf(tensor.ContainsFloat), "ContainsInt": reflect.ValueOf(tensor.ContainsInt), "ContainsString": reflect.ValueOf(tensor.ContainsString), + "CopyFromLargerShape": reflect.ValueOf(tensor.CopyFromLargerShape), "DefaultNumThreads": reflect.ValueOf(tensor.DefaultNumThreads), "DelimsN": reflect.ValueOf(tensor.DelimsN), "DelimsValues": reflect.ValueOf(tensor.DelimsValues), From 3028c9973cafb424ff8bac7405e0f2b2dcbdd6d7 Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Fri, 19 Dec 2025 08:47:01 +0100 Subject: [PATCH 24/97] physics: update go.mod --- go.mod | 2 +- go.sum | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/go.mod b/go.mod index e2f74ffa..83765ed5 100644 --- a/go.mod +++ b/go.mod @@ -10,7 +10,7 @@ go 1.23.4 // https://github.com/googleapis/go-genproto/issues/1015 require ( - cogentcore.org/core v0.3.13-0.20251122080528-7dff9fe2d85b + cogentcore.org/core v0.3.13-0.20251219074447-aad9d251c7e7 github.com/alecthomas/assert/v2 v2.6.0 github.com/cogentcore/readline v0.1.3 github.com/cogentcore/yaegi v0.0.0-20250622201820-b7838bdd95eb diff --git a/go.sum b/go.sum index 1dffdaf7..4d022928 100644 --- a/go.sum +++ b/go.sum @@ -1,5 +1,5 @@ -cogentcore.org/core v0.3.13-0.20251122080528-7dff9fe2d85b h1:YF5zD4OoObOKKI2C8atfgJ3giEw1ySTOCqxpUq3OyZs= -cogentcore.org/core v0.3.13-0.20251122080528-7dff9fe2d85b/go.mod h1:eDHnTCy1sBhAKN9NPsSCnBW3VAnwQBNA9nbGMo9r+Xs= +cogentcore.org/core v0.3.13-0.20251219074447-aad9d251c7e7 h1:cDlSzZUdONqsphHfrNuyyJN6lC0AQ9kGY4Idv4/HO24= +cogentcore.org/core v0.3.13-0.20251219074447-aad9d251c7e7/go.mod h1:eDHnTCy1sBhAKN9NPsSCnBW3VAnwQBNA9nbGMo9r+Xs= github.com/Bios-Marcel/wastebasket/v2 v2.0.3 h1:TkoDPcSqluhLGE+EssHu7UGmLgUEkWg7kNyHyyJ3Q9g= github.com/Bios-Marcel/wastebasket/v2 v2.0.3/go.mod h1:769oPCv6eH7ugl90DYIsWwjZh4hgNmMS3Zuhe1bH6KU= github.com/BurntSushi/toml v0.3.1/go.mod h1:xHWCNGjB5oqiDr8zfno3MHue2Ht5sIBksp03qcyfWMU= From 3e5fe6240bfe23d1d0f47be2a818855886a0cdc1 Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Fri, 19 Dec 2025 13:37:40 +0100 Subject: [PATCH 25/97] physics: full docs, swtich to half-size convention. --- docs/citedrefs.json | 1 + docs/content/physics.md | 80 ++++++++++++++++++++++++ docs/content/references.md | 12 ++++ docs/docs.go | 15 ++++- physics/README.md | 90 +-------------------------- physics/contact.go | 5 +- physics/contact.goal | 5 +- physics/examples/test1/test1.go | 6 +- physics/examples/virtroom/virtroom.go | 46 +++++++------- physics/shapes.go | 25 ++++---- physics/vars.go | 4 +- physics/world.go | 30 +++++---- physics/world.goal | 30 +++++---- physics/world/view.go | 5 +- 14 files changed, 199 insertions(+), 155 deletions(-) create mode 100644 docs/citedrefs.json create mode 100644 docs/content/physics.md create mode 100644 docs/content/references.md diff --git a/docs/citedrefs.json b/docs/citedrefs.json new file mode 100644 index 00000000..55d70350 --- /dev/null +++ b/docs/citedrefs.json @@ -0,0 +1 @@ +[{"type":"paper-conference","id":"MacklinMullerChentanez16","citation-key":"MacklinMullerChentanez16","author":[{"family":"Macklin","given":"Miles"},{"family":"Müller","given":"Matthias"},{"family":"Chentanez","given":"Nuttapong"}],"accessed":{"date-parts":[["2025",12,13]]},"available-date":{},"event-date":{},"issued":{"date-parts":[["2016",10,10]]},"original-date":{},"submitted":{},"collection-title":"MIG '16","container-title":"Proceedings of the 9th International Conference on Motion in Games","DOI":"10.1145/2994258.2994272","event-place":"New York, NY, USA","ISBN":"978-1-4503-4592-7","page":"49–54","publisher":"Association for Computing Machinery","publisher-place":"New York, NY, USA","source":"ACM Digital Library","title":"XPBD: position-based simulation of compliant constrained dynamics","title-short":"XPBD","URL":"https://doi.org/10.1145/2994258.2994272"},{"type":"article-journal","id":"MullerMacklinChentanezEtAl20","citation-key":"MullerMacklinChentanezEtAl20","language":"en","author":[{"family":"Müller","given":"Matthias"},{"family":"Macklin","given":"Miles"},{"family":"Chentanez","given":"Nuttapong"},{"family":"Jeschke","given":"Stefan"},{"family":"Kim","given":"Tae-Yong"}],"accessed":{"date-parts":[["2025",12,13]]},"available-date":{},"event-date":{},"issued":{"date-parts":[["2020"]]},"original-date":{},"submitted":{},"container-title":"Computer Graphics Forum","DOI":"10.1111/cgf.14105","ISSN":"1467-8659","issue":"8","page":"101–112","source":"Wiley Online Library","title":"Detailed Rigid Body Simulation with Extended Position Based Dynamics","URL":"https://onlinelibrary.wiley.com/doi/abs/10.1111/cgf.14105","volume":"39"},{"type":"thesis","id":"Mirtich96","citation-key":"Mirtich96","author":[{"family":"Mirtich","given":"Brian V."}],"accessed":{"date-parts":[["2025",12,19]]},"available-date":{},"event-date":{},"issued":{"date-parts":[["1996"]]},"original-date":{},"submitted":{},"event-place":"Berkeley, Ca","genre":"Ph.D. Thesis","publisher":"University of California Berkeley","publisher-place":"Berkeley, Ca","title":"Impulse-based Dynamic Simulation of Rigid Body Systems","URL":"https://people.eecs.berkeley.edu/~jfc/mirtich/thesis/mirtichThesis.pdf"},{"type":"article-journal","id":"Featherstone83","citation-key":"Featherstone83","language":"EN","author":[{"family":"Featherstone","given":"R."}],"accessed":{"date-parts":[["2025",12,19]]},"available-date":{},"event-date":{},"issued":{"date-parts":[["1983",3,1]]},"original-date":{},"submitted":{},"container-title":"The International Journal of Robotics Research","DOI":"10.1177/027836498300200102","ISSN":"0278-3649","issue":"1","page":"13–30","publisher":"SAGE Publications Ltd STM","source":"SAGE Journals","title":"The Calculation of Robot Dynamics Using Articulated-Body Inertias","URL":"https://doi.org/10.1177/027836498300200102","volume":"2"},{"type":"paper-conference","id":"Baraff96","citation-key":"Baraff96","language":"en","author":[{"family":"Baraff","given":"David"}],"accessed":{"date-parts":[["2025",12,19]]},"available-date":{},"event-date":{},"issued":{"date-parts":[["1996",8]]},"original-date":{},"submitted":{},"container-title":"Proceedings of the 23rd annual conference on Computer graphics and interactive techniques","DOI":"10.1145/237170.237226","event-title":"SIGGRAPH96: 23rd International Conference on Computer Graphics and Interactive Techniques","ISBN":"978-0-89791-746-9","page":"137–146","publisher":"ACM","source":"DOI.org (Crossref)","title":"Linear-time dynamics using Lagrange multipliers","URL":"https://dl.acm.org/doi/10.1145/237170.237226"},{"type":"article-journal","id":"NealenMullerKeiserEtAl06","citation-key":"NealenMullerKeiserEtAl06","language":"en","author":[{"family":"Nealen","given":"Andrew"},{"family":"Müller","given":"Matthias"},{"family":"Keiser","given":"Richard"},{"family":"Boxerman","given":"Eddy"},{"family":"Carlson","given":"Mark"}],"accessed":{"date-parts":[["2025",12,19]]},"available-date":{},"event-date":{},"issued":{"date-parts":[["2006"]]},"original-date":{},"submitted":{},"container-title":"Computer Graphics Forum","DOI":"10.1111/j.1467-8659.2006.01000.x","ISSN":"1467-8659","issue":"4","page":"809–836","source":"Wiley Online Library","title":"Physically Based Deformable Models in Computer Graphics","URL":"https://onlinelibrary.wiley.com/doi/abs/10.1111/j.1467-8659.2006.01000.x","volume":"25"}] diff --git a/docs/content/physics.md b/docs/content/physics.md new file mode 100644 index 00000000..168af003 --- /dev/null +++ b/docs/content/physics.md @@ -0,0 +1,80 @@ ++++ +bibfile = "ccnlab.json" ++++ + +**Physics** is a 3D physics simulator for creating virtual environments, including simulated robots and animals, which can run on the GPU or CPU using [[GoSL]]. See [[doc:physics]] for the API docs. The [xyz](https://cogentcore.org/core/xyz) 3D visualization framework can be used to view the physics using the [[doc:physics/world]] package, including grabbing first-person views from the perspective of a body element within the virtual world. It is actively used for simulating motor learning in [axon](https://github.com/emergent/axon). + +It is based on the design and algorithms from the [newton-physics](https://newton-physics.github.io/newton/guide/overview.html) framework developed by Disney Research, Google DeepMind, and NVIDIA, and implemented in the [NVIDIA Warp](https://nvidia.github.io/warp/basics.html) framework, which is conceptually similar to GoSL. + +The XPBD (eXtended Position-Based Dynamics) solver is exclusively implemented ([[@MacklinMullerChentanez16]] and [[@MullerMacklinChentanezEtAl20]]), which has impressive capabilities as shown in this [YouTube video](https://www.youtube.com/watch?v=CPq87E1vD8k). The key idea is to avoid working in the world of higher-order derivatives (accelerations) and use robust position-based updates, as discussed in [[#Physics solver algorithms]]. + +Consistent with [xyz](https://cogentcore.org/core/xyz) and [gpu](https://cogentcore.org/core/gpu), the default coordinate system has `Y` as the up axis, and `Z` is the depth axis, consistent with the [USD](https://openusd.org/release/index.html) standard. Newton-physics uses `Z` up by default, which is the robotics standard. + +## GoSL infrastructure + +As discussed in [[GoSL]], to run equivalent code on the GPU and the CPU (i.e., standard Go), all of the data needs to be represented in large arrays, implemented via [[tensor]]s, and all processing occurs via _parallel for loops_ that effectively process each element of these data arrays in parallel. Enum types are used to define the variables as the last inner-most dimension in the data tensors, e.g., [[doc:physics.BodyVars]], with accessor functions to get the relevant `math32` types (e.g., `math32.Vector3`) across the X,Y,Z components. + +## Bodies and Dynamics + +The basic element is a _body_, which is a rigid physical entity with a specific shape, mass, position and orientation. Call [[doc:physics.World]] `NewBody` to create a new one. There are (currently) only standard geometric [[#shapes]] available (arbitrary triangular meshes and soft bodies could be supported as needed in the future, based on existing newton-physics code). + +By itself, a body is static. To make a body that is subject to forces and can be connected to other bodies via [[#joints]], use `NewDynamic`, which creates and additional set of data to implement the dynamic equations of the physics solver. The initial position and orientation of the body can be restored via the `InitState` method. + +To optimize the collision detection computation, it is important to organize bodies into `World` and `Group` elements: +* World: Use different world indexes for separate collections of bodies that only interact amongst themselves, and global bodies that have a -1 index. By default everything goes in world = 0. +* Group: typically just use -1 for all static bodies (non-dynamic), which can interact with any dynamic body, but not with any other static body. And use 1 for all dynamic bodies, which can interact with each-other and static bodies. + +There is a special constraint where the parent and child on a same joint do not collide, as this often happens and would lead to weird behavior. + +## Shapes + +The elemental shapes are a `Box`, `Sphere`, `Cylinder` (Cone if one radius is 0), and `Capsule`: [[doc:physics.Shapes]]. The `Size` property on bodies is always the _half_ size, such as the radius or the half-height of a cylinder or capsule. This is used in `newton-physics` and makes more sense for center-based computations: physics operates on the center-of-mass of a body. Consistent with the overall coordinate system, the `Cylinder` and `Capsule` are oriented with `Y` as the height dimension, which is unfortunately inconsistent with the Z=up convention in `newton-physics`. + +## Joints + +The supported [[doc:physics.JointTypes]] include the following (DoF = degrees-of-freedom, names are based on standards in mechanical engineering and robotics): + +* `Prismatic` Prismatic allows translation along a single axis (i.e., _slider_): 1 DoF. + +* `Revolute` allows rotation about a single axis (axel): 1 DoF. + +* `Ball` allows rotation about all three axes (3 DoF). + +* `Fixed` locks all relative motion: 0 DoF. + +* `Free` allows full 6-DoF motion (translation and rotation). + +* `Distance` keeps two bodies a distance within joint limits: 6 DoF. + +* `D6` is a generic 6-DoF joint that can be configured with up to 3 linear DoF and 3 angular DoF. + +Use `NewJoint*` with _dynamic_ body indexes to create joints (e.g., `NewJointPrismatic` etc). Each joint can be positioned with a relative offset and orientation relative to the _parent_ and _child_ elements. The parent index can be set to -1 to anchor a child body in an arbitrary and fixed position within the overall world. + +## World viewer + +Typically, bodies are created using the enhanced functions in the [[doc:physics/world]] package, which provides a [[doc:physics/world.View]] wrapper for physics bodies. This wrapper has a default `Color` setting to provide simple color coding of bodies, and supports `NewView` and `InitView` functions that allow arbitrary visualization dynamics to be associated with each body (textures, meshes, dynamic updating etc). + +## Physics solver algorithms + +This section provides a brief overview of different physics solver algorithms, and motivates why we're using XPBD (see [[@MacklinMullerChentanez16]] and [[@MullerMacklinChentanezEtAl20]] for full info). There are two main categories of mathematical problems that these engines solve: + +* Impacts from contact / collisions among bodies. When two billiard balls hit each other, they rebound in an _elastic_ collision, for example. There are also forces of friction and graded levels of inelasticity in these dynamics. The primary problem here is that the instantaneous forces involved in these impacts can be huge (this is why objects tend to shatter when you drop them on a hard surface), because the momentum reverses within a very short period of time. Numerical integration techniques tend to perform poorly when dealing with such huge forces and resulting accelerations. + +* Integrating the effects of multiple joints connecting rigid bodies. Managing the multiple constraints that arise from a _chain_ of interconnected rigid body objects is particularly challenging, because each element in the chain has impacts on the other elements, as illustrated even with two such objects in the [double pendulum](https://en.wikipedia.org/wiki/Double_pendulum). A _naive_ explicit approach using standard Newtonian physics equations incurs exponentially costly computational cost, so some kind of more sophisticated approach is required for real-time simulation. + +For the first problem, the general approach is to summarize the overall effect of the impact at a more abstract level, in terms of net _velocities_, instead of simulating the actual forces and accelerations which get very large and unwieldy. This is what [[@^Mirtich96]] developed in his **impulse-based** approach to impacts. + +For the second problem, [[@^Featherstone83]] developed a **reduced coordinates** approach (also known as _generalized_ coordinates) that uses a complex set of mathematical equations to capture exactly the effective degrees of freedom in the whole chain. This requires detailed information about each element in the chain, and also requires sequential evaluation from the end of the chain up to its root. + +The widely-used [bullet physics](https://github.com/bulletphysics/bullet3) combines these two approaches as described in [[@^Mirtich96]], and provides a fast and relatively robust solution. However, the C++ based codebase has evolved many times over the years and is very difficult to understand. Furthermore, the complexity of the Featherstone algorithm makes it a formidible challenge to implement. + +Another widely-used approach to the joint-chain problem is based on introducing additional soft _constraints_ (technically known as _Lagrange multipliers_) that can iteratively distribute the forces in a way that can be computed in linear time, as developed by [[@^Baraff96]]. This is known as a **maximal coordinates** approach, because each body is represented using their standard position and orientation coordinates as if they were independent freely-moving objects. This approach was used in the widely-used [ODE (Open Dynamics Engine)](https://ode.org/) package. A drawback of this approach is that there can be non-physical gaps that emerge over time between bodies, as these soft constraints work their way through the system. + +In this context, the **position based dynamics (PBD)** approach ([[@NealenMullerKeiserEtAl06]]) takes the idea from the impulse-based approach (using velocities instead of accelerations) to the "next level" and goes straight to positions, skipping even velocities! It uses an implicit iterative integration method known as [Gauss-Sidel](https://en.wikipedia.org/wiki/Gauss%E2%80%93Seidel_method) to integrate forces into resulting changes in position, with soft constraint factors that allow this integration method to rapidly converge. + +The result is a fully _consistent_ updated set of positions for the objects that avoids the kinds of gaps that emerge in the Lagrange Multiplier approach. The approach is very fast and robust, and also has the distinct advantage of being relatively simple to implement, especially in a GPU-compatible parallel manner. Although the approach was developed for the even more challenging soft-body physics of deformable materials including cloth, it also provides robust solutions to the basic multi-joint rigid-body scenario. + +The XPBD solver that we implement ([[@MacklinMullerChentanez16]] and [[@MullerMacklinChentanezEtAl20]]) fixes a few important problems with the PBD approach, so that the same results are obtained regardless of the time step used, and physically accurate forces and velocities can be back-computed from the final integrated position updates, so applications that track these factors can now be used. Overall, it appears to be the most robust solver that can use relatively large step sizes and a fully parallel implementation for high performance. The main downside is a potential loss in precise physical accuracy, but in most situations this is minimal, and the advantages overall should strongly outweigh this disadvantage. + +Furthermore, the [newton-physics](https://github.com/newton-physics/newton) code for XPBD was very directly convertible to Go and GoSL (unlike the situation with bullet), so the overall process was relatively straightforward. + diff --git a/docs/content/references.md b/docs/content/references.md new file mode 100644 index 00000000..6064041d --- /dev/null +++ b/docs/content/references.md @@ -0,0 +1,12 @@ +

Baraff, D. (1996). Linear-time dynamics using Lagrange multipliers. In Proceedings of the 23rd annual conference on Computer graphics and interactive techniques (pp. 137–146). ACM. https://dl.acm.org/doi/10.1145/237170.237226 http://doi.org/10.1145/237170.237226

+ +

Featherstone, R. (1983). The Calculation of Robot Dynamics Using Articulated-Body Inertias. The International Journal of Robotics Research, 2, 13–30. https://doi.org/10.1177/027836498300200102 http://doi.org/10.1177/027836498300200102

+ +

Macklin, M., Müller, M., & Chentanez, N. (2016). XPBD: position-based simulation of compliant constrained dynamics. In Proceedings of the 9th International Conference on Motion in Games (pp. 49–54). Association for Computing Machinery. https://doi.org/10.1145/2994258.2994272 http://doi.org/10.1145/2994258.2994272

+ +

Mirtich, B.V. (1996). Impulse-based Dynamic Simulation of Rigid Body Systems. [unpublished Ph.D. Thesis, University of California Berkeley]. https://people.eecs.berkeley.edu/~jfc/mirtich/thesis/mirtichThesis.pdf

+ +

Müller, M., Macklin, M., Chentanez, N., Jeschke, S., & Kim, T. (2020). Detailed Rigid Body Simulation with Extended Position Based Dynamics. Computer Graphics Forum, 39, 101–112. https://onlinelibrary.wiley.com/doi/abs/10.1111/cgf.14105 http://doi.org/10.1111/cgf.14105

+ +

Nealen, A., Müller, M., Keiser, R., Boxerman, E., & Carlson, M. (2006). Physically Based Deformable Models in Computer Graphics. Computer Graphics Forum, 25, 809–836. https://onlinelibrary.wiley.com/doi/abs/10.1111/j.1467-8659.2006.01000.x http://doi.org/10.1111/j.1467-8659.2006.01000.x

+ diff --git a/docs/docs.go b/docs/docs.go index ac19b7a3..8dfad369 100644 --- a/docs/docs.go +++ b/docs/docs.go @@ -11,11 +11,20 @@ import ( "cogentcore.org/core/core" "cogentcore.org/core/htmlcore" "cogentcore.org/core/icons" + "cogentcore.org/core/text/csl" + _ "cogentcore.org/core/text/tex" // include this to get math "cogentcore.org/core/tree" _ "cogentcore.org/lab/yaegilab" ) -//go:embed content +// NOTE: you must make a symbolic link to the zotero CCNLab CSL file as ccnlab.json +// in this directory, to generate references and have the generated reference links +// use the official APA style. https://www.zotero.org/groups/340666/ccnlab +// Must configure using BetterBibTeX for zotero: https://retorque.re/zotero-better-bibtex/ + +//go:generate mdcite -vv -refs ./ccnlab.json -d ./content + +//go:embed content citedrefs.json var econtent embed.FS func main() { @@ -23,6 +32,10 @@ func main() { ct := content.NewContent(b).SetContent(econtent) ctx := ct.Context content.OfflineURL = "https://cogentcore.org/lab" + refs, err := csl.OpenFS(econtent, "citedrefs.json") + if err == nil { + ct.References = csl.NewKeyList(refs) + } ctx.AddWikilinkHandler(htmlcore.GoDocWikilink("doc", "cogentcore.org/lab")) b.AddTopBar(func(bar *core.Frame) { tb := core.NewToolbar(bar) diff --git a/physics/README.md b/physics/README.md index 4ef2f78a..07a1c314 100644 --- a/physics/README.md +++ b/physics/README.md @@ -1,94 +1,8 @@ # Physics engine for virtual reality -The `physics` engine is a 3D physics simulator for creating virtual environments, which can run on the GPU or CPU using [gosl](https://cogentcore.org/lab/gosl). +The `physics` engine is a 3D physics simulator for creating virtual environments, which can run on the GPU or CPU using [GoSL](https://cogentcore.org/lab/gosl). -All interactions are mediated by `Joint` elements that connect two rigid `Body` elements. Optimized joint types enable robust implementation of specific types of interactions. - -To enable GPU computation, the data is all stored in tensor structures, with `Dynamics` augmenting basic `Body` data for moving rigid bodies. Static elements participate in collisions but not joints. +See [physics docs](https://cogentcore.org/lab/physics) for the main docs. The [world](world) visualization sub-package manages a `View` element that links to physics bodies and generates an [xyz](https://cogentcore.org/core/xyz) 3D scenegraph based on the physics bodies, and updates this visualization efficiently as the physics is updated. -## XPBD: Extended Position-Based Dynamics - -My intuition in confronting the physics problem was to directly update positions instead of dealing with forces, accelerations, or even velocities, because positions are more robust. Higher-order derivatives are messy and unstable. Turns out that this approach has proven quite powerful, with the MacklinMullerChentanez16 and MullerMacklinChentanezEtAl20 papers on XPBD providing some very compelling results: https://www.youtube.com/watch?v=CPq87E1vD8k - -* https://github.com/newton-physics/newton -- supports XPBD as one of several solvers, MuJoCo is default. -* https://github.com/NVIDIAGameWorks/PhysX -- older NVIDIA project -- not sure what it uses -* https://mujoco.readthedocs.io/en/stable/overview.html -- MuJoCo is widely used and is the default for Newton. -* https://github.com/bulletphysics/bullet3 -- uses Featherstone for joints and impulse-based contacts. Featherstone is incredibly complex to implement. - -* https://github.com/InteractiveComputerGraphics/PositionBasedDynamics -- another PBD impl -* https://github.com/nobuo-nakagawa/xpbd " 2016 - -So, the project is now to implement the XPBD algorithm, which is theoretically very simple, and the Newton code provides a python-based GPU-organized version. - -## Notes: - -* Update `Contact` points where bodies will touch (Dynamic on Dynamic or Static). Static is strongly grouped with hierarchical bounding boxes to optimize that process. Dynamic groups are updated as a function of motion to remain compact. - -* Contact points have priority over joints and are addressed first. No penetration is allowed. Position and velocity are updated directly. - -* Joints are then updated -- only one joint per dependent dynamic body. - - -### Scenarios - -* body -> head -> eye: this follows a clear "support" / A -> B directional dynamic -- but not parallel, as updates need to flow along the chain. - -* foot -> ankle -> leg -> body: assume that foot gets contact support, then leg, body depend on that. but what about falling over, so laying on body -- now foot is free, and dependence goes the other way. need a full constraint satisfaction solution. - -* could do multi-step constraint satisfaction within each update, so everything just propagates and an update step happens when settled? allows fully general updating. simpler. seems good. - -* settle on positions, not velocities. velocity is a parameter to use as a constraint? - -## OLD: - -It provides a `Body` node for rigid body physics, along with some basic geometrical shapes thereof. The `physics` scene contains just the bare physics bodies and other elements, which can be updated independent of any visualization. - -See [virtroom example](examples/virtroom) for an implemented example that shows how to do everything. - -## Organizing the World - -It is most efficient to create a relatively deep tree with `Group` nodes that collect nearby `Body` objects at multiple levels of spatial scale. Bounding Boxes are computed at every level of Group, and pruning is done at every level, so large chunks of the tree can be eliminated easily with this strategy. - -Also, Nodes must be specifically flagged as being `Dynamic` -- otherwise they are assumed to be static -- and each type should be organized into separate top-level Groups (there can be multiple of each, but don't mix Dynamic and Static). Static nodes are never collided against each-other. Ideally, all the Dynamic nodes are in separate top-level, or at least second-to-top level groups -- this eliminates redundant A vs. B and B vs. A collisions and focuses each collision on the most relevant information. - -## Updating Modes - -There are two major modes of updating: Scripted or Physics -- scripted requires a program to control what happens on every time step, while physics uses computed forces from contacts, plus joint constraints, to update velocities (not yet supported). The update modes are just about which methods you call. - -The `Group` has a set of `World*` methods that should be used on the top-level world Group node node to do all the init and update steps. The update loops automatically exclude non Dynamic nodes. - -* `WorldInit` -- everyone calls this at the start to set the initial config - -* `WorldRelToAbs` -- for scripted mode when updating relative positions, rotations. - -* `WorldStep` -- for either scripted or physics modes, to update state from current velocities. - -* `WorldCollide` -- returns list of potential collision contacts based on projected motion, focusing on dynamic vs. static and dynamic vs. dynamic bodies, with optimized tree filtering. This is the first pass for collision detection. - -### Scripted Mode - -For Scripted mode, each update step typically involves manually updating the `Rel.Pos` and `.Quat` fields on `Body` objects to update their relative positions. This field is a `State` type and has `MoveOnAxis` and `QuatateOnAxis` (and a number of other rotation methods). The Move methods update the `LinVel` field to reflect any delta in movement. - -It is also possible to manually set the `Abs.LinVel` and `Abs.AngVel` fields and call `Step` to update. - -For collision detection, it is essential to have the `Abs.LinVel` field set to anticipate the effects of motion and determine likely future impacts. The RelToAbs update call does this automatically, and if you're instead using `Step` the `LinVel` is already set. Both calls will automatically compute an updated BBox and VelBBox. - -It is up to the user to manage the list of potential collisions, e.g., by setting velocity to 0 or bouncing back etc. - -### Physics Mode - - - -Currently, it provides collision detection and basic forward Euler physics updating, but it does not yet compute any forces for the interactions among the bodies. Ultimately we hope to figure out how the [Bullet]) system works and get that running here, in a clean and simple implementation. - -Incrementally, we will start with a basic explicitly driven form of physics that is sufficient to get started, and build from there. - - -The good news so far is that the full physics version as in Bullet is actually not too bad. The core update step is a super simple forward Euler, intuitive update (just add velocity to position, with a step size factor). The remaining work is just in computing the forces to update those velocities. Bullet uses a hybrid approach that is clearly described in the [Mirtich thesis](https://people.eecs.berkeley.edu/~jfc/mirtich/thesis/mirtichThesis.pdf), which combines *impulses* with a particular way of handling joints, due originally to Featherstone. Impulses are really simple conceptually: when two objects collide, they bounce back off of each other in proportion to their `Bounce` (coefficient of restitution) factor -- these collision impact forces dominate everything else, and aren't that hard to compute (similar conceptually to the `marbles` example in GoGi). The joint constraint stuff is a bit more complicated but not the worst. Everything can be done incrementally. And the resulting system will avoid the brittle nature of the full constraint-based approach taken in ODE, which caused a lot of crashes and instability in `cemer`. - -One of the major problems with the impulse-based approach: it causes otherwise "still" objects to jiggle around and slip down planes, seems eminently tractable with special-case code that doesn't seem too hard. - -more info: https://caseymuratori.com/blog_0003 - diff --git a/physics/contact.go b/physics/contact.go index 934c12f2..cb205717 100644 --- a/physics/contact.go +++ b/physics/contact.go @@ -70,7 +70,7 @@ func GroupsCollide(ga, gb int32) bool { return false } -// CollisionBroad +// CollisionBroad performs broad-phase collision detection, generating Contacts. func CollisionBroad(i uint32) { //gosl:kernel params := GetParams(0) ci := int32(i) @@ -240,8 +240,9 @@ func (wl *World) ConfigBodyCollidePairs() { } } params.BodyCollidePairsN = int32(np) - pt.SetShapeSizes(np, 2) + pt.SetShapeSizes(np+1, 2) // last one is for current contacts count wl.BodyCollidePairs = pt + wl.Contacts.SetShapeSizes(np, int(ContactVarsN)) fmt.Println("body pairs over alloc", nalc, np) } diff --git a/physics/contact.goal b/physics/contact.goal index 24aa2bad..0705beba 100644 --- a/physics/contact.goal +++ b/physics/contact.goal @@ -68,7 +68,7 @@ func GroupsCollide(ga, gb int32) bool { return false } -// CollisionBroad +// CollisionBroad performs broad-phase collision detection, generating Contacts. func CollisionBroad(i uint32) { //gosl:kernel params := GetParams(0) ci := int32(i) @@ -224,8 +224,9 @@ func (wl *World) ConfigBodyCollidePairs() { } } params.BodyCollidePairsN = int32(np) - pt.SetShapeSizes(np, 2) + pt.SetShapeSizes(np+1, 2) // last one is for current contacts count wl.BodyCollidePairs = pt + wl.Contacts.SetShapeSizes(np, int(ContactVarsN)) fmt.Println("body pairs over alloc", nalc, np) } diff --git a/physics/examples/test1/test1.go b/physics/examples/test1/test1.go index 96f3da49..2ecd65b8 100644 --- a/physics/examples/test1/test1.go +++ b/physics/examples/test1/test1.go @@ -50,13 +50,13 @@ func main() { rot := math32.NewQuat(0, 0, 0, 1) thick := float32(0.1) wr.NewBody(wl, "floor", physics.Box, "grey", math32.Vec3(10, thick, 10), - math32.Vec3(0, -thick/2, 0), rot) + math32.Vec3(0, -thick, 0), rot) - height := float32(1) + height := float32(.5) width := height * .4 depth := height * .15 b1 := wr.NewDynamic(wl, "body", physics.Box, "purple", 1.0, math32.Vec3(width, height, depth), - math32.Vec3(0, height/2, 0), rot) + math32.Vec3(0, height, 0), rot) bj := wl.NewJointRevolute(-1, b1.DynamicIndex, math32.Vec3(0, 0, 0), math32.Vec3(0, -height/2, 0), math32.Vec3(0, 1, 0)) // bj := wl.NewJointPrismatic(-1, b1.DynamicIndex, math32.Vec3(0, 0, 0), math32.Vec3(0, -height/2, 0), math32.Vec3(1, 0, 0)) diff --git a/physics/examples/virtroom/virtroom.go b/physics/examples/virtroom/virtroom.go index 2aedcba3..935b60c3 100644 --- a/physics/examples/virtroom/virtroom.go +++ b/physics/examples/virtroom/virtroom.go @@ -247,35 +247,39 @@ func (ev *Env) MakeRoom(name string, width, depth, height, thick float32) { wr := ev.World wl := ev.Physics rot := math32.NewQuat(0, 0, 0, 1) - wr.NewBody(wl, name+"_floor", physics.Box, "grey", math32.Vec3(width, thick, depth), - math32.Vec3(0, -thick/2, 0), rot) - wr.NewBody(wl, name+"_back-wall", physics.Box, "blue", math32.Vec3(width, height, thick), - math32.Vec3(0, height/2, -depth/2), rot) - wr.NewBody(wl, name+"_left-wall", physics.Box, "red", math32.Vec3(thick, height, depth), - math32.Vec3(-width/2, height/2, 0), rot) - wr.NewBody(wl, name+"_right-wall", physics.Box, "green", math32.Vec3(thick, height, depth), - math32.Vec3(width/2, height/2, 0), rot) - wr.NewBody(wl, name+"_front-wall", physics.Box, "yellow", math32.Vec3(width, height, thick), - math32.Vec3(0, height/2, depth/2), rot) + hw := width / 2 + hd := depth / 2 + hh := height / 2 + ht := thick / 2 + wr.NewBody(wl, name+"_floor", physics.Box, "grey", math32.Vec3(hw, ht, hd), + math32.Vec3(0, -ht, 0), rot) + wr.NewBody(wl, name+"_back-wall", physics.Box, "blue", math32.Vec3(hw, hh, ht), + math32.Vec3(0, hh, -hd), rot) + wr.NewBody(wl, name+"_left-wall", physics.Box, "red", math32.Vec3(ht, hh, hd), + math32.Vec3(-hw, hh, 0), rot) + wr.NewBody(wl, name+"_right-wall", physics.Box, "green", math32.Vec3(ht, hh, hd), + math32.Vec3(hw, hh, 0), rot) + wr.NewBody(wl, name+"_front-wall", physics.Box, "yellow", math32.Vec3(hw, hh, ht), + math32.Vec3(0, hh, hd), rot) } // MakeEmer constructs a new Emer virtual robot of given height (e.g., 1). func (ev *Env) MakeEmer(name string, height float32) { wr := ev.World wl := ev.Physics - width := height * .4 - depth := height * .15 - headsz := depth * 1.5 + hh := height / 2 + hw := hh * .4 + hd := hh * .15 + headsz := hd * 1.5 eyesz := headsz * .2 - hhsz := .5 * headsz mass := float32(50) // kg rot := math32.NewQuat(0, 0, 0, 1) - ev.Emer = wr.NewDynamic(wl, name+"_body", physics.Box, "purple", mass, math32.Vec3(width, height, depth), - math32.Vec3(0, height/2, 0), rot) - // body := physics.NewCapsule(emr, "body", math32.Vec3(0, height / 2, 0), height, width/2) - // body := physics.NewCylinder(emr, "body", math32.Vec3(0, height / 2, 0), height, width/2) + ev.Emer = wr.NewDynamic(wl, name+"_body", physics.Box, "purple", mass, math32.Vec3(hw, hh, hd), + math32.Vec3(0, hh, 0), rot) + // body := physics.NewCapsule(emr, "body", math32.Vec3(0, hh, 0), hh, hw) + // body := physics.NewCylinder(emr, "body", math32.Vec3(0, hh, 0), hh, hw) - headPos := math32.Vec3(0, height+hhsz, 0) + headPos := math32.Vec3(0, 2*hh+headsz, 0) head := wr.NewDynamic(wl, name+"_head", physics.Box, "tan", mass*.1, math32.Vec3(headsz, headsz, headsz), headPos, rot) head.InitView = func(sld *xyz.Solid) { @@ -290,10 +294,10 @@ func (ev *Env) MakeEmer(name string, height float32) { } wl.NewJointBall(ev.Emer.DynamicIndex, head.DynamicIndex, head.Pos, math32.Vec3(0, 0, 0)) vw := wr.NewDynamic(wl, name+"_eye-l", physics.Box, "green", mass*.001, math32.Vec3(eyesz, eyesz*.5, eyesz*.2), - headPos.Add(math32.Vec3(-hhsz*.6, headsz*.1, -(hhsz+eyesz*.3))), rot) + headPos.Add(math32.Vec3(-headsz*.6, headsz*.1, -(headsz+eyesz*.3))), rot) wl.NewJointBall(ev.Emer.DynamicIndex, vw.DynamicIndex, vw.Pos, math32.Vec3(0, 0, 0)) ev.EyeR = wr.NewDynamic(wl, name+"_eye-r", physics.Box, "green", mass*.001, math32.Vec3(eyesz, eyesz*.5, eyesz*.2), - headPos.Add(math32.Vec3(hhsz*.6, headsz*.1, -(hhsz+eyesz*.3))), rot) + headPos.Add(math32.Vec3(headsz*.6, headsz*.1, -(headsz+eyesz*.3))), rot) wl.NewJointBall(ev.Emer.DynamicIndex, ev.EyeR.DynamicIndex, ev.EyeR.Pos, math32.Vec3(0, 0, 0)) } diff --git a/physics/shapes.go b/physics/shapes.go index 013ed713..0bfb14d8 100644 --- a/physics/shapes.go +++ b/physics/shapes.go @@ -11,10 +11,15 @@ import "cogentcore.org/core/math32" //gosl:start // Shapes are elemental shapes for rigid bodies. +// In general, size dimensions are half values +// (e.g., radius, half-height, etc), which is natural for +// center-based body coordinates. type Shapes int32 //enums:enum const ( // Box is a 3D rectalinear shape. + // The sizes are _half_ sizes along each dimension, + // relative to the center. Box Shapes = iota // Sphere. SizeX is the radius. @@ -22,12 +27,12 @@ const ( // Cylinder, natively oriented vertically along the Y axis. // If one radius is 0, then it is a cone. - // SizeX = bottom radius, SizeY = height in Y axis, SizeZ = top radius. + // SizeX = bottom radius, SizeY = half-height in Y axis, SizeZ = top radius. Cylinder // Capsule, which is a cylinder with half-spheres on the ends. // Natively oriented vertically along the Y axis. - // SizeX = bottom radius, SizeY = height, SizeZ = top radius. + // SizeX = bottom radius, SizeY = half-height, SizeZ = top radius. Capsule ) @@ -39,16 +44,13 @@ func (sh Shapes) BBox(sz math32.Vector3) math32.Box3 { switch sh { case Box: - bb.SetMinMax(sz.MulScalar(-.5), sz.MulScalar(.5)) + bb.SetMinMax(sz.Negate(), sz) case Sphere: bb.SetMinMax(math32.Vec3(-sz.X, -sz.X, -sz.X), math32.Vec3(sz.X, sz.X, sz.X)) case Cylinder: - h2 := sz.Y / 2 - bb.SetMinMax(math32.Vec3(-sz.X, -h2, -sz.X), math32.Vec3(sz.Z, h2, sz.Z)) + bb.SetMinMax(math32.Vec3(-sz.X, -sz.Y, -sz.X), math32.Vec3(sz.Z, sz.Y, sz.Z)) case Capsule: - th := sz.X + sz.Y + sz.Z - h2 := th / 2 - bb.SetMinMax(math32.Vec3(-sz.X, -h2, -sz.X), math32.Vec3(sz.Z, h2, sz.Z)) + bb.SetMinMax(math32.Vec3(-sz.X, -sz.Y-sz.X, -sz.X), math32.Vec3(sz.Z, sz.Y+sz.Z, sz.Z)) } // bb.Area = 2*sz.X + 2*sz.Y + 2*sz.Z // bb.Volume = sz.X * sz.Y * sz.Z @@ -66,13 +68,14 @@ func (sh Shapes) Inertia(sz math32.Vector3, mass float32) math32.Matrix3 { ia := 2.0 / 5.0 * mass * r * r inertia = math32.Mat3(ia, 0.0, 0.0, 0.0, ia, 0.0, 0.0, 0.0, ia) case Box: - w := sz.X - h := sz.Y - d := sz.Z + w := 2 * sz.X + h := 2 * sz.Y + d := 2 * sz.Z ia := 1.0 / 12.0 * mass * (h*h + d*d) ib := 1.0 / 12.0 * mass * (w*w + d*d) ic := 1.0 / 12.0 * mass * (w*w + h*h) inertia = math32.Mat3(ia, 0.0, 0.0, 0.0, ib, 0.0, 0.0, 0.0, ic) + // todo: others: } return inertia } diff --git a/physics/vars.go b/physics/vars.go index 233c546f..14bd1bb3 100644 --- a/physics/vars.go +++ b/physics/vars.go @@ -45,7 +45,9 @@ var ( // BodyCollidePairs are pairs of Body indexes that could potentially collide // based on precomputed collision logic, using World, Group, and Joint indexes. - // [BodyCollidePairsN][2] + // The last entry is updated to contain the actual number of contacts generated + // on each collision iteration. + // [BodyCollidePairsN+1][2] //gosl:dims 2 BodyCollidePairs *tensor.Int32 diff --git a/physics/world.go b/physics/world.go index b8e71370..3d394c50 100644 --- a/physics/world.go +++ b/physics/world.go @@ -42,7 +42,9 @@ type World struct { // BodyCollidePairs are pairs of Body indexes that could potentially collide // based on precomputed collision logic, using World, Group, and Joint indexes. - // [BodyCollidePairsN][2] + // The last entry is updated to contain the actual number of contacts generated + // on each collision iteration. + // [BodyCollidePairsN+1][2] BodyCollidePairs *tensor.Int32 // Dynamics are the dynamic rigid body elements: these actually move. @@ -50,8 +52,10 @@ type World struct { // [body][cur/next][DynamicVarsN] Dynamics *tensor.Float32 `display:"no-inline"` - // Contacts are points of contact between bodies. - // [contact][ContactVarsN] + // Contacts are points of contact between bodies. Max possible size + // is BodyCollidePairsN, but actual size on given run is updated + // into last entry in BodyCollidePairs. + // [BodyCollidePairsN][ContactVarsN] Contacts *tensor.Float32 `display:"no-inline"` // JointControls are dynamic joint control inputs, per joint DoF @@ -85,15 +89,16 @@ func (wl *World) Init() { // Use this for Static elements; NewDynamic for dynamic elements. func (wl *World) NewBody(shape Shapes, size, pos math32.Vector3, rot math32.Quat) int32 { sizes := wl.Bodies.ShapeSizes() - n := int32(sizes[0]) - wl.Bodies.SetShapeSizes(int(n+1), int(BodyVarsN)) - wl.Params[0].BodiesN = n + 1 - SetBodyShape(n, shape) - SetBodyDynamic(n, -1) - SetBodySize(n, size) - SetBodyPos(n, pos) - SetBodyQuat(n, rot) - return n + idx := int32(sizes[0]) + wl.Bodies.SetShapeSizes(int(idx+1), int(BodyVarsN)) + wl.Params[0].BodiesN = idx + 1 + SetBodyShape(idx, shape) + SetBodyDynamic(idx, -1) + SetBodySize(idx, size) + SetBodyPos(idx, pos) + SetBodyQuat(idx, rot) + SetBodyGroup(idx, -1) // assume static + return idx } // NewDynamic adds a new dynamic body with given parameters. Returns the index. @@ -105,6 +110,7 @@ func (wl *World) NewDynamic(shape Shapes, mass float32, size, pos math32.Vector3 wl.Params[0].DynamicsN = dynIdx + 1 SetDynamicBody(dynIdx, bodyIdx) SetBodyDynamic(bodyIdx, dynIdx) + SetBodyGroup(bodyIdx, 1) // dynamic wl.SetMass(bodyIdx, shape, size, mass) return } diff --git a/physics/world.goal b/physics/world.goal index ae755e2d..1241d722 100644 --- a/physics/world.goal +++ b/physics/world.goal @@ -40,7 +40,9 @@ type World struct { // BodyCollidePairs are pairs of Body indexes that could potentially collide // based on precomputed collision logic, using World, Group, and Joint indexes. - // [BodyCollidePairsN][2] + // The last entry is updated to contain the actual number of contacts generated + // on each collision iteration. + // [BodyCollidePairsN+1][2] BodyCollidePairs *tensor.Int32 // Dynamics are the dynamic rigid body elements: these actually move. @@ -48,8 +50,10 @@ type World struct { // [body][cur/next][DynamicVarsN] Dynamics *tensor.Float32 `display:"no-inline"` - // Contacts are points of contact between bodies. - // [contact][ContactVarsN] + // Contacts are points of contact between bodies. Max possible size + // is BodyCollidePairsN, but actual size on given run is updated + // into last entry in BodyCollidePairs. + // [BodyCollidePairsN][ContactVarsN] Contacts *tensor.Float32 `display:"no-inline"` // JointControls are dynamic joint control inputs, per joint DoF @@ -83,15 +87,16 @@ func (wl *World) Init() { // Use this for Static elements; NewDynamic for dynamic elements. func (wl *World) NewBody(shape Shapes, size, pos math32.Vector3, rot math32.Quat) int32 { sizes := wl.Bodies.ShapeSizes() - n := int32(sizes[0]) - wl.Bodies.SetShapeSizes(int(n+1), int(BodyVarsN)) - wl.Params[0].BodiesN = n + 1 - SetBodyShape(n, shape) - SetBodyDynamic(n, -1) - SetBodySize(n, size) - SetBodyPos(n, pos) - SetBodyQuat(n, rot) - return n + idx := int32(sizes[0]) + wl.Bodies.SetShapeSizes(int(idx+1), int(BodyVarsN)) + wl.Params[0].BodiesN = idx + 1 + SetBodyShape(idx, shape) + SetBodyDynamic(idx, -1) + SetBodySize(idx, size) + SetBodyPos(idx, pos) + SetBodyQuat(idx, rot) + SetBodyGroup(idx, -1) // assume static + return idx } // NewDynamic adds a new dynamic body with given parameters. Returns the index. @@ -103,6 +108,7 @@ func (wl *World) NewDynamic(shape Shapes, mass float32, size, pos math32.Vector3 wl.Params[0].DynamicsN = dynIdx + 1 SetDynamicBody(dynIdx, bodyIdx) SetBodyDynamic(bodyIdx, dynIdx) + SetBodyGroup(bodyIdx, 1) // dynamic wl.SetMass(bodyIdx, shape, size, mass) return } diff --git a/physics/world/view.go b/physics/world/view.go index a0c1f912..f91d287f 100644 --- a/physics/world/view.go +++ b/physics/world/view.go @@ -138,7 +138,7 @@ func (vw *View) BoxInit(sld *xyz.Solid) { xyz.NewBox(sld.Scene, mnm, 1, 1, 1) } sld.SetMeshName(mnm) - sld.Pose.Scale = vw.Size + sld.Pose.Scale = vw.Size.MulScalar(2) vw.UpdateColor(vw.Color, sld) sld.Updater(func() { vw.UpdatePose(sld) @@ -155,6 +155,7 @@ func (vw *View) CylinderInit(sld *xyz.Solid) { } sld.SetMeshName(mnm) sld.Pose.Scale = vw.Size + sld.Pose.Scale.Y *= 2 vw.UpdateColor(vw.Color, sld) sld.Updater(func() { vw.UpdatePose(sld) @@ -170,7 +171,7 @@ func (vw *View) CapsuleInit(sld *xyz.Solid) { ms = xyz.NewCapsule(sld.Scene, mnm, 1, .2, 32, 1) } sld.SetMeshName(mnm) - sld.Pose.Scale.Set(vw.Size.X/.2, vw.Size.Y/1.4, vw.Size.Z/.2) + sld.Pose.Scale.Set(vw.Size.X/.2, 2*(vw.Size.Y/1.4), vw.Size.Z/.2) vw.UpdateColor(vw.Color, sld) sld.Updater(func() { vw.UpdatePose(sld) From df3d947cff500945c21f441788e5b858c8ffb13e Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Fri, 19 Dec 2025 17:20:25 +0100 Subject: [PATCH 26/97] physics: broad phase collision nearly done -- just need to allocate --- physics/body.go | 3 + physics/body.goal | 3 + physics/contact.go | 186 ++++++++++----------- physics/contact.goal | 196 ++++++++++++----------- physics/enumgen.go | 20 +-- physics/gosl.go | 2 + physics/params.go | 7 +- physics/shaders/CollisionBroad.wgsl | 167 ++++++++++++++++++- physics/shaders/DeltasFromJoints.wgsl | 14 +- physics/shaders/DynamicsCurToNext.wgsl | 14 +- physics/shaders/ForcesFromJoints.wgsl | 14 +- physics/shaders/InitDynamics.wgsl | 14 +- physics/shaders/StepBodyDeltas.wgsl | 14 +- physics/shaders/StepIntegrateBodies.wgsl | 14 +- physics/shaders/StepJointForces.wgsl | 14 +- physics/shaders/StepSolveJoints.wgsl | 14 +- physics/shapes.go | 106 ++++++++++-- physics/typegen.go | 6 +- physics/world.go | 4 + physics/world.goal | 4 + physics/world/view.go | 35 +++- 21 files changed, 574 insertions(+), 277 deletions(-) diff --git a/physics/body.go b/physics/body.go index e205de8e..597296c4 100644 --- a/physics/body.go +++ b/physics/body.go @@ -104,6 +104,9 @@ const ( BodyInvInertiaXZ BodyInvInertiaYZ BodyInvInertiaZZ + + // radius for broadphase collision + BodyRadius ) func GetBodyShape(idx int32) Shapes { diff --git a/physics/body.goal b/physics/body.goal index fd97c441..4f77f9e3 100644 --- a/physics/body.goal +++ b/physics/body.goal @@ -102,6 +102,9 @@ const ( BodyInvInertiaXZ BodyInvInertiaYZ BodyInvInertiaZZ + + // radius for broadphase collision + BodyRadius ) func GetBodyShape(idx int32) Shapes { diff --git a/physics/contact.go b/physics/contact.go index cb205717..d0eec0ad 100644 --- a/physics/contact.go +++ b/physics/contact.go @@ -7,8 +7,11 @@ package physics import ( - "cogentcore.org/lab/tensor" "fmt" + + "cogentcore.org/core/math32" + "cogentcore.org/lab/gosl/slmath" + "cogentcore.org/lab/tensor" ) //gosl:start @@ -70,6 +73,8 @@ func GroupsCollide(ga, gb int32) bool { return false } +// geometry/kernels.py/broadphase_collision_pairs + // CollisionBroad performs broad-phase collision detection, generating Contacts. func CollisionBroad(i uint32) { //gosl:kernel params := GetParams(0) @@ -79,97 +84,71 @@ func CollisionBroad(i uint32) { //gosl:kernel } ba := BodyCollidePairs.Value(int(ci), int(0)) bb := BodyCollidePairs.Value(int(ci), int(1)) - _ = ba - _ = bb - // rigid_a = shape_body[shape_a] - // if rigid_a == -1: - // - // X_ws_a = shape_transform[shape_a] - // - // else: - // - // X_ws_a = body_q[rigid_a] * shape_transform[shape_a] - // - // rigid_b = shape_body[shape_b] - // if rigid_b == -1: - // - // X_ws_b = shape_transform[shape_b] - // - // else: - // - // X_ws_b = body_q[rigid_b] * shape_transform[shape_b] - // - // type_a = shape_type[shape_a] - // type_b = shape_type[shape_b] - // # ensure unique ordering of shape pairs - // if type_a > type_b: - // - // shape_tmp = shape_a - // shape_a = shape_b - // shape_b = shape_tmp - // - // type_tmp = type_a - // type_a = type_b - // type_b = type_tmp - // - // X_tmp = X_ws_a - // X_ws_a = X_ws_b - // X_ws_b = X_tmp - // - // p_b = wp.transform_get_translation(X_ws_b) - // r_b = shape_radius[shape_b] - // if type_a == GeoType.PLANE and type_b == GeoType.PLANE: - // - // return - // - // # Use per-shape contact margins + xwAR := BodyDynamicPos(ba, params.Cur) + xwAQ := BodyDynamicQuat(ba, params.Cur) + xwBR := BodyDynamicPos(bb, params.Cur) + // xwBQ := BodyDynamicQuat(bb, params.Cur) + + // note: at <= bt + sA := GetBodyShape(ba) + sB := GetBodyShape(bb) + + rb := Bodies.Value(int(bb), int(BodyRadius)) + // if type_a == GeoType.PLANE and type_b == GeoType.PLANE: + // return + + // could be per-shape // margin = wp.max(shape_contact_margin[shape_a], shape_contact_margin[shape_b]) - // - // # bounding sphere check - // if type_a == GeoType.PLANE: - // - // query_b = wp.transform_point(wp.transform_inverse(X_ws_a), p_b) - // scale = shape_scale[shape_a] - // closest = closest_point_plane(scale[0], scale[1], query_b) - // d = wp.length(query_b - closest) - // if d > r_b + margin: - // return - // - // else: - // - // p_a = wp.transform_get_translation(X_ws_a) - // d = wp.length(p_a - p_b) - // r_a = shape_radius[shape_a] - // r_b = shape_radius[shape_b] - // if d > r_a + r_b + margin: - // return - // - // pair_index_ab = shape_a * num_shapes + shape_b - // pair_index_ba = shape_b * num_shapes + shape_a - // - // num_contacts_a, num_contacts_b = count_contact_points_for_pair( - // - // shape_a, shape_b, type_a, type_b, shape_scale, shape_source_ptr - // - // ) - // - // if contact_point_limit: - // - // # assign a limit per contact pair, if max_per_pair is set - // if max_per_pair > 0: - // # distribute maximum number of contact per pair in both directions - // max_per_pair_half = max_per_pair // 2 - // if num_contacts_b > 0: - // contact_point_limit[pair_index_ab] = max_per_pair_half - // contact_point_limit[pair_index_ba] = max_per_pair_half - // else: - // contact_point_limit[pair_index_ab] = max_per_pair - // contact_point_limit[pair_index_ba] = 0 - // else: - // contact_point_limit[pair_index_ab] = 0 - // contact_point_limit[pair_index_ba] = 0 - // + margin := params.ContactMargin + + // bounding sphere check + infPlane := false + if sA == Plane { + szA := BodySize(ba) + if szA.X == 0 { + infPlane = true + } + queryB := slmath.MulSpatialPoint(xwAR, xwAQ, xwBR) + closest := ClosestPointPlane(szA, queryB) + d := slmath.Length3(queryB.Sub(closest)) + if d > rb+margin { + return + } + } else { + d := slmath.Length3(xwAR.Sub(xwBR)) + ra := Bodies.Value(int(ba), int(BodyRadius)) + if d > ra+rb+margin { + return + } + } + // pair_index_ab = shape_a * num_shapes + shape_b + // pair_index_ba = shape_b * num_shapes + shape_a + + var ncB int32 + ncA := ShapePairContacts(sA, sB, infPlane, &ncB) + _ = ncA + + // ignoring this for now: + // if contact_point_limit: + // # assign a limit per contact pair, if max_per_pair is set + // if max_per_pair > 0: + // # distribute maximum number of contact per pair in both directions + // max_per_pair_half = max_per_pair // 2 + // if num_contacts_b > 0: + // contact_point_limit[pair_index_ab] = max_per_pair_half + // contact_point_limit[pair_index_ba] = max_per_pair_half + // else: + // contact_point_limit[pair_index_ab] = max_per_pair + // contact_point_limit[pair_index_ba] = 0 + // else: + // contact_point_limit[pair_index_ab] = 0 + // contact_point_limit[pair_index_ba] = 0 + + // now we just write results to big buffer of contact points. + // Key point is that Contacts needs to have up to 12 elements per pair possible! + // just allocate in order. + // # Allocate contact points using reusable method // _success = allocate_contact_points( // @@ -186,6 +165,18 @@ func CollisionBroad(i uint32) { //gosl:kernel // ) } +// ClosestPointPlane projects the point onto the quad in +// the xy plane (if size > 0.0, otherwise infinite. +func ClosestPointPlane(sz, pt math32.Vector3) math32.Vector3 { + cp := pt + if sz.X == 0.0 { + return cp + } + cp.X = math32.Clamp(pt.X, -sz.X, sz.X) + cp.Y = math32.Clamp(pt.Y, -sz.Y, sz.Y) + return cp +} + //gosl:end // IsChildDynamic returns true if dic is a direct child @@ -204,7 +195,8 @@ func (wl *World) IsChildDynamic(dip, dic int32) bool { // ConfigBodyCollidePairs compiles a list of body paris that could collide // based on world and group settings and not being direct parent -// child relationship within a joint. +// child relationship within a joint. Result has A with lower shape type, +// so that shapes are in a canonical order. func (wl *World) ConfigBodyCollidePairs() { params := &wl.Params[0] nb := params.BodiesN @@ -234,8 +226,16 @@ func (wl *World) ConfigBodyCollidePairs() { pt.SetShapeSizes(nalc, 2) fmt.Println("body pairs realoc", nalc) } - pt.Set(a, int(np), int(0)) - pt.Set(b, int(np), int(1)) + + sA := GetBodyShape(a) + sB := GetBodyShape(b) + if sA <= sB { + pt.Set(a, int(np), int(0)) + pt.Set(b, int(np), int(1)) + } else { + pt.Set(b, int(np), int(0)) + pt.Set(a, int(np), int(1)) + } np++ } } diff --git a/physics/contact.goal b/physics/contact.goal index 0705beba..9a10621d 100644 --- a/physics/contact.goal +++ b/physics/contact.goal @@ -6,6 +6,9 @@ package physics import ( "fmt" + + "cogentcore.org/core/math32" + "cogentcore.org/lab/gosl/slmath" "cogentcore.org/lab/tensor" ) @@ -68,6 +71,8 @@ func GroupsCollide(ga, gb int32) bool { return false } +// geometry/kernels.py/broadphase_collision_pairs + // CollisionBroad performs broad-phase collision detection, generating Contacts. func CollisionBroad(i uint32) { //gosl:kernel params := GetParams(0) @@ -77,97 +82,96 @@ func CollisionBroad(i uint32) { //gosl:kernel } ba := BodyCollidePairs[ci, 0] bb := BodyCollidePairs[ci, 1] - _ = ba - _ = bb + + xwAR := BodyDynamicPos(ba, params.Cur) + xwAQ := BodyDynamicQuat(ba, params.Cur) + xwBR := BodyDynamicPos(bb, params.Cur) + // xwBQ := BodyDynamicQuat(bb, params.Cur) + + // note: at <= bt + sA := GetBodyShape(ba) + sB := GetBodyShape(bb) + + rb := Bodies[bb, BodyRadius] + // if type_a == GeoType.PLANE and type_b == GeoType.PLANE: + // return + + // could be per-shape + // margin = wp.max(shape_contact_margin[shape_a], shape_contact_margin[shape_b]) + margin := params.ContactMargin + + // bounding sphere check + infPlane := false + if sA == Plane { + szA := BodySize(ba) + if szA.X == 0 { + infPlane = true + } + queryB := slmath.MulSpatialPoint(xwAR, xwAQ, xwBR) + closest := ClosestPointPlane(szA, queryB) + d := slmath.Length3(queryB.Sub(closest)) + if d > rb+margin { + return + } + } else { + d := slmath.Length3(xwAR.Sub(xwBR)) + ra := Bodies[ba, BodyRadius] + if d > ra+rb+margin { + return + } + } + // pair_index_ab = shape_a * num_shapes + shape_b + // pair_index_ba = shape_b * num_shapes + shape_a + + var ncB int32 + ncA := ShapePairContacts(sA, sB, infPlane, &ncB) + _ = ncA + + // ignoring this for now: + // if contact_point_limit: + // # assign a limit per contact pair, if max_per_pair is set + // if max_per_pair > 0: + // # distribute maximum number of contact per pair in both directions + // max_per_pair_half = max_per_pair // 2 + // if num_contacts_b > 0: + // contact_point_limit[pair_index_ab] = max_per_pair_half + // contact_point_limit[pair_index_ba] = max_per_pair_half + // else: + // contact_point_limit[pair_index_ab] = max_per_pair + // contact_point_limit[pair_index_ba] = 0 + // else: + // contact_point_limit[pair_index_ab] = 0 + // contact_point_limit[pair_index_ba] = 0 + + // now we just write results to big buffer of contact points. + // Key point is that Contacts needs to have up to 12 elements per pair possible! + // just allocate in order. - // rigid_a = shape_body[shape_a] - // if rigid_a == -1: - // X_ws_a = shape_transform[shape_a] - // else: - // X_ws_a = body_q[rigid_a] * shape_transform[shape_a] - // rigid_b = shape_body[shape_b] - // if rigid_b == -1: - // X_ws_b = shape_transform[shape_b] - // else: - // X_ws_b = body_q[rigid_b] * shape_transform[shape_b] -// - // type_a = shape_type[shape_a] - // type_b = shape_type[shape_b] - // # ensure unique ordering of shape pairs - // if type_a > type_b: - // shape_tmp = shape_a - // shape_a = shape_b - // shape_b = shape_tmp -// - // type_tmp = type_a - // type_a = type_b - // type_b = type_tmp -// - // X_tmp = X_ws_a - // X_ws_a = X_ws_b - // X_ws_b = X_tmp -// - // p_b = wp.transform_get_translation(X_ws_b) - // r_b = shape_radius[shape_b] - // if type_a == GeoType.PLANE and type_b == GeoType.PLANE: - // return -// - // # Use per-shape contact margins - // margin = wp.max(shape_contact_margin[shape_a], shape_contact_margin[shape_b]) -// - // # bounding sphere check - // if type_a == GeoType.PLANE: - // query_b = wp.transform_point(wp.transform_inverse(X_ws_a), p_b) - // scale = shape_scale[shape_a] - // closest = closest_point_plane(scale[0], scale[1], query_b) - // d = wp.length(query_b - closest) - // if d > r_b + margin: - // return - // else: - // p_a = wp.transform_get_translation(X_ws_a) - // d = wp.length(p_a - p_b) - // r_a = shape_radius[shape_a] - // r_b = shape_radius[shape_b] - // if d > r_a + r_b + margin: - // return -// - // pair_index_ab = shape_a * num_shapes + shape_b - // pair_index_ba = shape_b * num_shapes + shape_a -// - // num_contacts_a, num_contacts_b = count_contact_points_for_pair( - // shape_a, shape_b, type_a, type_b, shape_scale, shape_source_ptr - // ) -// - // if contact_point_limit: - // # assign a limit per contact pair, if max_per_pair is set - // if max_per_pair > 0: - // # distribute maximum number of contact per pair in both directions - // max_per_pair_half = max_per_pair // 2 - // if num_contacts_b > 0: - // contact_point_limit[pair_index_ab] = max_per_pair_half - // contact_point_limit[pair_index_ba] = max_per_pair_half - // else: - // contact_point_limit[pair_index_ab] = max_per_pair - // contact_point_limit[pair_index_ba] = 0 - // else: - // contact_point_limit[pair_index_ab] = 0 - // contact_point_limit[pair_index_ba] = 0 -// - // # Allocate contact points using reusable method - // _success = allocate_contact_points( - // num_contacts_a, - // num_contacts_b, - // shape_a, - // shape_b, - // rigid_contact_max, - // contact_count, - // contact_shape0, - // contact_shape1, - // contact_point_id, - // ) + // # Allocate contact points using reusable method + // _success = allocate_contact_points( + // num_contacts_a, + // num_contacts_b, + // shape_a, + // shape_b, + // rigid_contact_max, + // contact_count, + // contact_shape0, + // contact_shape1, + // contact_point_id, + // ) } - +// ClosestPointPlane projects the point onto the quad in +// the xy plane (if size > 0.0, otherwise infinite. +func ClosestPointPlane(sz, pt math32.Vector3) math32.Vector3 { + cp := pt + if sz.X == 0.0 { + return cp + } + cp.X = math32.Clamp(pt.X, -sz.X, sz.X) + cp.Y = math32.Clamp(pt.Y, -sz.Y, sz.Y) + return cp +} //gosl:end @@ -185,10 +189,10 @@ func (wl *World) IsChildDynamic(dip, dic int32) bool { return false } - // ConfigBodyCollidePairs compiles a list of body paris that could collide // based on world and group settings and not being direct parent -// child relationship within a joint. +// child relationship within a joint. Result has A with lower shape type, +// so that shapes are in a canonical order. func (wl *World) ConfigBodyCollidePairs() { params := &wl.Params[0] nb := params.BodiesN @@ -218,8 +222,16 @@ func (wl *World) ConfigBodyCollidePairs() { pt.SetShapeSizes(nalc, 2) fmt.Println("body pairs realoc", nalc) } - pt[np, 0] = a - pt[np, 1] = b + + sA := GetBodyShape(a) + sB := GetBodyShape(b) + if sA <= sB { + pt[np, 0] = a + pt[np, 1] = b + } else { + pt[np, 0] = b + pt[np, 1] = a + } np++ } } diff --git a/physics/enumgen.go b/physics/enumgen.go index b08b11da..86b4fb66 100644 --- a/physics/enumgen.go +++ b/physics/enumgen.go @@ -6,20 +6,20 @@ import ( "cogentcore.org/core/enums" ) -var _BodyVarsValues = []BodyVars{0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38} +var _BodyVarsValues = []BodyVars{0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39} // BodyVarsN is the highest valid value for type BodyVars, plus one. // //gosl:start -const BodyVarsN BodyVars = 39 +const BodyVarsN BodyVars = 40 //gosl:end -var _BodyVarsValueMap = map[string]BodyVars{`BodyShape`: 0, `BodyDynamic`: 1, `BodyWorld`: 2, `BodyGroup`: 3, `BodySizeX`: 4, `BodySizeY`: 5, `BodySizeZ`: 6, `BodyMass`: 7, `BodyInvMass`: 8, `BodyBounce`: 9, `BodyFriction`: 10, `BodyPosX`: 11, `BodyPosY`: 12, `BodyPosZ`: 13, `BodyQuatX`: 14, `BodyQuatY`: 15, `BodyQuatZ`: 16, `BodyQuatW`: 17, `BodyComX`: 18, `BodyComY`: 19, `BodyComZ`: 20, `BodyInertiaXX`: 21, `BodyInertiaYX`: 22, `BodyInertiaZX`: 23, `BodyInertiaXY`: 24, `BodyInertiaYY`: 25, `BodyInertiaZY`: 26, `BodyInertiaXZ`: 27, `BodyInertiaYZ`: 28, `BodyInertiaZZ`: 29, `BodyInvInertiaXX`: 30, `BodyInvInertiaYX`: 31, `BodyInvInertiaZX`: 32, `BodyInvInertiaXY`: 33, `BodyInvInertiaYY`: 34, `BodyInvInertiaZY`: 35, `BodyInvInertiaXZ`: 36, `BodyInvInertiaYZ`: 37, `BodyInvInertiaZZ`: 38} +var _BodyVarsValueMap = map[string]BodyVars{`BodyShape`: 0, `BodyDynamic`: 1, `BodyWorld`: 2, `BodyGroup`: 3, `BodySizeX`: 4, `BodySizeY`: 5, `BodySizeZ`: 6, `BodyMass`: 7, `BodyInvMass`: 8, `BodyBounce`: 9, `BodyFriction`: 10, `BodyPosX`: 11, `BodyPosY`: 12, `BodyPosZ`: 13, `BodyQuatX`: 14, `BodyQuatY`: 15, `BodyQuatZ`: 16, `BodyQuatW`: 17, `BodyComX`: 18, `BodyComY`: 19, `BodyComZ`: 20, `BodyInertiaXX`: 21, `BodyInertiaYX`: 22, `BodyInertiaZX`: 23, `BodyInertiaXY`: 24, `BodyInertiaYY`: 25, `BodyInertiaZY`: 26, `BodyInertiaXZ`: 27, `BodyInertiaYZ`: 28, `BodyInertiaZZ`: 29, `BodyInvInertiaXX`: 30, `BodyInvInertiaYX`: 31, `BodyInvInertiaZX`: 32, `BodyInvInertiaXY`: 33, `BodyInvInertiaYY`: 34, `BodyInvInertiaZY`: 35, `BodyInvInertiaXZ`: 36, `BodyInvInertiaYZ`: 37, `BodyInvInertiaZZ`: 38, `BodyRadius`: 39} -var _BodyVarsDescMap = map[BodyVars]string{0: `BodyShape is the shape type of the object, as a Shapes type.`, 1: `BodyDynamic is the index into Dynamics for this body, which is -1 for static bodies. Use this to get current Pos and Quat values for a dynamic body.`, 2: `BodyWorld partitions bodies into different worlds for collision detection: Global bodies = -1 can collide with everything; otherwise only items within the same world collide.`, 3: `BodyGroup partitions bodies within worlds into different groups for collision detection. 0 does not collide with anything. Negative numbers are global within a world, except they don't collide amongst themselves (all non-dynamic bodies should go in -1 because they don't collide amongst each-other, but do potentially collide with dynamics). Positive numbers only collide amongst themselves, and with negative groups, but not other positive groups. This is for more special-purpose dynamics: in general use 1 for all dynamic bodies. There is an automatic constraint that the two objects within a single joint do not collide with each other, so this does not need to be handled here.`, 4: `BodySize is the size of the object (values depend on shape type).`, 5: ``, 6: ``, 7: `BodyMass is the mass of the object.`, 8: `BodyInvMass is 1/mass of the object or 0 if no mass.`, 9: `BodyBounce specifies the COR or coefficient of restitution (0..1), which determines how elastic the collision is, i.e., final velocity / initial velocity.`, 10: `BodyFriction coefficient: how much friction is generated by transverse motion. Additive across the two surfaces.`, 11: `3D position of body (structural center).`, 12: ``, 13: ``, 14: `Quaternion rotation of body.`, 15: ``, 16: ``, 17: ``, 18: `Relative center-of-mass offset from 3D position of body.`, 19: ``, 20: ``, 21: `Inertia 3x3 matrix (column matrix organization, r,c labels).`, 22: ``, 23: ``, 24: ``, 25: ``, 26: ``, 27: ``, 28: ``, 29: ``, 30: `InvInertia inverse inertia 3x3 matrix (column matrix organization, r,c labels).`, 31: ``, 32: ``, 33: ``, 34: ``, 35: ``, 36: ``, 37: ``, 38: ``} +var _BodyVarsDescMap = map[BodyVars]string{0: `BodyShape is the shape type of the object, as a Shapes type.`, 1: `BodyDynamic is the index into Dynamics for this body, which is -1 for static bodies. Use this to get current Pos and Quat values for a dynamic body.`, 2: `BodyWorld partitions bodies into different worlds for collision detection: Global bodies = -1 can collide with everything; otherwise only items within the same world collide.`, 3: `BodyGroup partitions bodies within worlds into different groups for collision detection. 0 does not collide with anything. Negative numbers are global within a world, except they don't collide amongst themselves (all non-dynamic bodies should go in -1 because they don't collide amongst each-other, but do potentially collide with dynamics). Positive numbers only collide amongst themselves, and with negative groups, but not other positive groups. This is for more special-purpose dynamics: in general use 1 for all dynamic bodies. There is an automatic constraint that the two objects within a single joint do not collide with each other, so this does not need to be handled here.`, 4: `BodySize is the size of the object (values depend on shape type).`, 5: ``, 6: ``, 7: `BodyMass is the mass of the object.`, 8: `BodyInvMass is 1/mass of the object or 0 if no mass.`, 9: `BodyBounce specifies the COR or coefficient of restitution (0..1), which determines how elastic the collision is, i.e., final velocity / initial velocity.`, 10: `BodyFriction coefficient: how much friction is generated by transverse motion. Additive across the two surfaces.`, 11: `3D position of body (structural center).`, 12: ``, 13: ``, 14: `Quaternion rotation of body.`, 15: ``, 16: ``, 17: ``, 18: `Relative center-of-mass offset from 3D position of body.`, 19: ``, 20: ``, 21: `Inertia 3x3 matrix (column matrix organization, r,c labels).`, 22: ``, 23: ``, 24: ``, 25: ``, 26: ``, 27: ``, 28: ``, 29: ``, 30: `InvInertia inverse inertia 3x3 matrix (column matrix organization, r,c labels).`, 31: ``, 32: ``, 33: ``, 34: ``, 35: ``, 36: ``, 37: ``, 38: ``, 39: `radius for broadphase collision`} -var _BodyVarsMap = map[BodyVars]string{0: `BodyShape`, 1: `BodyDynamic`, 2: `BodyWorld`, 3: `BodyGroup`, 4: `BodySizeX`, 5: `BodySizeY`, 6: `BodySizeZ`, 7: `BodyMass`, 8: `BodyInvMass`, 9: `BodyBounce`, 10: `BodyFriction`, 11: `BodyPosX`, 12: `BodyPosY`, 13: `BodyPosZ`, 14: `BodyQuatX`, 15: `BodyQuatY`, 16: `BodyQuatZ`, 17: `BodyQuatW`, 18: `BodyComX`, 19: `BodyComY`, 20: `BodyComZ`, 21: `BodyInertiaXX`, 22: `BodyInertiaYX`, 23: `BodyInertiaZX`, 24: `BodyInertiaXY`, 25: `BodyInertiaYY`, 26: `BodyInertiaZY`, 27: `BodyInertiaXZ`, 28: `BodyInertiaYZ`, 29: `BodyInertiaZZ`, 30: `BodyInvInertiaXX`, 31: `BodyInvInertiaYX`, 32: `BodyInvInertiaZX`, 33: `BodyInvInertiaXY`, 34: `BodyInvInertiaYY`, 35: `BodyInvInertiaZY`, 36: `BodyInvInertiaXZ`, 37: `BodyInvInertiaYZ`, 38: `BodyInvInertiaZZ`} +var _BodyVarsMap = map[BodyVars]string{0: `BodyShape`, 1: `BodyDynamic`, 2: `BodyWorld`, 3: `BodyGroup`, 4: `BodySizeX`, 5: `BodySizeY`, 6: `BodySizeZ`, 7: `BodyMass`, 8: `BodyInvMass`, 9: `BodyBounce`, 10: `BodyFriction`, 11: `BodyPosX`, 12: `BodyPosY`, 13: `BodyPosZ`, 14: `BodyQuatX`, 15: `BodyQuatY`, 16: `BodyQuatZ`, 17: `BodyQuatW`, 18: `BodyComX`, 19: `BodyComY`, 20: `BodyComZ`, 21: `BodyInertiaXX`, 22: `BodyInertiaYX`, 23: `BodyInertiaZX`, 24: `BodyInertiaXY`, 25: `BodyInertiaYY`, 26: `BodyInertiaZY`, 27: `BodyInertiaXZ`, 28: `BodyInertiaYZ`, 29: `BodyInertiaZZ`, 30: `BodyInvInertiaXX`, 31: `BodyInvInertiaYX`, 32: `BodyInvInertiaZX`, 33: `BodyInvInertiaXY`, 34: `BodyInvInertiaYY`, 35: `BodyInvInertiaZY`, 36: `BodyInvInertiaXZ`, 37: `BodyInvInertiaYZ`, 38: `BodyInvInertiaZZ`, 39: `BodyRadius`} // String returns the string representation of this BodyVars value. func (i BodyVars) String() string { return enums.String(i, _BodyVarsMap) } @@ -378,20 +378,20 @@ func (i *JointDoFVars) UnmarshalText(text []byte) error { return enums.UnmarshalText(i, text, "JointDoFVars") } -var _ShapesValues = []Shapes{0, 1, 2, 3} +var _ShapesValues = []Shapes{0, 1, 2, 3, 4} // ShapesN is the highest valid value for type Shapes, plus one. // //gosl:start -const ShapesN Shapes = 4 +const ShapesN Shapes = 5 //gosl:end -var _ShapesValueMap = map[string]Shapes{`Box`: 0, `Sphere`: 1, `Cylinder`: 2, `Capsule`: 3} +var _ShapesValueMap = map[string]Shapes{`Plane`: 0, `Sphere`: 1, `Capsule`: 2, `Cylinder`: 3, `Box`: 4} -var _ShapesDescMap = map[Shapes]string{0: `Box is a 3D rectalinear shape.`, 1: `Sphere. SizeX is the radius.`, 2: `Cylinder, natively oriented vertically along the Y axis. If one radius is 0, then it is a cone. SizeX = bottom radius, SizeY = height in Y axis, SizeZ = top radius.`, 3: `Capsule, which is a cylinder with half-spheres on the ends. Natively oriented vertically along the Y axis. SizeX = bottom radius, SizeY = height, SizeZ = top radius.`} +var _ShapesDescMap = map[Shapes]string{0: `Plane cannot be a dynamic shape, but is most efficient for collision computations. Use size = 0 for an infinite plane.`, 1: `Sphere. SizeX is the radius.`, 2: `Capsule, which is a cylinder with half-spheres on the ends. Natively oriented vertically along the Y axis. SizeX = bottom radius, SizeY = half-height, SizeZ = top radius.`, 3: `Cylinder, natively oriented vertically along the Y axis. If one radius is 0, then it is a cone. SizeX = bottom radius, SizeY = half-height in Y axis, SizeZ = top radius. Cylinder can not collide with a Box.`, 4: `Box is a 3D rectalinear shape. The sizes are _half_ sizes along each dimension, relative to the center.`} -var _ShapesMap = map[Shapes]string{0: `Box`, 1: `Sphere`, 2: `Cylinder`, 3: `Capsule`} +var _ShapesMap = map[Shapes]string{0: `Plane`, 1: `Sphere`, 2: `Capsule`, 3: `Cylinder`, 4: `Box`} // String returns the string representation of this Shapes value. func (i Shapes) String() string { return enums.String(i, _ShapesMap) } diff --git a/physics/gosl.go b/physics/gosl.go index bdf9557e..9c5968d8 100644 --- a/physics/gosl.go +++ b/physics/gosl.go @@ -112,7 +112,9 @@ func GPUInit() { var pl *gpu.ComputePipeline pl = gpu.NewComputePipelineShaderFS(shaders, "shaders/CollisionBroad.wgsl", sy) pl.AddVarUsed(0, "TensorStrides") + pl.AddVarUsed(1, "Bodies") pl.AddVarUsed(1, "BodyCollidePairs") + pl.AddVarUsed(2, "Dynamics") pl.AddVarUsed(0, "Params") pl = gpu.NewComputePipelineShaderFS(shaders, "shaders/DeltasFromJoints.wgsl", sy) pl.AddVarUsed(0, "TensorStrides") diff --git a/physics/params.go b/physics/params.go index e3624a2a..c90cd54b 100644 --- a/physics/params.go +++ b/physics/params.go @@ -46,6 +46,10 @@ type PhysParams struct { // Restitution Restitution slbool.Bool `default:"false"` + // Contact margin is the extra distance for broadphase collision + // around rigid bodies. + ContactMargin float32 + // Index for the current state (0 or 1, alternates with Next). Cur int32 `edit:"-"` @@ -71,8 +75,6 @@ type PhysParams struct { // to examine. BodyCollidePairsN int32 `edit:"-"` - pad int32 - // Gravity is the gravity acceleration function Gravity slvec.Vector3 } @@ -90,6 +92,7 @@ func (pr *PhysParams) Defaults() { pr.AngularDamping = 0 pr.ContactWeighting.SetBool(true) pr.Restitution.SetBool(false) + pr.ContactMargin = 0.1 } //gosl:end diff --git a/physics/shaders/CollisionBroad.wgsl b/physics/shaders/CollisionBroad.wgsl index 63e1bb2a..0d3900e2 100644 --- a/physics/shaders/CollisionBroad.wgsl +++ b/physics/shaders/CollisionBroad.wgsl @@ -7,9 +7,13 @@ var TensorStrides: array; @group(0) @binding(1) var Params: array; // // Bodies are the rigid body elements (dynamic and static), // specifying the constant, non-dynamic properties, // which is initial state for dynamics. // [body][BodyVarsN] +@group(1) @binding(0) +var Bodies: array; @group(1) @binding(4) var BodyCollidePairs: array; // // Dynamics are the dynamic rigid body elements: these actually move. // Two alternating states are used: Params.Cur and Params.Next. // [dyn body][cur/next][DynamicVarsN] +@group(2) @binding(0) +var Dynamics: array; // // JointControls are dynamic joint control inputs, per joint DoF // (in correspondence with [JointDoFs]). This can be uploaded to the // GPU at every step. // [dof][JointControlVarsN] alias GPUVars = i32; @@ -24,6 +28,10 @@ fn Index2D(s0: u32, s1: u32, i0: u32, i1: u32) -> u32 { return s0 * i0 + s1 * i1; } +fn Index3D(s0: u32, s1: u32, s2: u32, i0: u32, i1: u32, i2: u32) -> u32 { + return s0 * i0 + s1 * i1 + s2 * i2; +} + //////// import: "vars.go" @@ -68,6 +76,34 @@ const BodyInvInertiaZY: BodyVars = 35; const BodyInvInertiaXZ: BodyVars = 36; const BodyInvInertiaYZ: BodyVars = 37; const BodyInvInertiaZZ: BodyVars = 38; +const BodyRadius: BodyVars = 39; +fn GetBodyShape(idx: i32) -> Shapes { + return Shapes(bitcast(Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyShape))])); +} +fn GetBodyDynamic(idx: i32) -> i32 { + return i32(bitcast(Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyDynamic))])); +} +fn BodySize(idx: i32) -> vec3 { + return vec3(Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodySizeX))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodySizeY))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodySizeZ))]); +} +fn BodyPos(idx: i32) -> vec3 { + return vec3(Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyPosX))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyPosY))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyPosZ))]); +} +fn BodyQuat(idx: i32) -> vec4 { + return vec4(Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyQuatX))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyQuatY))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyQuatZ))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyQuatW))]); +} +fn BodyDynamicPos(idx: i32,cni: i32) -> vec3 { + var didx = GetBodyDynamic(idx); + if (didx < 0) { + return BodyPos(idx); + }return DynamicPos(didx, cni); +} +fn BodyDynamicQuat(idx: i32,cni: i32) -> vec4 { + var didx = GetBodyDynamic(idx); + if (didx < 0) { + return BodyQuat(idx); + }return DynamicQuat(didx, cni); +} //////// import: "contact.go" alias ContactVars = i32; //enums:enum @@ -90,8 +126,38 @@ const ContactForceZ: ContactVars = 15; fn CollisionBroad(i: u32) { //gosl:kernel let params = Params[0];; var ci = i32(i);; if (ci >= params.BodyCollidePairsN) { return; -}; var ba = BodyCollidePairs[Index2D(TensorStrides[40], TensorStrides[41], u32(ci), u32(0))];; var bb = BodyCollidePairs[Index2D(TensorStrides[40], TensorStrides[41], -u32(ci), u32(1))];; _ = ba;; _ = bb; } +}; var ba = BodyCollidePairs[Index2D(TensorStrides[40], TensorStrides[41], u32(ci), u32(0))];; var bb = BodyCollidePairs[Index2D(TensorStrides[40], TensorStrides[41], u32(ci), u32(1))];; var xwAR = BodyDynamicPos(ba, params.Cur);; var xwAQ = BodyDynamicQuat(ba, params.Cur);; var xwBR = BodyDynamicPos(bb, params.Cur);; +var sA = GetBodyShape(ba);; var sB = GetBodyShape(bb);; var rb = Bodies[Index2D(TensorStrides[0], TensorStrides[1], +u32(bb), u32(BodyRadius))];; +var margin = params.ContactMargin;; +var infPlane = false;; if (sA == Plane) { + var szA = BodySize(ba); + if (szA.x == 0) { + infPlane = true; + } + var queryB = MulSpatialPoint(xwAR, xwAQ, xwBR); + var closest = ClosestPointPlane(szA, queryB); + var d = Length3(queryB-(closest)); + if (d > rb+margin) { + return; + } +} else { + var d = Length3(xwAR-(xwBR)); + var ra = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(ba), u32(BodyRadius))]; + if (d > ra+rb+margin) { + return; + } +}; +var ncB: i32;; var ncA = ShapePairContacts(sA, sB, infPlane, &ncB);; _ = ncA; +} +fn ClosestPointPlane(sz: vec3,pt: vec3) -> vec3 { + var cp = pt; + if (sz.x == 0.0) { + return cp; + } + cp.x = clamp(pt.x, -sz.x, sz.x); + cp.y = clamp(pt.y, -sz.y, sz.y);return cp; +} //////// import: "control.go" alias JointControlVars = i32; //enums:enum @@ -133,9 +199,15 @@ const DynDeltaZ: DynamicVars = 28; const DynAngDeltaX: DynamicVars = 29; const DynAngDeltaY: DynamicVars = 30; const DynAngDeltaZ: DynamicVars = 31; +fn DynamicPos(idx: i32,cni: i32) -> vec3 { + return vec3(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosZ))]); +} +fn DynamicQuat(idx: i32,cni: i32) -> vec4 { + return vec4(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatZ))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatW))]); +} //////// import: "enumgen.go" -const BodyVarsN: BodyVars = 39; +const BodyVarsN: BodyVars = 40; const ContactVarsN: ContactVars = 16; const JointControlVarsN: JointControlVars = 3; const DynamicVarsN: DynamicVars = 32; @@ -143,7 +215,7 @@ const GPUVarsN: GPUVars = 9; const JointTypesN: JointTypes = 7; const JointVarsN: JointVars = 50; const JointDoFVarsN: JointDoFVars = 7; -const ShapesN: Shapes = 4; +const ShapesN: Shapes = 5; //////// import: "joint.go" const JointLimitUnlimited = 1e10; @@ -228,6 +300,7 @@ struct PhysParams { AngularDamping: f32, ContactWeighting: i32, Restitution: i32, + ContactMargin: f32, Cur: i32, Next: i32, BodiesN: i32, @@ -236,22 +309,100 @@ struct PhysParams { JointDoFsN: i32, BodyJointsMax: i32, BodyCollidePairsN: i32, - pad: i32, Gravity: vec4, } //////// import: "shapes.go" alias Shapes = i32; //enums:enum -const Box: Shapes = 0; +const Plane: Shapes = 0; const Sphere: Shapes = 1; -const Cylinder: Shapes = 2; -const Capsule: Shapes = 3; +const Capsule: Shapes = 2; +const Cylinder: Shapes = 3; +const Box: Shapes = 4; +fn ShapePairContacts(a: Shapes,b: Shapes, infPlane: bool, ba: ptr) -> i32 { + *ba = i32(0); + switch (a) { + case Plane: { + switch (b) { + case Plane: { + return i32(0); + } + case Sphere: { + return i32(1); + } + case Capsule: { + if (infPlane) { + return i32(2); + } else { + return 2 + 4; + } + } + case Cylinder: { + return i32(4); + } + case Box: { + if (infPlane) { + return i32(8); + } else { + return 8 + 4; + } + } + default: { + return i32(0); + } + } + } + case Sphere: { + return i32(1); + } + case Capsule: { + switch (b) { + case Capsule: { + return i32(2); + } + case Box: { + return i32(8); + } + default: { + return i32(0); + } + } + } + case Cylinder: { + return i32( // no box collisions! + 0); + } + case Box: { + *ba = i32(12); + return i32(12); + } + default: { + return i32(0); + } + } +} //////// import: "slmath-matrix3.go" //////// import: "slmath-quaternion.go" +fn MulQuatVector(q: vec4, v: vec3) -> vec3 { + var xyz = vec3(q.x, q.y, q.z); + var t = MulScalar3(Cross3(xyz, v), f32(f32(2)));return v+(MulScalar3(t, q.w))+(Cross3(xyz, t)); +} +fn MulSpatialPoint(xP: vec3, xQ: vec4, p: vec3) -> vec3 { + var dp = MulQuatVector(xQ, p);return dp+(xP); +} //////// import: "slmath-vector3.go" +fn MulScalar3(v: vec3, s: f32) -> vec3 { + return vec3(v.x*s, v.y*s, v.z*s); +} +fn Length3(v: vec3) -> f32 { + return sqrt(v.x*v.x + v.y*v.y + v.z*v.z); +} +fn Cross3(v: vec3,o: vec3) -> vec3 { + return vec3(v.y*o.z-v.z*o.y, v.z*o.x-v.x*o.z, v.x*o.y-v.y*o.x); +} //////// import: "step.go" diff --git a/physics/shaders/DeltasFromJoints.wgsl b/physics/shaders/DeltasFromJoints.wgsl index 5a46bbf6..94651932 100644 --- a/physics/shaders/DeltasFromJoints.wgsl +++ b/physics/shaders/DeltasFromJoints.wgsl @@ -76,6 +76,7 @@ const BodyInvInertiaZY: BodyVars = 35; const BodyInvInertiaXZ: BodyVars = 36; const BodyInvInertiaYZ: BodyVars = 37; const BodyInvInertiaZZ: BodyVars = 38; +const BodyRadius: BodyVars = 39; //////// import: "contact.go" alias ContactVars = i32; //enums:enum @@ -148,7 +149,7 @@ fn SetDynamicAngDelta(idx: i32,cni: i32, angDelta: vec3) { } //////// import: "enumgen.go" -const BodyVarsN: BodyVars = 39; +const BodyVarsN: BodyVars = 40; const ContactVarsN: ContactVars = 16; const JointControlVarsN: JointControlVars = 3; const DynamicVarsN: DynamicVars = 32; @@ -156,7 +157,7 @@ const GPUVarsN: GPUVars = 9; const JointTypesN: JointTypes = 7; const JointVarsN: JointVars = 50; const JointDoFVarsN: JointDoFVars = 7; -const ShapesN: Shapes = 4; +const ShapesN: Shapes = 5; //////// import: "joint.go" const JointLimitUnlimited = 1e10; @@ -253,6 +254,7 @@ struct PhysParams { AngularDamping: f32, ContactWeighting: i32, Restitution: i32, + ContactMargin: f32, Cur: i32, Next: i32, BodiesN: i32, @@ -261,16 +263,16 @@ struct PhysParams { JointDoFsN: i32, BodyJointsMax: i32, BodyCollidePairsN: i32, - pad: i32, Gravity: vec4, } //////// import: "shapes.go" alias Shapes = i32; //enums:enum -const Box: Shapes = 0; +const Plane: Shapes = 0; const Sphere: Shapes = 1; -const Cylinder: Shapes = 2; -const Capsule: Shapes = 3; +const Capsule: Shapes = 2; +const Cylinder: Shapes = 3; +const Box: Shapes = 4; //////// import: "slmath-matrix3.go" diff --git a/physics/shaders/DynamicsCurToNext.wgsl b/physics/shaders/DynamicsCurToNext.wgsl index d06cdf81..a2cf33fa 100644 --- a/physics/shaders/DynamicsCurToNext.wgsl +++ b/physics/shaders/DynamicsCurToNext.wgsl @@ -68,6 +68,7 @@ const BodyInvInertiaZY: BodyVars = 35; const BodyInvInertiaXZ: BodyVars = 36; const BodyInvInertiaYZ: BodyVars = 37; const BodyInvInertiaZZ: BodyVars = 38; +const BodyRadius: BodyVars = 39; //////// import: "contact.go" alias ContactVars = i32; //enums:enum @@ -130,7 +131,7 @@ const DynAngDeltaY: DynamicVars = 30; const DynAngDeltaZ: DynamicVars = 31; //////// import: "enumgen.go" -const BodyVarsN: BodyVars = 39; +const BodyVarsN: BodyVars = 40; const ContactVarsN: ContactVars = 16; const JointControlVarsN: JointControlVars = 3; const DynamicVarsN: DynamicVars = 32; @@ -138,7 +139,7 @@ const GPUVarsN: GPUVars = 9; const JointTypesN: JointTypes = 7; const JointVarsN: JointVars = 50; const JointDoFVarsN: JointDoFVars = 7; -const ShapesN: Shapes = 4; +const ShapesN: Shapes = 5; //////// import: "joint.go" const JointLimitUnlimited = 1e10; @@ -223,6 +224,7 @@ struct PhysParams { AngularDamping: f32, ContactWeighting: i32, Restitution: i32, + ContactMargin: f32, Cur: i32, Next: i32, BodiesN: i32, @@ -231,16 +233,16 @@ struct PhysParams { JointDoFsN: i32, BodyJointsMax: i32, BodyCollidePairsN: i32, - pad: i32, Gravity: vec4, } //////// import: "shapes.go" alias Shapes = i32; //enums:enum -const Box: Shapes = 0; +const Plane: Shapes = 0; const Sphere: Shapes = 1; -const Cylinder: Shapes = 2; -const Capsule: Shapes = 3; +const Capsule: Shapes = 2; +const Cylinder: Shapes = 3; +const Box: Shapes = 4; //////// import: "slmath-matrix3.go" diff --git a/physics/shaders/ForcesFromJoints.wgsl b/physics/shaders/ForcesFromJoints.wgsl index b476fb5b..4366862e 100644 --- a/physics/shaders/ForcesFromJoints.wgsl +++ b/physics/shaders/ForcesFromJoints.wgsl @@ -76,6 +76,7 @@ const BodyInvInertiaZY: BodyVars = 35; const BodyInvInertiaXZ: BodyVars = 36; const BodyInvInertiaYZ: BodyVars = 37; const BodyInvInertiaZZ: BodyVars = 38; +const BodyRadius: BodyVars = 39; //////// import: "contact.go" alias ContactVars = i32; //enums:enum @@ -148,7 +149,7 @@ fn SetDynamicTorque(idx: i32,cni: i32, torque: vec3) { } //////// import: "enumgen.go" -const BodyVarsN: BodyVars = 39; +const BodyVarsN: BodyVars = 40; const ContactVarsN: ContactVars = 16; const JointControlVarsN: JointControlVars = 3; const DynamicVarsN: DynamicVars = 32; @@ -156,7 +157,7 @@ const GPUVarsN: GPUVars = 9; const JointTypesN: JointTypes = 7; const JointVarsN: JointVars = 50; const JointDoFVarsN: JointDoFVars = 7; -const ShapesN: Shapes = 4; +const ShapesN: Shapes = 5; //////// import: "joint.go" const JointLimitUnlimited = 1e10; @@ -253,6 +254,7 @@ struct PhysParams { AngularDamping: f32, ContactWeighting: i32, Restitution: i32, + ContactMargin: f32, Cur: i32, Next: i32, BodiesN: i32, @@ -261,16 +263,16 @@ struct PhysParams { JointDoFsN: i32, BodyJointsMax: i32, BodyCollidePairsN: i32, - pad: i32, Gravity: vec4, } //////// import: "shapes.go" alias Shapes = i32; //enums:enum -const Box: Shapes = 0; +const Plane: Shapes = 0; const Sphere: Shapes = 1; -const Cylinder: Shapes = 2; -const Capsule: Shapes = 3; +const Capsule: Shapes = 2; +const Cylinder: Shapes = 3; +const Box: Shapes = 4; //////// import: "slmath-matrix3.go" diff --git a/physics/shaders/InitDynamics.wgsl b/physics/shaders/InitDynamics.wgsl index ea6d843c..bab5c8e9 100644 --- a/physics/shaders/InitDynamics.wgsl +++ b/physics/shaders/InitDynamics.wgsl @@ -74,6 +74,7 @@ const BodyInvInertiaZY: BodyVars = 35; const BodyInvInertiaXZ: BodyVars = 36; const BodyInvInertiaYZ: BodyVars = 37; const BodyInvInertiaZZ: BodyVars = 38; +const BodyRadius: BodyVars = 39; //////// import: "contact.go" alias ContactVars = i32; //enums:enum @@ -139,7 +140,7 @@ fn DynamicBody(idx: i32) -> i32 { } //////// import: "enumgen.go" -const BodyVarsN: BodyVars = 39; +const BodyVarsN: BodyVars = 40; const ContactVarsN: ContactVars = 16; const JointControlVarsN: JointControlVars = 3; const DynamicVarsN: DynamicVars = 32; @@ -147,7 +148,7 @@ const GPUVarsN: GPUVars = 9; const JointTypesN: JointTypes = 7; const JointVarsN: JointVars = 50; const JointDoFVarsN: JointDoFVars = 7; -const ShapesN: Shapes = 4; +const ShapesN: Shapes = 5; //////// import: "joint.go" const JointLimitUnlimited = 1e10; @@ -232,6 +233,7 @@ struct PhysParams { AngularDamping: f32, ContactWeighting: i32, Restitution: i32, + ContactMargin: f32, Cur: i32, Next: i32, BodiesN: i32, @@ -240,16 +242,16 @@ struct PhysParams { JointDoFsN: i32, BodyJointsMax: i32, BodyCollidePairsN: i32, - pad: i32, Gravity: vec4, } //////// import: "shapes.go" alias Shapes = i32; //enums:enum -const Box: Shapes = 0; +const Plane: Shapes = 0; const Sphere: Shapes = 1; -const Cylinder: Shapes = 2; -const Capsule: Shapes = 3; +const Capsule: Shapes = 2; +const Cylinder: Shapes = 3; +const Box: Shapes = 4; //////// import: "slmath-matrix3.go" diff --git a/physics/shaders/StepBodyDeltas.wgsl b/physics/shaders/StepBodyDeltas.wgsl index dd1885f1..46bd97b7 100644 --- a/physics/shaders/StepBodyDeltas.wgsl +++ b/physics/shaders/StepBodyDeltas.wgsl @@ -74,6 +74,7 @@ const BodyInvInertiaZY: BodyVars = 35; const BodyInvInertiaXZ: BodyVars = 36; const BodyInvInertiaYZ: BodyVars = 37; const BodyInvInertiaZZ: BodyVars = 38; +const BodyRadius: BodyVars = 39; fn BodyCom(idx: i32) -> vec3 { return vec3(Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComX))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComY))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComZ))]); } @@ -185,7 +186,7 @@ fn SetDynamicAngDelta(idx: i32,cni: i32, angDelta: vec3) { } //////// import: "enumgen.go" -const BodyVarsN: BodyVars = 39; +const BodyVarsN: BodyVars = 40; const ContactVarsN: ContactVars = 16; const JointControlVarsN: JointControlVars = 3; const DynamicVarsN: DynamicVars = 32; @@ -193,7 +194,7 @@ const GPUVarsN: GPUVars = 9; const JointTypesN: JointTypes = 7; const JointVarsN: JointVars = 50; const JointDoFVarsN: JointDoFVars = 7; -const ShapesN: Shapes = 4; +const ShapesN: Shapes = 5; //////// import: "joint.go" const JointLimitUnlimited = 1e10; @@ -278,6 +279,7 @@ struct PhysParams { AngularDamping: f32, ContactWeighting: i32, Restitution: i32, + ContactMargin: f32, Cur: i32, Next: i32, BodiesN: i32, @@ -286,16 +288,16 @@ struct PhysParams { JointDoFsN: i32, BodyJointsMax: i32, BodyCollidePairsN: i32, - pad: i32, Gravity: vec4, } //////// import: "shapes.go" alias Shapes = i32; //enums:enum -const Box: Shapes = 0; +const Plane: Shapes = 0; const Sphere: Shapes = 1; -const Cylinder: Shapes = 2; -const Capsule: Shapes = 3; +const Capsule: Shapes = 2; +const Cylinder: Shapes = 3; +const Box: Shapes = 4; //////// import: "slmath-matrix3.go" diff --git a/physics/shaders/StepIntegrateBodies.wgsl b/physics/shaders/StepIntegrateBodies.wgsl index d5403cf7..6c252f71 100644 --- a/physics/shaders/StepIntegrateBodies.wgsl +++ b/physics/shaders/StepIntegrateBodies.wgsl @@ -74,6 +74,7 @@ const BodyInvInertiaZY: BodyVars = 35; const BodyInvInertiaXZ: BodyVars = 36; const BodyInvInertiaYZ: BodyVars = 37; const BodyInvInertiaZZ: BodyVars = 38; +const BodyRadius: BodyVars = 39; fn BodyCom(idx: i32) -> vec3 { return vec3(Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComX))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComY))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComZ))]); } @@ -191,7 +192,7 @@ fn SetDynamicAngDelta(idx: i32,cni: i32, angDelta: vec3) { } //////// import: "enumgen.go" -const BodyVarsN: BodyVars = 39; +const BodyVarsN: BodyVars = 40; const ContactVarsN: ContactVars = 16; const JointControlVarsN: JointControlVars = 3; const DynamicVarsN: DynamicVars = 32; @@ -199,7 +200,7 @@ const GPUVarsN: GPUVars = 9; const JointTypesN: JointTypes = 7; const JointVarsN: JointVars = 50; const JointDoFVarsN: JointDoFVars = 7; -const ShapesN: Shapes = 4; +const ShapesN: Shapes = 5; //////// import: "joint.go" const JointLimitUnlimited = 1e10; @@ -284,6 +285,7 @@ struct PhysParams { AngularDamping: f32, ContactWeighting: i32, Restitution: i32, + ContactMargin: f32, Cur: i32, Next: i32, BodiesN: i32, @@ -292,16 +294,16 @@ struct PhysParams { JointDoFsN: i32, BodyJointsMax: i32, BodyCollidePairsN: i32, - pad: i32, Gravity: vec4, } //////// import: "shapes.go" alias Shapes = i32; //enums:enum -const Box: Shapes = 0; +const Plane: Shapes = 0; const Sphere: Shapes = 1; -const Cylinder: Shapes = 2; -const Capsule: Shapes = 3; +const Capsule: Shapes = 2; +const Cylinder: Shapes = 3; +const Box: Shapes = 4; //////// import: "slmath-matrix3.go" diff --git a/physics/shaders/StepJointForces.wgsl b/physics/shaders/StepJointForces.wgsl index 1b560b30..925da1de 100644 --- a/physics/shaders/StepJointForces.wgsl +++ b/physics/shaders/StepJointForces.wgsl @@ -80,6 +80,7 @@ const BodyInvInertiaZY: BodyVars = 35; const BodyInvInertiaXZ: BodyVars = 36; const BodyInvInertiaYZ: BodyVars = 37; const BodyInvInertiaZZ: BodyVars = 38; +const BodyRadius: BodyVars = 39; fn BodyCom(idx: i32) -> vec3 { return vec3(Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComX))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComY))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComZ))]); } @@ -157,7 +158,7 @@ fn DynamicQuat(idx: i32,cni: i32) -> vec4 { } //////// import: "enumgen.go" -const BodyVarsN: BodyVars = 39; +const BodyVarsN: BodyVars = 40; const ContactVarsN: ContactVars = 16; const JointControlVarsN: JointControlVars = 3; const DynamicVarsN: DynamicVars = 32; @@ -165,7 +166,7 @@ const GPUVarsN: GPUVars = 9; const JointTypesN: JointTypes = 7; const JointVarsN: JointVars = 50; const JointDoFVarsN: JointDoFVars = 7; -const ShapesN: Shapes = 4; +const ShapesN: Shapes = 5; //////// import: "joint.go" const JointLimitUnlimited = 1e10; @@ -303,6 +304,7 @@ struct PhysParams { AngularDamping: f32, ContactWeighting: i32, Restitution: i32, + ContactMargin: f32, Cur: i32, Next: i32, BodiesN: i32, @@ -311,16 +313,16 @@ struct PhysParams { JointDoFsN: i32, BodyJointsMax: i32, BodyCollidePairsN: i32, - pad: i32, Gravity: vec4, } //////// import: "shapes.go" alias Shapes = i32; //enums:enum -const Box: Shapes = 0; +const Plane: Shapes = 0; const Sphere: Shapes = 1; -const Cylinder: Shapes = 2; -const Capsule: Shapes = 3; +const Capsule: Shapes = 2; +const Cylinder: Shapes = 3; +const Box: Shapes = 4; //////// import: "slmath-matrix3.go" diff --git a/physics/shaders/StepSolveJoints.wgsl b/physics/shaders/StepSolveJoints.wgsl index 6383b6ea..412e3346 100644 --- a/physics/shaders/StepSolveJoints.wgsl +++ b/physics/shaders/StepSolveJoints.wgsl @@ -80,6 +80,7 @@ const BodyInvInertiaZY: BodyVars = 35; const BodyInvInertiaXZ: BodyVars = 36; const BodyInvInertiaYZ: BodyVars = 37; const BodyInvInertiaZZ: BodyVars = 38; +const BodyRadius: BodyVars = 39; fn BodyCom(idx: i32) -> vec3 { return vec3(Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComX))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComY))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComZ))]); } @@ -168,7 +169,7 @@ fn DynamicAngDelta(idx: i32,cni: i32) -> vec3 { } //////// import: "enumgen.go" -const BodyVarsN: BodyVars = 39; +const BodyVarsN: BodyVars = 40; const ContactVarsN: ContactVars = 16; const JointControlVarsN: JointControlVars = 3; const DynamicVarsN: DynamicVars = 32; @@ -176,7 +177,7 @@ const GPUVarsN: GPUVars = 9; const JointTypesN: JointTypes = 7; const JointVarsN: JointVars = 50; const JointDoFVarsN: JointDoFVars = 7; -const ShapesN: Shapes = 4; +const ShapesN: Shapes = 5; //////// import: "joint.go" const JointLimitUnlimited = 1e10; @@ -323,6 +324,7 @@ struct PhysParams { AngularDamping: f32, ContactWeighting: i32, Restitution: i32, + ContactMargin: f32, Cur: i32, Next: i32, BodiesN: i32, @@ -331,16 +333,16 @@ struct PhysParams { JointDoFsN: i32, BodyJointsMax: i32, BodyCollidePairsN: i32, - pad: i32, Gravity: vec4, } //////// import: "shapes.go" alias Shapes = i32; //enums:enum -const Box: Shapes = 0; +const Plane: Shapes = 0; const Sphere: Shapes = 1; -const Cylinder: Shapes = 2; -const Capsule: Shapes = 3; +const Capsule: Shapes = 2; +const Cylinder: Shapes = 3; +const Box: Shapes = 4; //////// import: "slmath-matrix3.go" diff --git a/physics/shapes.go b/physics/shapes.go index 0bfb14d8..484717df 100644 --- a/physics/shapes.go +++ b/physics/shapes.go @@ -10,6 +10,8 @@ import "cogentcore.org/core/math32" //gosl:start +// newton: geometry/types.py + // Shapes are elemental shapes for rigid bodies. // In general, size dimensions are half values // (e.g., radius, half-height, etc), which is natural for @@ -17,43 +19,117 @@ import "cogentcore.org/core/math32" type Shapes int32 //enums:enum const ( - // Box is a 3D rectalinear shape. - // The sizes are _half_ sizes along each dimension, - // relative to the center. - Box Shapes = iota + // Plane cannot be a dynamic shape, but is most efficient for + // collision computations. Use size = 0 for an infinite plane. + Plane Shapes = iota // Sphere. SizeX is the radius. Sphere + // Capsule, which is a cylinder with half-spheres on the ends. + // Natively oriented vertically along the Y axis. + // SizeX = bottom radius, SizeY = half-height, SizeZ = top radius. + Capsule + + // todo: Ellipsoid goes here + // Cylinder, natively oriented vertically along the Y axis. // If one radius is 0, then it is a cone. // SizeX = bottom radius, SizeY = half-height in Y axis, SizeZ = top radius. + // Cylinder can not collide with a Box. Cylinder - // Capsule, which is a cylinder with half-spheres on the ends. - // Natively oriented vertically along the Y axis. - // SizeX = bottom radius, SizeY = half-height, SizeZ = top radius. - Capsule + // Box is a 3D rectalinear shape. + // The sizes are _half_ sizes along each dimension, + // relative to the center. + Box ) +// ShapePairContacts returns the number of contact points possible +// for given pair of shapes. a <= b ordering. returns from a to b, +// ba is from b to a (mostly 0). +// infPlane means that a is a Plane and it is infinite (size = 0). +func ShapePairContacts(a, b Shapes, infPlane bool, ba *int32) int32 { + *ba = 0 + switch a { + case Plane: + switch b { + case Plane: + return 0 + case Sphere: + return 1 + case Capsule: + if infPlane { + return 2 + } else { + return 2 + 4 + } + case Cylinder: + return 4 + case Box: + if infPlane { + return 8 + } else { + return 8 + 4 + } + default: + return 0 + } + case Sphere: + return 1 + case Capsule: + switch b { + case Capsule: + return 2 + case Box: + return 8 + default: + return 0 + } + case Cylinder: + return 0 // no box collisions! + case Box: + *ba = 12 + return 12 + default: + return 0 + } +} + //gosl:end +// Radius returns the shape radius for given size. +// this is used for broad-phase collision. +func (sh Shapes) Radius(sz math32.Vector3) float32 { + switch sh { + case Plane: + if sz.X > 0 { + return sz.Length() + } + return 1.0e6 // infinite + case Sphere: + return sz.X + case Capsule, Cylinder: + return max(sz.X, sz.Z) + sz.Y // over-estimate for cylinder + case Box: + return sz.Length() + } + return 0 +} + // BBox returns the bounding box for shape of given size. func (sh Shapes) BBox(sz math32.Vector3) math32.Box3 { var bb math32.Box3 - switch sh { - case Box: - bb.SetMinMax(sz.Negate(), sz) case Sphere: bb.SetMinMax(math32.Vec3(-sz.X, -sz.X, -sz.X), math32.Vec3(sz.X, sz.X, sz.X)) - case Cylinder: - bb.SetMinMax(math32.Vec3(-sz.X, -sz.Y, -sz.X), math32.Vec3(sz.Z, sz.Y, sz.Z)) case Capsule: bb.SetMinMax(math32.Vec3(-sz.X, -sz.Y-sz.X, -sz.X), math32.Vec3(sz.Z, sz.Y+sz.Z, sz.Z)) + case Cylinder: + bb.SetMinMax(math32.Vec3(-sz.X, -sz.Y, -sz.X), math32.Vec3(sz.Z, sz.Y, sz.Z)) + case Box: + bb.SetMinMax(sz.Negate(), sz) } - // bb.Area = 2*sz.X + 2*sz.Y + 2*sz.Z - // bb.Volume = sz.X * sz.Y * sz.Z return bb } diff --git a/physics/typegen.go b/physics/typegen.go index 2acb7228..b21775e3 100644 --- a/physics/typegen.go +++ b/physics/typegen.go @@ -24,10 +24,10 @@ var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.JointVars", var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.JointDoFVars", IDName: "joint-do-f-vars", Doc: "JointDoFVars are joint DoF state variables stored in tensor.Float32,\none for each DoF."}) -var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.PhysParams", IDName: "phys-params", Doc: "PhysParams are the physics parameters", Directives: []types.Directive{{Tool: "gosl", Directive: "start"}}, Fields: []types.Field{{Name: "Iterations", Doc: "Iterations is the number of iterations to perform."}, {Name: "Dt", Doc: "Dt is the integration stepsize."}, {Name: "SoftRelax", Doc: "SoftRelax is soft-body relaxation constant."}, {Name: "JointLinearRelax", Doc: "JointLinearRelax is joint linear relaxation constant."}, {Name: "JointAngularRelax", Doc: "JointAngularRelax is joint angular relaxation constant."}, {Name: "JointLinearComply", Doc: "JointLinearComply is joint linear compliance constant."}, {Name: "JointAngularComply", Doc: "JointAngularComply is joint angular compliance constant."}, {Name: "ContactRelax", Doc: "ContactRelax is rigid contact relaxation constant."}, {Name: "AngularDamping", Doc: "AngularDamping is damping of angular motion."}, {Name: "ContactWeighting", Doc: "Contact weighting"}, {Name: "Restitution", Doc: "Restitution"}, {Name: "Cur", Doc: "Index for the current state (0 or 1, alternates with Next)."}, {Name: "Next", Doc: "Index for the next state (1 or 0, alternates with Cur)."}, {Name: "BodiesN", Doc: "BodiesN is number of rigid bodies."}, {Name: "DynamicsN", Doc: "DynamicsN is number of dynamics bodies."}, {Name: "JointsN", Doc: "JointsN is number of joints."}, {Name: "JointDoFsN", Doc: "JointDoFsN is number of joint DoFs."}, {Name: "BodyJointsMax", Doc: "BodyJointsMax is max number of joints per body + 1 for actual n."}, {Name: "BodyCollidePairsN", Doc: "BodyCollidePairsN is the total number of pre-compiled collision pairs\nto examine."}, {Name: "pad"}, {Name: "Gravity", Doc: "Gravity is the gravity acceleration function"}}}) +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.PhysParams", IDName: "phys-params", Doc: "PhysParams are the physics parameters", Directives: []types.Directive{{Tool: "gosl", Directive: "start"}}, Fields: []types.Field{{Name: "Iterations", Doc: "Iterations is the number of iterations to perform."}, {Name: "Dt", Doc: "Dt is the integration stepsize."}, {Name: "SoftRelax", Doc: "SoftRelax is soft-body relaxation constant."}, {Name: "JointLinearRelax", Doc: "JointLinearRelax is joint linear relaxation constant."}, {Name: "JointAngularRelax", Doc: "JointAngularRelax is joint angular relaxation constant."}, {Name: "JointLinearComply", Doc: "JointLinearComply is joint linear compliance constant."}, {Name: "JointAngularComply", Doc: "JointAngularComply is joint angular compliance constant."}, {Name: "ContactRelax", Doc: "ContactRelax is rigid contact relaxation constant."}, {Name: "AngularDamping", Doc: "AngularDamping is damping of angular motion."}, {Name: "ContactWeighting", Doc: "Contact weighting"}, {Name: "Restitution", Doc: "Restitution"}, {Name: "ContactMargin", Doc: "Contact margin is the extra distance for broadphase collision\naround rigid bodies."}, {Name: "Cur", Doc: "Index for the current state (0 or 1, alternates with Next)."}, {Name: "Next", Doc: "Index for the next state (1 or 0, alternates with Cur)."}, {Name: "BodiesN", Doc: "BodiesN is number of rigid bodies."}, {Name: "DynamicsN", Doc: "DynamicsN is number of dynamics bodies."}, {Name: "JointsN", Doc: "JointsN is number of joints."}, {Name: "JointDoFsN", Doc: "JointDoFsN is number of joint DoFs."}, {Name: "BodyJointsMax", Doc: "BodyJointsMax is max number of joints per body + 1 for actual n."}, {Name: "BodyCollidePairsN", Doc: "BodyCollidePairsN is the total number of pre-compiled collision pairs\nto examine."}, {Name: "Gravity", Doc: "Gravity is the gravity acceleration function"}}}) -var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.Shapes", IDName: "shapes", Doc: "Shapes are elemental shapes for rigid bodies."}) +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.Shapes", IDName: "shapes", Doc: "Shapes are elemental shapes for rigid bodies.\nIn general, size dimensions are half values\n(e.g., radius, half-height, etc), which is natural for\ncenter-based body coordinates."}) var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.State", IDName: "state", Doc: "State contains the basic physical state including position, orientation, velocity.\nThese are only the values that can be either relative or absolute -- other physical\nstate values such as Mass should go in Rigid.", Methods: []types.Method{{Name: "MoveOnAxis", Doc: "MoveOnAxis moves (translates) the specified distance on the specified local axis,\nrelative to the current rotation orientation.\nThe axis is normalized prior to aplying the distance factor.\nSets the LinVel to motion vector.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z", "dist"}}, {Name: "MoveOnAxisAbs", Doc: "MoveOnAxisAbs moves (translates) the specified distance on the specified local axis,\nin absolute X,Y,Z coordinates (does not apply the Quat rotation factor.\nThe axis is normalized prior to aplying the distance factor.\nSets the LinVel to motion vector.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z", "dist"}}, {Name: "SetEulerRotation", Doc: "SetEulerRotation sets the rotation in Euler angles (degrees).", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z"}}, {Name: "EulerRotation", Doc: "EulerRotation returns the current rotation in Euler angles (degrees).", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Returns: []string{"Vector3"}}, {Name: "SetAxisRotation", Doc: "SetAxisRotation sets rotation from local axis and angle in degrees.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z", "angle"}}, {Name: "RotateOnAxis", Doc: "RotateOnAxis rotates around the specified local axis the specified angle in degrees.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z", "angle"}}, {Name: "RotateEuler", Doc: "RotateEuler rotates by given Euler angles (in degrees) relative to existing rotation.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z"}}}, Fields: []types.Field{{Name: "Pos", Doc: "Pos is the position of center of mass of object."}, {Name: "Quat", Doc: "Quat is the rotation specified as a quaternion."}, {Name: "LinVel", Doc: "LinVel is the linear velocity."}, {Name: "AngVel", Doc: "AngVel is the angular velocity."}}}) -var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.World", IDName: "world", Doc: "World contains and manages all of the physics elements.", Directives: []types.Directive{{Tool: "go", Directive: "generate", Args: []string{"core", "generate", "-add-types", "-gosl"}}}, Fields: []types.Field{{Name: "GPU", Doc: "GPU determines whether to use GPU (else CPU)."}, {Name: "Params", Doc: "Params are global parameters."}, {Name: "Bodies", Doc: "Bodies are the rigid body elements (dynamic and static),\nspecifying the constant, non-dynamic properties,\nwhich is initial state for dynamics.\n[body][BodyVarsN]"}, {Name: "Joints", Doc: "Joints is a list of permanent joints connecting bodies,\nwhich do not change (no dynamic variables).\n[joint][JointVarsN]"}, {Name: "JointDoFs", Doc: "JointDoFs is a list of joint DoF parameters, allocated per joint.\n[dof][JointDoFVars]"}, {Name: "BodyJoints", Doc: "BodyJoints is a list of joint indexes for each dynamic body, for aggregating.\n[dyn body][parent, child][Params.BodyJointsMax]"}, {Name: "BodyCollidePairs", Doc: "BodyCollidePairs are pairs of Body indexes that could potentially collide\nbased on precomputed collision logic, using World, Group, and Joint indexes.\n[BodyCollidePairsN][2]"}, {Name: "Dynamics", Doc: "Dynamics are the dynamic rigid body elements: these actually move.\nThe first set of variables are for initial values, and the second current.\n[body][cur/next][DynamicVarsN]"}, {Name: "Contacts", Doc: "Contacts are points of contact between bodies.\n[contact][ContactVarsN]"}, {Name: "JointControls", Doc: "JointControls are dynamic joint control inputs, per joint DoF\n(in correspondence with [JointDoFs]). This can be uploaded to the\nGPU at every step.\n[dof][JointControlVarsN]"}}}) +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.World", IDName: "world", Doc: "World contains and manages all of the physics elements.", Directives: []types.Directive{{Tool: "go", Directive: "generate", Args: []string{"core", "generate", "-add-types", "-gosl"}}}, Fields: []types.Field{{Name: "GPU", Doc: "GPU determines whether to use GPU (else CPU)."}, {Name: "Params", Doc: "Params are global parameters."}, {Name: "Bodies", Doc: "Bodies are the rigid body elements (dynamic and static),\nspecifying the constant, non-dynamic properties,\nwhich is initial state for dynamics.\n[body][BodyVarsN]"}, {Name: "Joints", Doc: "Joints is a list of permanent joints connecting bodies,\nwhich do not change (no dynamic variables).\n[joint][JointVarsN]"}, {Name: "JointDoFs", Doc: "JointDoFs is a list of joint DoF parameters, allocated per joint.\n[dof][JointDoFVars]"}, {Name: "BodyJoints", Doc: "BodyJoints is a list of joint indexes for each dynamic body, for aggregating.\n[dyn body][parent, child][Params.BodyJointsMax]"}, {Name: "BodyCollidePairs", Doc: "BodyCollidePairs are pairs of Body indexes that could potentially collide\nbased on precomputed collision logic, using World, Group, and Joint indexes.\nThe last entry is updated to contain the actual number of contacts generated\non each collision iteration.\n[BodyCollidePairsN+1][2]"}, {Name: "Dynamics", Doc: "Dynamics are the dynamic rigid body elements: these actually move.\nThe first set of variables are for initial values, and the second current.\n[body][cur/next][DynamicVarsN]"}, {Name: "Contacts", Doc: "Contacts are points of contact between bodies. Max possible size\nis BodyCollidePairsN, but actual size on given run is updated\ninto last entry in BodyCollidePairs.\n[BodyCollidePairsN][ContactVarsN]"}, {Name: "JointControls", Doc: "JointControls are dynamic joint control inputs, per joint DoF\n(in correspondence with [JointDoFs]). This can be uploaded to the\nGPU at every step.\n[dof][JointControlVarsN]"}}}) diff --git a/physics/world.go b/physics/world.go index 3d394c50..cef2097b 100644 --- a/physics/world.go +++ b/physics/world.go @@ -102,7 +102,11 @@ func (wl *World) NewBody(shape Shapes, size, pos math32.Vector3, rot math32.Quat } // NewDynamic adds a new dynamic body with given parameters. Returns the index. +// Shape cannot be [Plane]. func (wl *World) NewDynamic(shape Shapes, mass float32, size, pos math32.Vector3, rot math32.Quat) (bodyIdx, dynIdx int32) { + if shape == Plane { + panic("physics.NewDynamic: shape cannot be Plane") + } bodyIdx = wl.NewBody(shape, size, pos, rot) sizes := wl.Dynamics.ShapeSizes() dynIdx = int32(sizes[0]) diff --git a/physics/world.goal b/physics/world.goal index 1241d722..8128a462 100644 --- a/physics/world.goal +++ b/physics/world.goal @@ -100,7 +100,11 @@ func (wl *World) NewBody(shape Shapes, size, pos math32.Vector3, rot math32.Quat } // NewDynamic adds a new dynamic body with given parameters. Returns the index. +// Shape cannot be [Plane]. func (wl *World) NewDynamic(shape Shapes, mass float32, size, pos math32.Vector3, rot math32.Quat) (bodyIdx, dynIdx int32) { + if shape == Plane { + panic("physics.NewDynamic: shape cannot be Plane") + } bodyIdx = wl.NewBody(shape, size, pos, rot) sizes := wl.Dynamics.ShapeSizes() dynIdx = int32(sizes[0]) diff --git a/physics/world/view.go b/physics/world/view.go index f91d287f..8c6cb557 100644 --- a/physics/world/view.go +++ b/physics/world/view.go @@ -118,14 +118,16 @@ func (vw *View) Init(sld *xyz.Solid) { return } switch vw.Shape { - case physics.Box: - vw.BoxInit(sld) - case physics.Cylinder: - vw.CylinderInit(sld) - case physics.Capsule: - vw.CapsuleInit(sld) + case physics.Plane: + vw.PlaneInit(sld) case physics.Sphere: vw.SphereInit(sld) + case physics.Capsule: + vw.CapsuleInit(sld) + case physics.Cylinder: + vw.CylinderInit(sld) + case physics.Box: + vw.BoxInit(sld) } } @@ -139,6 +141,27 @@ func (vw *View) BoxInit(sld *xyz.Solid) { } sld.SetMeshName(mnm) sld.Pose.Scale = vw.Size.MulScalar(2) + vw.UpdateColor(vw.Color, s6ld) + sld.Updater(func() { + vw.UpdatePose(sld) + }) +} + +// PlaneInit is the default InitView function for [physics.Plane]. +// Only updates Pose in Updater: if node will change size or color, +// add updaters for that. +func (vw *View) PlaneInit(sld *xyz.Solid) { + mnm := "physics.Plane" + if ms, _ := sld.Scene.MeshByName(mnm); ms == nil { + xyz.NewPlane(sld.Scene, mnm, 1, 1) + } + sld.SetMeshName(mnm) + if vw.Size.X == 0 { + inf := 1e6 + sld.Pose.Scale = math32.Vec3(inf, inf, 1) + } else { + sld.Pose.Scale = vw.Size.MulScalar(2) + } vw.UpdateColor(vw.Color, sld) sld.Updater(func() { vw.UpdatePose(sld) From ea19b62775b14e2090def9ea02fc0b6ecff65387 Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Sat, 20 Dec 2025 00:34:11 +0100 Subject: [PATCH 27/97] physics: gosl:wgsl sections finally working -- too many red herrings -- ugh. --- goal/testdata/test.goal | 3 + goal/transpile/transpile_test.go | 17 +++-- gosl/gotosl/extract.go | 85 +++++++----------------- gosl/gotosl/nodes.go | 16 ++++- gosl/gotosl/sledits.go | 1 + gosl/gotosl/testdata/basic.go | 4 ++ gosl/gotosl/testdata/basic.goal | 4 ++ gosl/gotosl/typegen.go | 2 +- physics/contact.go | 25 ++++++- physics/contact.goal | 25 ++++++- physics/enumgen.go | 10 +-- physics/gosl.go | 29 ++++++-- physics/shaders/CollisionBroad.wgsl | 27 +++++++- physics/shaders/DeltasFromJoints.wgsl | 2 +- physics/shaders/DynamicsCurToNext.wgsl | 2 +- physics/shaders/ForcesFromJoints.wgsl | 2 +- physics/shaders/InitDynamics.wgsl | 2 +- physics/shaders/StepBodyDeltas.wgsl | 2 +- physics/shaders/StepIntegrateBodies.wgsl | 2 +- physics/shaders/StepJointForces.wgsl | 4 +- physics/shaders/StepSolveJoints.wgsl | 4 +- physics/typegen.go | 2 +- physics/vars.go | 11 ++- physics/world.go | 16 +++-- physics/world.goal | 16 +++-- 25 files changed, 196 insertions(+), 117 deletions(-) diff --git a/goal/testdata/test.goal b/goal/testdata/test.goal index a93be7ec..c5818450 100644 --- a/goal/testdata/test.goal +++ b/goal/testdata/test.goal @@ -9,6 +9,9 @@ fmt.Println(x) fmt.Println(nd) fmt.Println(sz) fmt.Println(sh) +//gosl:wgsl +// a += 5 +//gosl:end type MyStru struct { Name string diff --git a/goal/transpile/transpile_test.go b/goal/transpile/transpile_test.go index e64c5a46..62ff16ec 100644 --- a/goal/transpile/transpile_test.go +++ b/goal/transpile/transpile_test.go @@ -5,12 +5,10 @@ package transpile import ( - "fmt" "testing" _ "cogentcore.org/lab/stats/metric" _ "cogentcore.org/lab/stats/stats" - "cogentcore.org/lab/tensor" _ "cogentcore.org/lab/tensor/tmath" "github.com/stretchr/testify/assert" ) @@ -255,10 +253,14 @@ func TestCur(t *testing.T) { func TestCurCode(t *testing.T) { // logx.UserLevel = slog.LevelDebug tests := []exIn{ - {`## totalTime := 100 -driver := zeros(totalTime)`, - `totalTime := tensor.Tensor(tensor.NewIntScalar(100)) -driver := tensor.Tensor(tensor.NewFloat64(tensor.AsIntSlice(totalTime) ...))`}, + {` b := 5 + //gosl:wgsl + // a := 2 + //gosl:end`, + `b := 5 +//gosl:wgsl +// a := 2 +//gosl:end`}, } for _, test := range tests { st := NewState() @@ -267,9 +269,6 @@ driver := tensor.Tensor(tensor.NewFloat64(tensor.AsIntSlice(totalTime) ...))`}, o := st.Code() assert.Equal(t, test.e, o) } - totalTime := tensor.Tensor(tensor.NewIntScalar(100)) - driver := tensor.Tensor(tensor.NewFloat64(tensor.AsIntSlice(totalTime)...)) - fmt.Println(driver) } func TestMath(t *testing.T) { diff --git a/gosl/gotosl/extract.go b/gosl/gotosl/extract.go index 8291ad09..559ba965 100644 --- a/gosl/gotosl/extract.go +++ b/gosl/gotosl/extract.go @@ -61,10 +61,11 @@ func (st *State) ExtractGosl(lines [][]byte) (outLines [][]byte, hasVars bool) { imp := []byte("import") kernel := []byte("//gosl:kernel") fnc := []byte("func") + comment := []byte("// ") inReg := false - inHlsl := false - inNoHlsl := false + inWgsl := false + inNoWgsl := false for li, ln := range lines { tln := bytes.TrimSpace(ln) isKey := bytes.HasPrefix(tln, key) @@ -75,15 +76,28 @@ func (st *State) ExtractGosl(lines [][]byte) (outLines [][]byte, hasVars bool) { } switch { case inReg && isKey && bytes.HasPrefix(keyStr, end): - if inHlsl || inNoHlsl { - outLines = append(outLines, ln) + if inWgsl || inNoWgsl { + inWgsl = false + inNoWgsl = false + } else { + inReg = false } - inReg = false - inHlsl = false - inNoHlsl = false case inReg && isKey && bytes.HasPrefix(keyStr, vars): hasVars = true outLines = append(outLines, ln) + case isKey && bytes.HasPrefix(keyStr, nowgsl): + inReg = true + inNoWgsl = true + outLines = append(outLines, ln) // key to include self here + case isKey && bytes.HasPrefix(keyStr, wgsl): + inReg = true + inWgsl = true + case inWgsl: + if bytes.HasPrefix(tln, comment) { + outLines = append(outLines, tln[3:]) + } else { + outLines = append(outLines, ln) + } case inReg: for pkg := range st.ImportPackages { // remove package prefixes if !bytes.Contains(ln, imp) { @@ -131,14 +145,6 @@ func (st *State) ExtractGosl(lines [][]byte) (outLines [][]byte, hasVars bool) { outLines = append(outLines, ln) case isKey && bytes.HasPrefix(keyStr, start): inReg = true - case isKey && bytes.HasPrefix(keyStr, nowgsl): - inReg = true - inNoHlsl = true - outLines = append(outLines, ln) // key to include self here - case isKey && bytes.HasPrefix(keyStr, wgsl): - inReg = true - inHlsl = true - outLines = append(outLines, ln) } } return @@ -169,16 +175,9 @@ func (st *State) AppendGoHeader(lines [][]byte) [][]byte { return olns } -// ExtractWGSL extracts the WGSL code embedded within .Go files, -// which is commented out in the Go code -- remove comments. +// ExtractWGSL extracts key stuff for WGSL code, not package +// and import directives. func (st *State) ExtractWGSL(lines [][]byte) [][]byte { - key := []byte("//gosl:") - wgsl := []byte("wgsl") - nowgsl := []byte("nowgsl") - end := []byte("end") - stComment := []byte("/*") - edComment := []byte("*/") - comment := []byte("// ") pack := []byte("package") imp := []byte("import") lparen := []byte("(") @@ -204,43 +203,5 @@ func (st *State) ExtractWGSL(lines [][]byte) [][]byte { } lines = lines[stln:] // get rid of package, import - - inHlsl := false - inNoHlsl := false - noHlslStart := 0 - for li := 0; li < len(lines); li++ { - ln := lines[li] - isKey := bytes.HasPrefix(ln, key) - var keyStr []byte - if isKey { - keyStr = ln[len(key):] - // fmt.Printf("key: %s\n", string(keyStr)) - } - switch { - case inNoHlsl && isKey && bytes.HasPrefix(keyStr, end): - lines = slices.Delete(lines, noHlslStart, li+1) - li -= ((li + 1) - noHlslStart) - inNoHlsl = false - case inHlsl && isKey && bytes.HasPrefix(keyStr, end): - lines = slices.Delete(lines, li, li+1) - li-- - inHlsl = false - case inHlsl: - switch { - case bytes.HasPrefix(ln, stComment) || bytes.HasPrefix(ln, edComment): - lines = slices.Delete(lines, li, li+1) - li-- - case bytes.HasPrefix(ln, comment): - lines[li] = ln[3:] - } - case isKey && bytes.HasPrefix(keyStr, wgsl): - inHlsl = true - lines = slices.Delete(lines, li, li+1) - li-- - case isKey && bytes.HasPrefix(keyStr, nowgsl): - inNoHlsl = true - noHlslStart = li - } - } return lines } diff --git a/gosl/gotosl/nodes.go b/gosl/gotosl/nodes.go index 3825e803..ca6f0217 100644 --- a/gosl/gotosl/nodes.go +++ b/gosl/gotosl/nodes.go @@ -1719,7 +1719,18 @@ func (p *printer) getNamedType(typ types.Type) (*types.Named, error) { func (p *printer) globalVarBasic(idx *ast.IndexExpr) { id, ok := idx.X.(*ast.Ident) if !ok { - return + if sel, ok := idx.X.(*ast.SelectorExpr); ok { + if sel.Sel.Name != "Values" { + return + } + id, ok = sel.X.(*ast.Ident) + if !ok { + return + } + // fall through with this.. + } else { + return + } } st := p.GoToSL gvr := st.GlobalVar(id.Name) @@ -1728,6 +1739,9 @@ func (p *printer) globalVarBasic(idx *ast.IndexExpr) { } if p.curFunc != nil { p.curFunc.AddVarUsed(gvr) + if p.curMethIsAtomic { + p.curFunc.AddAtomic(gvr) + } } } diff --git a/gosl/gotosl/sledits.go b/gosl/gotosl/sledits.go index 08f5f39d..b39b63af 100644 --- a/gosl/gotosl/sledits.go +++ b/gosl/gotosl/sledits.go @@ -56,6 +56,7 @@ var Replaces = []Replace{ {[]byte("math32.Vec4"), []byte("vec4")}, {[]byte("math32.NewQuat"), []byte("vec4")}, {[]byte("math32.Mat3"), []byte("mat3x3f")}, + {[]byte(".Values["), []byte("[")}, {[]byte("float32"), []byte("f32")}, {[]byte("float64"), []byte("f64")}, // TODO: not yet supported {[]byte("uint32"), []byte("u32")}, diff --git a/gosl/gotosl/testdata/basic.go b/gosl/gotosl/testdata/basic.go index fd079bd8..d68613bf 100644 --- a/gosl/gotosl/testdata/basic.go +++ b/gosl/gotosl/testdata/basic.go @@ -163,6 +163,7 @@ func (ps *ParamStruct) IntegFromRaw(idx int) float32 { integ += newVal Data.Set(integ, int(idx), int(Integ)) Data.Set(math32.Exp(-integ), int(idx), int(Exp)) + Data.Values[idx*2] = 55.0 a := ps.VXYf.X b := ps.VXYf.V() @@ -177,6 +178,9 @@ func (ps *ParamStruct) IntegFromRaw(idx int) float32 { ctx := GetCtx(0) ps.AnotherMeth(ctx, idx, &a) bv := Big.Value(int(idx), int(Integ)) + //gosl:wgsl + // bv *= 2 // will be uncommented in wgsl + //gosl:end Big.Set(bv*2+c.Y+d.Z, int(idx), int(Exp)) return Data.Value(int(idx), int(Exp)) } diff --git a/gosl/gotosl/testdata/basic.goal b/gosl/gotosl/testdata/basic.goal index 87e4da7f..77a5df1e 100644 --- a/gosl/gotosl/testdata/basic.goal +++ b/gosl/gotosl/testdata/basic.goal @@ -156,6 +156,7 @@ func (ps *ParamStruct) IntegFromRaw(idx int) float32 { integ += newVal Data[idx, Integ] = integ Data[idx, Exp] = math32.Exp(-integ) + Data.Values[idx*2] = 55.0 a := ps.VXYf.X b := ps.VXYf.V() @@ -170,6 +171,9 @@ func (ps *ParamStruct) IntegFromRaw(idx int) float32 { ctx := GetCtx(0) ps.AnotherMeth(ctx, idx, &a) bv := Big[idx, Integ] + //gosl:wgsl + // bv *= 2 // will be uncommented in wgsl + //gosl:end Big[idx, Exp] = bv*2 + c.Y + d.Z return Data[idx, Exp] } diff --git a/gosl/gotosl/typegen.go b/gosl/gotosl/typegen.go index 1ffde584..56094949 100644 --- a/gosl/gotosl/typegen.go +++ b/gosl/gotosl/typegen.go @@ -8,7 +8,7 @@ import ( var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/gosl/gotosl.Function", IDName: "function", Doc: "Function represents the call graph of functions", Fields: []types.Field{{Name: "Name"}, {Name: "Funcs"}, {Name: "Atomics"}, {Name: "VarsUsed"}}}) -var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/gosl/gotosl.Config", IDName: "config", Doc: "Config has the configuration info for the gosl system.", Fields: []types.Field{{Name: "Output", Doc: "Output is the output directory for shader code,\nrelative to where gosl is invoked; must not be an empty string."}, {Name: "Exclude", Doc: "Exclude is a comma-separated list of names of functions to exclude from exporting to WGSL."}, {Name: "Keep", Doc: "Keep keeps temporary converted versions of the source files, for debugging."}, {Name: "Debug", Doc: "Debug enables debugging messages while running."}, {Name: "MaxBufferSize", Doc: "MaxBufferSize is the maximum size for any buffer.\nThis is often platform-dependent, but is needed for\naccessing variables that have multiple buffers.\nIt is compiled into the kernel code as a constant,\nand must fit in a uint32 number.\nFor web, the number is 134217728 = 128 MiB."}}}) +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/gosl/gotosl.Config", IDName: "config", Doc: "Config has the configuration info for the gosl system.", Fields: []types.Field{{Name: "Output", Doc: "Output is the output directory for shader code,\nrelative to where gosl is invoked; must not be an empty string."}, {Name: "Exclude", Doc: "Exclude is a comma-separated list of names of functions to exclude from exporting to WGSL."}, {Name: "Keep", Doc: "Keep keeps temporary converted versions of the source files, for debugging."}, {Name: "Debug", Doc: "Debug enables debugging messages while running."}, {Name: "MaxBufferSize", Doc: "MaxBufferSize is the maximum size for any buffer.\nThis is often platform-dependent, but is needed for\naccessing variables that have multiple buffers.\nIt is compiled into the kernel code as a constant,\nand must fit in a uint32 number.\nThe default is 32 byte aligned down version of 2147483647 max for nvidia"}}}) var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/gosl/gotosl.System", IDName: "system", Doc: "System represents a ComputeSystem, and its kernels and variables.", Fields: []types.Field{{Name: "Name"}, {Name: "Kernels", Doc: "Kernels are the kernels using this compute system."}, {Name: "Groups", Doc: "Groups are the variables for this compute system."}, {Name: "NTensors", Doc: "NTensors is the number of tensor vars."}}}) diff --git a/physics/contact.go b/physics/contact.go index d0eec0ad..2482cfad 100644 --- a/physics/contact.go +++ b/physics/contact.go @@ -8,6 +8,7 @@ package physics import ( "fmt" + "sync/atomic" "cogentcore.org/core/math32" "cogentcore.org/lab/gosl/slmath" @@ -146,9 +147,27 @@ func CollisionBroad(i uint32) { //gosl:kernel // contact_point_limit[pair_index_ba] = 0 // now we just write results to big buffer of contact points. - // Key point is that Contacts needs to have up to 12 elements per pair possible! - // just allocate in order. + nci := atomic.AddInt32(&ContactCount.Values[int(params.Cur)], ncA+ncB) // returns previous? + + //gosl:wgsl + // nci += ncA + ncB // wgsl returns orig, Go returns new + //gosl:end + + if nci > params.ContactsMax { + return + } + + for i := range ncA { + // todo: accessors + Contacts.Set(ba, int(nci+i), int(ContactA)) + Contacts.Set(bb, int(nci+i), int(ContactB)) + // etc + } + for i := range ncB { + Contacts.Set(ba, int(nci+ncA+i), int(ContactA)) // flip?? + Contacts.Set(bb, int(nci+ncA+i), int(ContactA)) + } // # Allocate contact points using reusable method // _success = allocate_contact_points( // @@ -240,7 +259,7 @@ func (wl *World) ConfigBodyCollidePairs() { } } params.BodyCollidePairsN = int32(np) - pt.SetShapeSizes(np+1, 2) // last one is for current contacts count + pt.SetShapeSizes(np, 2) wl.BodyCollidePairs = pt wl.Contacts.SetShapeSizes(np, int(ContactVarsN)) fmt.Println("body pairs over alloc", nalc, np) diff --git a/physics/contact.goal b/physics/contact.goal index 9a10621d..7bf417f6 100644 --- a/physics/contact.goal +++ b/physics/contact.goal @@ -6,6 +6,7 @@ package physics import ( "fmt" + "sync/atomic" "cogentcore.org/core/math32" "cogentcore.org/lab/gosl/slmath" @@ -144,9 +145,27 @@ func CollisionBroad(i uint32) { //gosl:kernel // contact_point_limit[pair_index_ba] = 0 // now we just write results to big buffer of contact points. - // Key point is that Contacts needs to have up to 12 elements per pair possible! - // just allocate in order. + nci := atomic.AddInt32(&ContactCount.Values[int(params.Cur)], ncA + ncB) // returns previous? + + //gosl:wgsl + // nci += ncA + ncB // wgsl returns orig, Go returns new + //gosl:end + + if nci > params.ContactsMax { + return + } + + for i := range ncA { + // todo: accessors + Contacts[nci + i, ContactA] = ba + Contacts[nci + i, ContactB] = bb + // etc + } + for i := range ncB { + Contacts[nci + ncA + i, ContactA] = ba // flip?? + Contacts[nci + ncA + i, ContactA] = bb + } // # Allocate contact points using reusable method // _success = allocate_contact_points( // num_contacts_a, @@ -236,7 +255,7 @@ func (wl *World) ConfigBodyCollidePairs() { } } params.BodyCollidePairsN = int32(np) - pt.SetShapeSizes(np+1, 2) // last one is for current contacts count + pt.SetShapeSizes(np, 2) wl.BodyCollidePairs = pt wl.Contacts.SetShapeSizes(np, int(ContactVarsN)) fmt.Println("body pairs over alloc", nalc, np) diff --git a/physics/enumgen.go b/physics/enumgen.go index 86b4fb66..7ff33fcb 100644 --- a/physics/enumgen.go +++ b/physics/enumgen.go @@ -192,20 +192,20 @@ func (i *DynamicVars) UnmarshalText(text []byte) error { return enums.UnmarshalText(i, text, "DynamicVars") } -var _GPUVarsValues = []GPUVars{0, 1, 2, 3, 4, 5, 6, 7, 8} +var _GPUVarsValues = []GPUVars{0, 1, 2, 3, 4, 5, 6, 7, 8, 9} // GPUVarsN is the highest valid value for type GPUVars, plus one. // //gosl:start -const GPUVarsN GPUVars = 9 +const GPUVarsN GPUVars = 10 //gosl:end -var _GPUVarsValueMap = map[string]GPUVars{`ParamsVar`: 0, `BodiesVar`: 1, `JointsVar`: 2, `JointDoFsVar`: 3, `BodyJointsVar`: 4, `BodyCollidePairsVar`: 5, `DynamicsVar`: 6, `ContactsVar`: 7, `JointControlsVar`: 8} +var _GPUVarsValueMap = map[string]GPUVars{`ParamsVar`: 0, `BodiesVar`: 1, `JointsVar`: 2, `JointDoFsVar`: 3, `BodyJointsVar`: 4, `BodyCollidePairsVar`: 5, `DynamicsVar`: 6, `ContactCountVar`: 7, `ContactsVar`: 8, `JointControlsVar`: 9} -var _GPUVarsDescMap = map[GPUVars]string{0: ``, 1: ``, 2: ``, 3: ``, 4: ``, 5: ``, 6: ``, 7: ``, 8: ``} +var _GPUVarsDescMap = map[GPUVars]string{0: ``, 1: ``, 2: ``, 3: ``, 4: ``, 5: ``, 6: ``, 7: ``, 8: ``, 9: ``} -var _GPUVarsMap = map[GPUVars]string{0: `ParamsVar`, 1: `BodiesVar`, 2: `JointsVar`, 3: `JointDoFsVar`, 4: `BodyJointsVar`, 5: `BodyCollidePairsVar`, 6: `DynamicsVar`, 7: `ContactsVar`, 8: `JointControlsVar`} +var _GPUVarsMap = map[GPUVars]string{0: `ParamsVar`, 1: `BodiesVar`, 2: `JointsVar`, 3: `JointDoFsVar`, 4: `BodyJointsVar`, 5: `BodyCollidePairsVar`, 6: `DynamicsVar`, 7: `ContactCountVar`, 8: `ContactsVar`, 9: `JointControlsVar`} // String returns the string representation of this GPUVars value. func (i GPUVars) String() string { return enums.String(i, _GPUVarsMap) } diff --git a/physics/gosl.go b/physics/gosl.go index 9c5968d8..d0fba392 100644 --- a/physics/gosl.go +++ b/physics/gosl.go @@ -46,8 +46,9 @@ const ( BodyJointsVar GPUVars = 4 BodyCollidePairsVar GPUVars = 5 DynamicsVar GPUVars = 6 - ContactsVar GPUVars = 7 - JointControlsVar GPUVars = 8 + ContactCountVar GPUVars = 7 + ContactsVar GPUVars = 8 + JointControlsVar GPUVars = 9 ) // Tensor stride variables @@ -99,6 +100,7 @@ func GPUInit() { var vr *gpu.Var _ = vr vr = sgp.Add("Dynamics", gpu.Float32, 1, gpu.ComputeShader) + vr = sgp.Add("ContactCount", gpu.Int32, 1, gpu.ComputeShader) vr = sgp.Add("Contacts", gpu.Float32, 1, gpu.ComputeShader) sgp.SetNValues(1) } @@ -114,6 +116,8 @@ func GPUInit() { pl.AddVarUsed(0, "TensorStrides") pl.AddVarUsed(1, "Bodies") pl.AddVarUsed(1, "BodyCollidePairs") + pl.AddVarUsed(2, "ContactCount") + pl.AddVarUsed(2, "Contacts") pl.AddVarUsed(2, "Dynamics") pl.AddVarUsed(0, "Params") pl = gpu.NewComputePipelineShaderFS(shaders, "shaders/DeltasFromJoints.wgsl", sy) @@ -606,6 +610,9 @@ func ToGPU(vars ...GPUVars) { case DynamicsVar: v, _ := syVars.ValueByIndex(2, "Dynamics", 0) gpu.SetValueFrom(v, Dynamics.Values) + case ContactCountVar: + v, _ := syVars.ValueByIndex(2, "ContactCount", 0) + gpu.SetValueFrom(v, ContactCount.Values) case ContactsVar: v, _ := syVars.ValueByIndex(2, "Contacts", 0) gpu.SetValueFrom(v, Contacts.Values) @@ -633,7 +640,7 @@ func ToGPUTensorStrides() { } sy := GPUSystem syVars := sy.Vars() - TensorStrides.SetShapeSizes(80) + TensorStrides.SetShapeSizes(90) TensorStrides.SetInt1D(Bodies.Shape().Strides[0], 0) TensorStrides.SetInt1D(Bodies.Shape().Strides[1], 1) TensorStrides.SetInt1D(Joints.Shape().Strides[0], 10) @@ -648,10 +655,11 @@ func ToGPUTensorStrides() { TensorStrides.SetInt1D(Dynamics.Shape().Strides[0], 50) TensorStrides.SetInt1D(Dynamics.Shape().Strides[1], 51) TensorStrides.SetInt1D(Dynamics.Shape().Strides[2], 52) - TensorStrides.SetInt1D(Contacts.Shape().Strides[0], 60) - TensorStrides.SetInt1D(Contacts.Shape().Strides[1], 61) - TensorStrides.SetInt1D(JointControls.Shape().Strides[0], 70) - TensorStrides.SetInt1D(JointControls.Shape().Strides[1], 71) + TensorStrides.SetInt1D(ContactCount.Shape().Strides[0], 60) + TensorStrides.SetInt1D(Contacts.Shape().Strides[0], 70) + TensorStrides.SetInt1D(Contacts.Shape().Strides[1], 71) + TensorStrides.SetInt1D(JointControls.Shape().Strides[0], 80) + TensorStrides.SetInt1D(JointControls.Shape().Strides[1], 81) v, _ := syVars.ValueByIndex(0, "TensorStrides", 0) gpu.SetValueFrom(v, TensorStrides.Values) } @@ -683,6 +691,9 @@ func ReadFromGPU(vars ...GPUVars) { case DynamicsVar: v, _ := syVars.ValueByIndex(2, "Dynamics", 0) v.GPUToRead(sy.CommandEncoder) + case ContactCountVar: + v, _ := syVars.ValueByIndex(2, "ContactCount", 0) + v.GPUToRead(sy.CommandEncoder) case ContactsVar: v, _ := syVars.ValueByIndex(2, "Contacts", 0) v.GPUToRead(sy.CommandEncoder) @@ -727,6 +738,10 @@ func SyncFromGPU(vars ...GPUVars) { v, _ := syVars.ValueByIndex(2, "Dynamics", 0) v.ReadSync() gpu.ReadToBytes(v, Dynamics.Values) + case ContactCountVar: + v, _ := syVars.ValueByIndex(2, "ContactCount", 0) + v.ReadSync() + gpu.ReadToBytes(v, ContactCount.Values) case ContactsVar: v, _ := syVars.ValueByIndex(2, "Contacts", 0) v.ReadSync() diff --git a/physics/shaders/CollisionBroad.wgsl b/physics/shaders/CollisionBroad.wgsl index 0d3900e2..00df7821 100644 --- a/physics/shaders/CollisionBroad.wgsl +++ b/physics/shaders/CollisionBroad.wgsl @@ -14,6 +14,10 @@ var BodyCollidePairs: array; // // Dynamics are the dynamic rigid body elements: these actually move. // Two alternating states are used: Params.Cur and Params.Next. // [dyn body][cur/next][DynamicVarsN] @group(2) @binding(0) var Dynamics: array; +@group(2) @binding(1) +var ContactCount: array>; +@group(2) @binding(2) +var Contacts: array; // // JointControls are dynamic joint control inputs, per joint DoF // (in correspondence with [JointDoFs]). This can be uploaded to the // GPU at every step. // [dof][JointControlVarsN] alias GPUVars = i32; @@ -32,6 +36,10 @@ fn Index3D(s0: u32, s1: u32, s2: u32, i0: u32, i1: u32, i2: u32) -> u32 { return s0 * i0 + s1 * i1 + s2 * i2; } +fn Index1D(s0: u32, i0: u32) -> u32 { + return s0 * i0; +} + //////// import: "vars.go" @@ -148,8 +156,21 @@ var infPlane = false;; if (sA == Plane) { return; } }; -var ncB: i32;; var ncA = ShapePairContacts(sA, sB, infPlane, &ncB);; _ = ncA; -} +var ncB: i32;; var ncA = ShapePairContacts(sA, sB, infPlane, &ncB);; _ = ncA;; +var nci = atomicAdd(&ContactCount[i32(params.Cur)], ncA+ncB);; // returns previous? +nci += ncA + ncB;; // wgsl returns orig, Go returns new +if (nci > params.ContactsMax) { + return; +}; for (var i=0; i,pt: vec3) -> vec3 { var cp = pt; if (sz.x == 0.0) { @@ -211,7 +232,7 @@ const BodyVarsN: BodyVars = 40; const ContactVarsN: ContactVars = 16; const JointControlVarsN: JointControlVars = 3; const DynamicVarsN: DynamicVars = 32; -const GPUVarsN: GPUVars = 9; +const GPUVarsN: GPUVars = 10; const JointTypesN: JointTypes = 7; const JointVarsN: JointVars = 50; const JointDoFVarsN: JointDoFVars = 7; diff --git a/physics/shaders/DeltasFromJoints.wgsl b/physics/shaders/DeltasFromJoints.wgsl index 94651932..68dc8d98 100644 --- a/physics/shaders/DeltasFromJoints.wgsl +++ b/physics/shaders/DeltasFromJoints.wgsl @@ -153,7 +153,7 @@ const BodyVarsN: BodyVars = 40; const ContactVarsN: ContactVars = 16; const JointControlVarsN: JointControlVars = 3; const DynamicVarsN: DynamicVars = 32; -const GPUVarsN: GPUVars = 9; +const GPUVarsN: GPUVars = 10; const JointTypesN: JointTypes = 7; const JointVarsN: JointVars = 50; const JointDoFVarsN: JointDoFVars = 7; diff --git a/physics/shaders/DynamicsCurToNext.wgsl b/physics/shaders/DynamicsCurToNext.wgsl index a2cf33fa..c7b8e884 100644 --- a/physics/shaders/DynamicsCurToNext.wgsl +++ b/physics/shaders/DynamicsCurToNext.wgsl @@ -135,7 +135,7 @@ const BodyVarsN: BodyVars = 40; const ContactVarsN: ContactVars = 16; const JointControlVarsN: JointControlVars = 3; const DynamicVarsN: DynamicVars = 32; -const GPUVarsN: GPUVars = 9; +const GPUVarsN: GPUVars = 10; const JointTypesN: JointTypes = 7; const JointVarsN: JointVars = 50; const JointDoFVarsN: JointDoFVars = 7; diff --git a/physics/shaders/ForcesFromJoints.wgsl b/physics/shaders/ForcesFromJoints.wgsl index 4366862e..000dbbaa 100644 --- a/physics/shaders/ForcesFromJoints.wgsl +++ b/physics/shaders/ForcesFromJoints.wgsl @@ -153,7 +153,7 @@ const BodyVarsN: BodyVars = 40; const ContactVarsN: ContactVars = 16; const JointControlVarsN: JointControlVars = 3; const DynamicVarsN: DynamicVars = 32; -const GPUVarsN: GPUVars = 9; +const GPUVarsN: GPUVars = 10; const JointTypesN: JointTypes = 7; const JointVarsN: JointVars = 50; const JointDoFVarsN: JointDoFVars = 7; diff --git a/physics/shaders/InitDynamics.wgsl b/physics/shaders/InitDynamics.wgsl index bab5c8e9..df625a9d 100644 --- a/physics/shaders/InitDynamics.wgsl +++ b/physics/shaders/InitDynamics.wgsl @@ -144,7 +144,7 @@ const BodyVarsN: BodyVars = 40; const ContactVarsN: ContactVars = 16; const JointControlVarsN: JointControlVars = 3; const DynamicVarsN: DynamicVars = 32; -const GPUVarsN: GPUVars = 9; +const GPUVarsN: GPUVars = 10; const JointTypesN: JointTypes = 7; const JointVarsN: JointVars = 50; const JointDoFVarsN: JointDoFVars = 7; diff --git a/physics/shaders/StepBodyDeltas.wgsl b/physics/shaders/StepBodyDeltas.wgsl index 46bd97b7..7d71e1e2 100644 --- a/physics/shaders/StepBodyDeltas.wgsl +++ b/physics/shaders/StepBodyDeltas.wgsl @@ -190,7 +190,7 @@ const BodyVarsN: BodyVars = 40; const ContactVarsN: ContactVars = 16; const JointControlVarsN: JointControlVars = 3; const DynamicVarsN: DynamicVars = 32; -const GPUVarsN: GPUVars = 9; +const GPUVarsN: GPUVars = 10; const JointTypesN: JointTypes = 7; const JointVarsN: JointVars = 50; const JointDoFVarsN: JointDoFVars = 7; diff --git a/physics/shaders/StepIntegrateBodies.wgsl b/physics/shaders/StepIntegrateBodies.wgsl index 6c252f71..c725fc08 100644 --- a/physics/shaders/StepIntegrateBodies.wgsl +++ b/physics/shaders/StepIntegrateBodies.wgsl @@ -196,7 +196,7 @@ const BodyVarsN: BodyVars = 40; const ContactVarsN: ContactVars = 16; const JointControlVarsN: JointControlVars = 3; const DynamicVarsN: DynamicVars = 32; -const GPUVarsN: GPUVars = 9; +const GPUVarsN: GPUVars = 10; const JointTypesN: JointTypes = 7; const JointVarsN: JointVars = 50; const JointDoFVarsN: JointDoFVars = 7; diff --git a/physics/shaders/StepJointForces.wgsl b/physics/shaders/StepJointForces.wgsl index 925da1de..cc7aba72 100644 --- a/physics/shaders/StepJointForces.wgsl +++ b/physics/shaders/StepJointForces.wgsl @@ -110,7 +110,7 @@ const JointControlForce: JointControlVars = 0; const JointTargetPos: JointControlVars = 1; const JointTargetVel: JointControlVars = 2; fn JointControl(idx: i32,dof: i32, vr: JointControlVars) -> f32 { - return JointControls[Index2D(TensorStrides[70], TensorStrides[71], u32(JointDoFIndex(idx, dof)), u32(vr))]; + return JointControls[Index2D(TensorStrides[80], TensorStrides[81], u32(JointDoFIndex(idx, dof)), u32(vr))]; } //////// import: "dynamics.go" @@ -162,7 +162,7 @@ const BodyVarsN: BodyVars = 40; const ContactVarsN: ContactVars = 16; const JointControlVarsN: JointControlVars = 3; const DynamicVarsN: DynamicVars = 32; -const GPUVarsN: GPUVars = 9; +const GPUVarsN: GPUVars = 10; const JointTypesN: JointTypes = 7; const JointVarsN: JointVars = 50; const JointDoFVarsN: JointDoFVars = 7; diff --git a/physics/shaders/StepSolveJoints.wgsl b/physics/shaders/StepSolveJoints.wgsl index 412e3346..94449356 100644 --- a/physics/shaders/StepSolveJoints.wgsl +++ b/physics/shaders/StepSolveJoints.wgsl @@ -115,7 +115,7 @@ const JointControlForce: JointControlVars = 0; const JointTargetPos: JointControlVars = 1; const JointTargetVel: JointControlVars = 2; fn JointControl(idx: i32,dof: i32, vr: JointControlVars) -> f32 { - return JointControls[Index2D(TensorStrides[70], TensorStrides[71], u32(JointDoFIndex(idx, dof)), u32(vr))]; + return JointControls[Index2D(TensorStrides[80], TensorStrides[81], u32(JointDoFIndex(idx, dof)), u32(vr))]; } //////// import: "dynamics.go" @@ -173,7 +173,7 @@ const BodyVarsN: BodyVars = 40; const ContactVarsN: ContactVars = 16; const JointControlVarsN: JointControlVars = 3; const DynamicVarsN: DynamicVars = 32; -const GPUVarsN: GPUVars = 9; +const GPUVarsN: GPUVars = 10; const JointTypesN: JointTypes = 7; const JointVarsN: JointVars = 50; const JointDoFVarsN: JointDoFVars = 7; diff --git a/physics/typegen.go b/physics/typegen.go index b21775e3..9e59bc14 100644 --- a/physics/typegen.go +++ b/physics/typegen.go @@ -30,4 +30,4 @@ var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.Shapes", IDN var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.State", IDName: "state", Doc: "State contains the basic physical state including position, orientation, velocity.\nThese are only the values that can be either relative or absolute -- other physical\nstate values such as Mass should go in Rigid.", Methods: []types.Method{{Name: "MoveOnAxis", Doc: "MoveOnAxis moves (translates) the specified distance on the specified local axis,\nrelative to the current rotation orientation.\nThe axis is normalized prior to aplying the distance factor.\nSets the LinVel to motion vector.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z", "dist"}}, {Name: "MoveOnAxisAbs", Doc: "MoveOnAxisAbs moves (translates) the specified distance on the specified local axis,\nin absolute X,Y,Z coordinates (does not apply the Quat rotation factor.\nThe axis is normalized prior to aplying the distance factor.\nSets the LinVel to motion vector.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z", "dist"}}, {Name: "SetEulerRotation", Doc: "SetEulerRotation sets the rotation in Euler angles (degrees).", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z"}}, {Name: "EulerRotation", Doc: "EulerRotation returns the current rotation in Euler angles (degrees).", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Returns: []string{"Vector3"}}, {Name: "SetAxisRotation", Doc: "SetAxisRotation sets rotation from local axis and angle in degrees.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z", "angle"}}, {Name: "RotateOnAxis", Doc: "RotateOnAxis rotates around the specified local axis the specified angle in degrees.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z", "angle"}}, {Name: "RotateEuler", Doc: "RotateEuler rotates by given Euler angles (in degrees) relative to existing rotation.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z"}}}, Fields: []types.Field{{Name: "Pos", Doc: "Pos is the position of center of mass of object."}, {Name: "Quat", Doc: "Quat is the rotation specified as a quaternion."}, {Name: "LinVel", Doc: "LinVel is the linear velocity."}, {Name: "AngVel", Doc: "AngVel is the angular velocity."}}}) -var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.World", IDName: "world", Doc: "World contains and manages all of the physics elements.", Directives: []types.Directive{{Tool: "go", Directive: "generate", Args: []string{"core", "generate", "-add-types", "-gosl"}}}, Fields: []types.Field{{Name: "GPU", Doc: "GPU determines whether to use GPU (else CPU)."}, {Name: "Params", Doc: "Params are global parameters."}, {Name: "Bodies", Doc: "Bodies are the rigid body elements (dynamic and static),\nspecifying the constant, non-dynamic properties,\nwhich is initial state for dynamics.\n[body][BodyVarsN]"}, {Name: "Joints", Doc: "Joints is a list of permanent joints connecting bodies,\nwhich do not change (no dynamic variables).\n[joint][JointVarsN]"}, {Name: "JointDoFs", Doc: "JointDoFs is a list of joint DoF parameters, allocated per joint.\n[dof][JointDoFVars]"}, {Name: "BodyJoints", Doc: "BodyJoints is a list of joint indexes for each dynamic body, for aggregating.\n[dyn body][parent, child][Params.BodyJointsMax]"}, {Name: "BodyCollidePairs", Doc: "BodyCollidePairs are pairs of Body indexes that could potentially collide\nbased on precomputed collision logic, using World, Group, and Joint indexes.\nThe last entry is updated to contain the actual number of contacts generated\non each collision iteration.\n[BodyCollidePairsN+1][2]"}, {Name: "Dynamics", Doc: "Dynamics are the dynamic rigid body elements: these actually move.\nThe first set of variables are for initial values, and the second current.\n[body][cur/next][DynamicVarsN]"}, {Name: "Contacts", Doc: "Contacts are points of contact between bodies. Max possible size\nis BodyCollidePairsN, but actual size on given run is updated\ninto last entry in BodyCollidePairs.\n[BodyCollidePairsN][ContactVarsN]"}, {Name: "JointControls", Doc: "JointControls are dynamic joint control inputs, per joint DoF\n(in correspondence with [JointDoFs]). This can be uploaded to the\nGPU at every step.\n[dof][JointControlVarsN]"}}}) +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.World", IDName: "world", Doc: "World contains and manages all of the physics elements.", Directives: []types.Directive{{Tool: "go", Directive: "generate", Args: []string{"core", "generate", "-add-types", "-gosl"}}}, Fields: []types.Field{{Name: "GPU", Doc: "GPU determines whether to use GPU (else CPU)."}, {Name: "Params", Doc: "Params are global parameters."}, {Name: "Bodies", Doc: "Bodies are the rigid body elements (dynamic and static),\nspecifying the constant, non-dynamic properties,\nwhich is initial state for dynamics.\n[body][BodyVarsN]"}, {Name: "Joints", Doc: "Joints is a list of permanent joints connecting bodies,\nwhich do not change (no dynamic variables).\n[joint][JointVarsN]"}, {Name: "JointDoFs", Doc: "JointDoFs is a list of joint DoF parameters, allocated per joint.\n[dof][JointDoFVars]"}, {Name: "BodyJoints", Doc: "BodyJoints is a list of joint indexes for each dynamic body, for aggregating.\n[dyn body][parent, child][Params.BodyJointsMax]"}, {Name: "BodyCollidePairs", Doc: "BodyCollidePairs are pairs of Body indexes that could potentially collide\nbased on precomputed collision logic, using World, Group, and Joint indexes.\n[BodyCollidePairsN][2]"}, {Name: "Dynamics", Doc: "Dynamics are the dynamic rigid body elements: these actually move.\nThe first set of variables are for initial values, and the second current.\n[body][cur/next][DynamicVarsN]"}, {Name: "ContactCount", Doc: "ContactCount has number of points of contact between bodies.\nparams.Cur is written to and params.Next is zeroed in\nnarrow-phase contacts processing, so it is ready for next time.\n[2]"}, {Name: "Contacts", Doc: "Contacts are points of contact between bodies. Max possible size\nis BodyCollidePairsN, but actual size on given run is updated\ninto ContactCount.\n[BodyCollidePairsN][ContactVarsN]"}, {Name: "JointControls", Doc: "JointControls are dynamic joint control inputs, per joint DoF\n(in correspondence with [JointDoFs]). This can be uploaded to the\nGPU at every step.\n[dof][JointControlVarsN]"}}}) diff --git a/physics/vars.go b/physics/vars.go index 14bd1bb3..5737f9a3 100644 --- a/physics/vars.go +++ b/physics/vars.go @@ -6,7 +6,7 @@ package physics import "cogentcore.org/lab/tensor" -//go:generate gosl -exclude=Update,Defaults,ShouldDisplay -max-buffer-size=2147483616 +//go:generate gosl -keep -exclude=Update,Defaults,ShouldDisplay -max-buffer-size=2147483616 //gosl:start @@ -46,7 +46,7 @@ var ( // BodyCollidePairs are pairs of Body indexes that could potentially collide // based on precomputed collision logic, using World, Group, and Joint indexes. // The last entry is updated to contain the actual number of contacts generated - // on each collision iteration. + // in the CollisionBroad routine on each collision iteration. // [BodyCollidePairsN+1][2] //gosl:dims 2 BodyCollidePairs *tensor.Int32 @@ -58,6 +58,13 @@ var ( //gosl:dims 3 Dynamics *tensor.Float32 + // ContactCount has number of points of contact between bodies. + // params.Cur is written to and params.Next is zeroed in + // narrow-phase contacts processing, so it is ready for next time. + // [2] + //gosl:dims 1 + ContactCount *tensor.Int32 + // Contacts are points of contact between bodies. // [contact][ContactVarsN] //gosl:dims 2 diff --git a/physics/world.go b/physics/world.go index cef2097b..13c027ff 100644 --- a/physics/world.go +++ b/physics/world.go @@ -42,9 +42,7 @@ type World struct { // BodyCollidePairs are pairs of Body indexes that could potentially collide // based on precomputed collision logic, using World, Group, and Joint indexes. - // The last entry is updated to contain the actual number of contacts generated - // on each collision iteration. - // [BodyCollidePairsN+1][2] + // [BodyCollidePairsN][2] BodyCollidePairs *tensor.Int32 // Dynamics are the dynamic rigid body elements: these actually move. @@ -52,9 +50,15 @@ type World struct { // [body][cur/next][DynamicVarsN] Dynamics *tensor.Float32 `display:"no-inline"` + // ContactCount has number of points of contact between bodies. + // params.Cur is written to and params.Next is zeroed in + // narrow-phase contacts processing, so it is ready for next time. + // [2] + ContactCount *tensor.Int32 `display:"no-inline"` + // Contacts are points of contact between bodies. Max possible size // is BodyCollidePairsN, but actual size on given run is updated - // into last entry in BodyCollidePairs. + // into ContactCount. // [BodyCollidePairsN][ContactVarsN] Contacts *tensor.Float32 `display:"no-inline"` @@ -80,6 +84,7 @@ func (wl *World) Init() { wl.JointDoFs = tensor.NewFloat32(0, int(JointDoFVarsN)) wl.BodyJoints = tensor.NewInt32(0, 2, 2) wl.Dynamics = tensor.NewFloat32(0, 2, int(DynamicVarsN)) + wl.ContactCount = tensor.NewInt32(2) wl.Contacts = tensor.NewFloat32(0, int(ContactVarsN)) wl.JointControls = tensor.NewFloat32(0, int(JointControlVarsN)) wl.SetAsCurrentVars() @@ -140,6 +145,7 @@ func (wl *World) SetAsCurrentVars() { BodyJoints = wl.BodyJoints JointDoFs = wl.JointDoFs Dynamics = wl.Dynamics + ContactCount = wl.ContactCount Contacts = wl.Contacts JointControls = wl.JointControls } @@ -156,5 +162,5 @@ func (wl *World) GPUInit() { // the GPU. This is done in GPUInit, and if current switched. func (wl *World) ToGPUInfra() { ToGPUTensorStrides() - ToGPU(ParamsVar, BodiesVar, JointsVar, BodyJointsVar, JointDoFsVar) + ToGPU(ParamsVar, BodiesVar, JointsVar, BodyJointsVar, JointDoFsVar, DynamicsVar, ContactCountVar, ContactsVar) } diff --git a/physics/world.goal b/physics/world.goal index 8128a462..ed306070 100644 --- a/physics/world.goal +++ b/physics/world.goal @@ -40,9 +40,7 @@ type World struct { // BodyCollidePairs are pairs of Body indexes that could potentially collide // based on precomputed collision logic, using World, Group, and Joint indexes. - // The last entry is updated to contain the actual number of contacts generated - // on each collision iteration. - // [BodyCollidePairsN+1][2] + // [BodyCollidePairsN][2] BodyCollidePairs *tensor.Int32 // Dynamics are the dynamic rigid body elements: these actually move. @@ -50,9 +48,15 @@ type World struct { // [body][cur/next][DynamicVarsN] Dynamics *tensor.Float32 `display:"no-inline"` + // ContactCount has number of points of contact between bodies. + // params.Cur is written to and params.Next is zeroed in + // narrow-phase contacts processing, so it is ready for next time. + // [2] + ContactCount *tensor.Int32 `display:"no-inline"` + // Contacts are points of contact between bodies. Max possible size // is BodyCollidePairsN, but actual size on given run is updated - // into last entry in BodyCollidePairs. + // into ContactCount. // [BodyCollidePairsN][ContactVarsN] Contacts *tensor.Float32 `display:"no-inline"` @@ -78,6 +82,7 @@ func (wl *World) Init() { wl.JointDoFs = tensor.NewFloat32(0, int(JointDoFVarsN)) wl.BodyJoints = tensor.NewInt32(0, 2, 2) wl.Dynamics = tensor.NewFloat32(0, 2, int(DynamicVarsN)) + wl.ContactCount = tensor.NewInt32(2) wl.Contacts = tensor.NewFloat32(0, int(ContactVarsN)) wl.JointControls = tensor.NewFloat32(0, int(JointControlVarsN)) wl.SetAsCurrentVars() @@ -138,6 +143,7 @@ func (wl *World) SetAsCurrentVars() { BodyJoints = wl.BodyJoints JointDoFs = wl.JointDoFs Dynamics = wl.Dynamics + ContactCount = wl.ContactCount Contacts = wl.Contacts JointControls = wl.JointControls } @@ -154,5 +160,5 @@ func (wl *World) GPUInit() { // the GPU. This is done in GPUInit, and if current switched. func (wl *World) ToGPUInfra() { ToGPUTensorStrides() - ToGPU(ParamsVar, BodiesVar, JointsVar, BodyJointsVar, JointDoFsVar) + ToGPU(ParamsVar, BodiesVar, JointsVar, BodyJointsVar, JointDoFsVar, DynamicsVar, ContactCountVar, ContactsVar) } From ee170813a8bd0f73407922b55c9a6a1968faf527 Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Sat, 20 Dec 2025 16:27:45 +0100 Subject: [PATCH 28/97] physics: broad-phase collision complete, with allocation of contacts etc. --- docs/content/gosl.md | 66 ++++--- physics/config.go | 1 + physics/config.goal | 1 + physics/contact.go | 235 +++++++++++++---------- physics/contact.goal | 234 +++++++++++++--------- physics/enumgen.go | 10 +- physics/params.go | 5 + physics/shaders/CollisionBroad.wgsl | 131 ++++++++----- physics/shaders/DeltasFromJoints.wgsl | 35 ++-- physics/shaders/DynamicsCurToNext.wgsl | 35 ++-- physics/shaders/ForcesFromJoints.wgsl | 35 ++-- physics/shaders/InitDynamics.wgsl | 35 ++-- physics/shaders/StepBodyDeltas.wgsl | 35 ++-- physics/shaders/StepIntegrateBodies.wgsl | 35 ++-- physics/shaders/StepJointForces.wgsl | 35 ++-- physics/shaders/StepSolveJoints.wgsl | 35 ++-- physics/shapes.go | 2 + physics/typegen.go | 2 +- physics/vars.go | 5 +- 19 files changed, 576 insertions(+), 396 deletions(-) diff --git a/docs/content/gosl.md b/docs/content/gosl.md index 891222a7..9f0ec270 100644 --- a/docs/content/gosl.md +++ b/docs/content/gosl.md @@ -4,9 +4,9 @@ Name = "GoSL" **GoSL** (via the `gosl` executable) allows you to write Go programs that run on [[GPU]] hardware, by transpiling Go into the WGSL shader language used by [WebGPU](https://www.w3.org/TR/webgpu/), thereby establishing the _Go shader language_. -GoSL uses the [core gpu](https://github.com/cogentcore/core/tree/main/gpu) compute shader system, and can take advantage of the [[Goal]] transpiler to provide a more natural tensor indexing syntax. +GoSL uses the [core gpu](https://cogentcore.org/core/gpu) compute shader system, and can take advantage of the [[Goal]] transpiler to provide a more natural tensor indexing syntax. -Functionally, GoSL is similar to [NVIDIA warp](https://github.com/NVIDIA/warp) -- [docs](https://nvidia.github.io/warp/basics.html), which uses python as the original source and converts it to either C++ or CUDA code. In GoSL, the original code is directly Go, so we just need to do the WGSL part. Unlike warp, WGSL runs on all GPU platforms, including the web (warp only runs on NVIDIA GPUs, on desktop). +Functionally, GoSL is similar to [NVIDIA warp](https://github.com/NVIDIA/warp) -- [docs](https://nvidia.github.io/warp/basics.html), which uses Python as the original source and converts it to either C++ or CUDA code. In GoSL, the original code is directly Go, so we just need to do the WGSL part. Unlike warp, WGSL runs on all GPU platforms, including the web (warp only runs on NVIDIA GPUs, on desktop / servers). The relevant regions of Go code to be run on the GPU are tagged using the `//gosl:start` and `//gosl:end` comment directives, and this code must only use basic expressions and concrete types that will compile correctly in a GPU shader (see [[#Restrictions]] below). Method functions and pass-by-reference pointer arguments to `struct` types are supported and incur no additional compute cost due to inlining (see notes below for more detail). @@ -29,15 +29,15 @@ It is also strongly recommended to install the `naga` WGSL compiler from [wgpu]( There are two key elements for GPU-enabled code: -1. One or more [[#Kernels]] compute functions that take an _index_ argument and perform computations for that specific index of data, _in parallel_. **GPU computation is effectively just a parallel `for` loop**. On the GPU, each such kernel is implemented by its own separate compute shader code, and one of the main functions of `gosl` is to generate this code from the Go sources, in the automatically created `shaders/` directory. +1. One or more [[#Kernel]] compute functions that take an _index_ argument and perform computations for that specific index of data, _in parallel_. **GPU computation is effectively just a parallel `for` loop**. On the GPU, each such kernel is implemented by its own separate compute shader code, and one of the main functions of `gosl` is to generate this code from the Go sources, in the automatically created `shaders/` directory. 2. [[#Global variables]] on which the kernel functions _exclusively_ operate: all relevant data must be specifically copied from the CPU to the GPU and back. As explained in the [[GPU]] docs, each GPU compute shader is effectively a _standalone_ program operating on these global variables. To replicate this environment on the CPU, so the code works in both contexts, we need to make these variables global in the CPU (Go) environment as well. **IMPORTANT:** All tensor variables must be sent to the GPU at least once before running, because the generated code does not know the size of the tensor until this is done! This is true even if a variable is just a results variable that does not logically need to be uploaded to the GPU -- the overhead at startup for a single such transfer is not worth the extra complexity of creating the necessary alternative init code. -`gosl` generates a file named `gosl.go` in your package directory that initializes the GPU with all of the global variables, and functions for running the kernels and syncing the gobal variable data back and forth between the CPu and GPU. +`gosl` generates a file named `gosl.go` in your package directory that initializes the GPU with all of the global variables, and functions for running the kernels and syncing the gobal variable data back and forth between the CPU and GPU. -## Kernels +## Kernel Each distinct compute kernel must be tagged with a `//gosl:kernel` comment directive, as in this example (from `examples/basic`): ```go @@ -52,7 +52,7 @@ func Compute(i uint32) { //gosl:kernel The kernel functions receive a `uint32` index argument, and use this to index into the global variables containing the relevant data. -**IMPORTANT:** the dispatch of kernels on the GPU is in blocks of 64 processors, so i will exceed the number that you pass into the `Run` function! It is essential to check bounds in every kernel. +**IMPORTANT:** the dispatch of kernels on the GPU is in blocks of 64 processors, so `i` will exceed the number that you pass into the `Run` function! It is essential to check bounds in every kernel. Typically the kernel code itself just calls other relevant function(s) using the index, as in the above example. Critically, _all_ of the data that a kernel function ultimately depends on must be contained with the global variables, and these variables must have been sync'd up to the GPU from the CPU prior to running the kernel (more on this below). @@ -84,7 +84,7 @@ var ( All such variables must be either: 1. A `slice` of GPU-alignment compatible `struct` types, such as `ParamStruct` in the above example. In general such structs should be marked as `//gosl:read-only` due to various challenges associated with writing to structs, detailed below. Due to lack of package-relative naming in the final WGSL file, any struct type defined in a sub package will show up unqualified in the generated `gosl.go` file, so a type alias is required to allow the resulting Go file to build properly. -2. A `tensor` of a GPU-compatible elemental data type (`float32`, `uint32`, or `int32`), with the number of dimensions indicated by the `//gosl:dims ` tag as shown above. This is the preferred type for writable data. +2. A [[tensor]] of a GPU-compatible elemental data type (`float32`, `uint32`, or `int32`), with the number of dimensions indicated by the `//gosl:dims ` tag as shown above. This is the preferred type for writable data. You can also just declare a slice of elemental GPU-compatible data values such as `float32`, but it is generally preferable to use the tensor instead, because it has built-in support for higher-dimensional indexing in a way that is transparent between CPU and GPU. @@ -107,14 +107,14 @@ TODO: this could be encoded in the TensorStrides. It will always be the outer-mo Each kernel belongs to a `gpu.ComputeSystem`, and each such system has one specific configuration of memory variables. In general, it is best to use a single set of global variables, and perform as much of the computation as possible on this set of variables, to minimize the number of memory transfers. However, if necessary, multiple systems can be defined, using an optional additional system name argument for the `args` and `kernel` tags. -In addition, the vars can be organized into _groups_, which generally should have similar memory syncing behavior, as documented in the [core gpu](https://github.com/cogentcore/core/tree/main/gpu) system. +In addition, the vars can be organized into _groups_, which generally should have similar memory syncing behavior, as documented in the [core gpu](https://cogentcore.org/core/gpu) system. Here's an example with multiple groups: ```go //gosl:vars [system name] var ( // Layer-level parameters - //gosl:group -uniform Params + //gosl:group Params Layers []LayerParam // note: struct with appropriate memory alignment // Path-level parameters @@ -157,6 +157,7 @@ It is absolutely essential to understand that _all data must already be on the G ## GPU relevant code taggng In a large GPU-based application, you should organize your code as you normally would in any standard Go application, distributing it across different files and packages. The GPU-relevant parts of each of those files can be tagged with the gosl tags: + ``` //gosl:start @@ -164,15 +165,24 @@ In a large GPU-based application, you should organize your code as you normally //gosl:end ``` + to make this code available to all of the shaders that are generated. Use the `//gosl:import "package/path"` directive to import GPU-relevant code from other packages, similar to the standard Go import directive. It is assumed that many other Go imports are not GPU relevant, so this separate directive is required. If any `enums` variables are defined, pass the `-gosl` flag to the `core generate` command to ensure that the `N` value is tagged with `//gosl:start` and `//gosl:end` tags. -**IMPORTANT:** all `.go` and `.wgsl` files are removed from the `shaders` directory prior to processing to ensure everything there is current -- always specify a different source location for any custom `.wgsl` files that are included. +It is also possible to include code that is _exclusively_ run on the GPU, by enclosing a commented-out section of code surrounded by these comment directives: + +``` + //gosl:wgsl + // nci += ncA + ncB // wgsl returns orig, Go returns new + //gosl:end +``` + +So the Go code just sees this as a comment, but `gosl` uncomments the code and processes it (as Go code, but can also just be raw WGSL). In general this should not be necessary, but one key place where it is needed is in the return values of atomic functions as described below. -# Command line usage +## Command line usage ``` gosl [flags] @@ -194,7 +204,7 @@ The flags are: Any `struct` types encountered will be checked for 16-byte alignment of sub-types and overall sizes as an even multiple of 16 bytes (4 `float32` or `int32` values), which is the alignment used in WGSL and glsl shader languages, and the underlying GPU hardware presumably. Look for error messages on the output from the gosl run. This ensures that direct byte-wise copies of data between CPU and GPU will be successful. The fact that `gosl` operates directly on the original CPU-side Go code uniquely enables it to perform these alignment checks, which are otherwise a major source of difficult-to-diagnose bugs. -# Restrictions +## Restrictions In general shader code should be simple mathematical expressions and data types, with minimal control logic via `if`, `for` statements, and only using the subset of Go that is consistent with C. Here are specific restrictions: @@ -247,25 +257,35 @@ var PathGBuf: array>; atomicAdd(&PathGBuf[idx], val); ``` -This also unfortunately has the side-effect that you cannot do _non-atomic_ operations on atomic variables, as discussed extensively here: https://github.com/gpuweb/gpuweb/issues/2377 GoSL automatically detects the use of atomic functions on GPU variables, and tags them as atomic. +This also unfortunately has the side-effect that you cannot do _non-atomic_ operations on atomic variables, as discussed extensively on [gpuweb](https://github.com/gpuweb/gpuweb/issues/2377). GoSL automatically detects the use of atomic functions on GPU variables, and tags them as atomic. -## Random numbers: slrand +Currently, only atomic `int32` and `uint32` are supported. See this [gpuweb issue](https://github.com/gpuweb/gpuweb/issues/4894), which also has a workaround by translating `float32` back and forth from `uint32`. Managing this transparently with the Go version which does directly support float32 should be possible, but managing the shadow variable etc is a bit of overhead and complexity. -See [[doc:gosl/slrand]] for a shader-optimized random number generation package, which is supported by `gosl` -- it will convert `slrand` calls into appropriate WGSL named function calls. `gosl` will also copy the `slrand.wgsl` file, which contains the full source code for the RNG, into the destination `shaders` directory, so it can be included with a simple local path: +If you need the return value from an atomic operation, then unfortunately Go and WGSL have different semantics for the Add etc operations: Go returns the value _after_ the addition, while WGSL returns the value _before_ the addition. Thus, a wgsl-specific code section is needed: -```go -//gosl:wgsl mycode -// #include "slrand.wgsl" -//gosl:end mycode ``` + nci := atomic.AddInt32(&ContactCount.Values[int(params.Cur)], ncA + ncB) + // Go returns post-added value, while WGSL returns pre-added value -## WGSL vector variables from math32.Vector2 etc: not yet + //gosl:wgsl + // nci += ncA + ncB // wgsl now matches Go + //gosl:end +``` + +## Random numbers: slrand -WGSL supports variables like `vec4` which is equivalent to `math32.Vector4`. Unfortunately, it would be difficult to have simultaneous, transparent support for both of these types across Go and WGSL, requiring rewriting expressions on the WGSL side. It is possible, but would take a fair amount of work, and is not yet planned. +See [[doc:gosl/slrand]] for a shader-optimized random number generation package, which is supported by `gosl`. e.g., `slrand.Float32` returns a random 0-1 `float32` value. -## Performance -With sufficiently large N, and ignoring the data copying setup time, around ~80x speedup is typical on a Macbook Pro with M1 processor. The `rand` example produces a 175x speedup! +## WGSL vector variables from math32.Vector* etc +WGSL supports variables like `vec4` which is equivalent to `math32.Vector4`. All such vector, matrix, and quaternion types are automatically converted, and basic math operations via methods like `.Add`, `.Mul` etc are automatically converted into corresponding `+` and `*` operations in WGSL. + +The [[doc:gosl/slmath]] package contains definitions of many other `math32` package methods as standalone functions that are compatible with WGSL translation (the existing `math32` methods are generally not). Thus, while it does require use of this separate library, it is possible to support all the major math32 vector, matrix, and quaternion functionality. + +To include the `math32.Vector2` and `math32.Vector3` types in a struct, use the [[doc:gosl/slvec]] equivalent types, which adds the necessary padding so that they align on the required 4x32 bit size for any struct field that is another struct. Call the `V()` and `SetV()` methods to get / set the actual corresponding `math32.Vector*` type -- that is handled correctly for the WGSL case. + +## Performance +With sufficiently large N, and ignoring the data copying setup time, dramatic many-factor speedups of ~10x up to ~100x or more are definitely possible. In general for naturally parallelizable code, it has been absolutely worth the effort to implement via GoSL. diff --git a/physics/config.go b/physics/config.go index c327d3c1..c06d09c6 100644 --- a/physics/config.go +++ b/physics/config.go @@ -13,6 +13,7 @@ package physics func (wl *World) Config() { wl.ConfigJoints() wl.ConfigBodyCollidePairs() + wl.SetMaxContacts() wl.SetAsCurrent() wl.GPUInit() wl.InitState() diff --git a/physics/config.goal b/physics/config.goal index 69712aab..c48361c2 100644 --- a/physics/config.goal +++ b/physics/config.goal @@ -11,6 +11,7 @@ package physics func (wl *World) Config() { wl.ConfigJoints() wl.ConfigBodyCollidePairs() + wl.SetMaxContacts() wl.SetAsCurrent() wl.GPUInit() wl.InitState() diff --git a/physics/contact.go b/physics/contact.go index 2482cfad..755de6b5 100644 --- a/physics/contact.go +++ b/physics/contact.go @@ -8,6 +8,7 @@ package physics import ( "fmt" + "math" "sync/atomic" "cogentcore.org/core/math32" @@ -29,6 +30,9 @@ const ( // the other body index ContactB + // contact point index for A-B pair + ContactPointIdx + // contact point on body A ContactAPointX ContactAPointY @@ -54,6 +58,70 @@ const ( ContactForceZ ) +func SetContactA(idx, bodIdx int32) { + Contacts.Set(math.Float32frombits(uint32(bodIdx)), int(idx), int(ContactA)) +} + +func GetContactA(idx int32) int32 { + return int32(math.Float32bits(Contacts.Value(int(idx), int(ContactA)))) +} + +func SetContactB(idx, bodIdx int32) { + Contacts.Set(math.Float32frombits(uint32(bodIdx)), int(idx), int(ContactB)) +} + +func GetContactB(idx int32) int32 { + return int32(math.Float32bits(Contacts.Value(int(idx), int(ContactB)))) +} + +func SetContactPointIdx(idx, ptIdx int32) { + Contacts.Set(math.Float32frombits(uint32(ptIdx)), int(idx), int(ContactPointIdx)) +} + +func GetContactPointIdx(idx int32) int32 { + return int32(math.Float32bits(Contacts.Value(int(idx), int(ContactPointIdx)))) +} + +func ContactAPoint(idx int32) math32.Vector3 { + return math32.Vec3(Contacts.Value(int(idx), int(ContactAPointX)), Contacts.Value(int(idx), int(ContactAPointY)), Contacts.Value(int(idx), int(ContactAPointZ))) +} + +func SetContactAPoint(idx int32, pos math32.Vector3) { + Contacts.Set(pos.X, int(idx), int(ContactAPointX)) + Contacts.Set(pos.Y, int(idx), int(ContactAPointY)) + Contacts.Set(pos.Z, int(idx), int(ContactAPointZ)) +} + +func ContactBPoint(idx int32) math32.Vector3 { + return math32.Vec3(Contacts.Value(int(idx), int(ContactBPointX)), Contacts.Value(int(idx), int(ContactBPointY)), Contacts.Value(int(idx), int(ContactBPointZ))) +} + +func SetContactBPoint(idx int32, pos math32.Vector3) { + Contacts.Set(pos.X, int(idx), int(ContactBPointX)) + Contacts.Set(pos.Y, int(idx), int(ContactBPointY)) + Contacts.Set(pos.Z, int(idx), int(ContactBPointZ)) +} + +func ContactNorm(idx int32) math32.Vector3 { + return math32.Vec3(Contacts.Value(int(idx), int(ContactNormX)), Contacts.Value(int(idx), int(ContactNormY)), Contacts.Value(int(idx), int(ContactNormZ))) +} + +func SetContactNorm(idx int32, pos math32.Vector3) { + Contacts.Set(pos.X, int(idx), int(ContactNormX)) + Contacts.Set(pos.Y, int(idx), int(ContactNormY)) + Contacts.Set(pos.Z, int(idx), int(ContactNormZ)) +} + +func ContactForce(idx int32) math32.Vector3 { + return math32.Vec3(Contacts.Value(int(idx), int(ContactForceX)), Contacts.Value(int(idx), int(ContactForceY)), Contacts.Value(int(idx), int(ContactForceZ))) +} + +func SetContactForce(idx int32, pos math32.Vector3) { + Contacts.Set(pos.X, int(idx), int(ContactForceX)) + Contacts.Set(pos.Y, int(idx), int(ContactForceY)) + Contacts.Set(pos.Z, int(idx), int(ContactForceZ)) +} + func WorldsCollide(wa, wb int32) bool { if wa != -1 && wb != -1 && wa != wb { return false @@ -74,7 +142,7 @@ func GroupsCollide(ga, gb int32) bool { return false } -// geometry/kernels.py/broadphase_collision_pairs +// newton: geometry/kernels.py: broadphase_collision_pairs // CollisionBroad performs broad-phase collision detection, generating Contacts. func CollisionBroad(i uint32) { //gosl:kernel @@ -83,19 +151,19 @@ func CollisionBroad(i uint32) { //gosl:kernel if ci >= params.BodyCollidePairsN { return } - ba := BodyCollidePairs.Value(int(ci), int(0)) - bb := BodyCollidePairs.Value(int(ci), int(1)) + biA := BodyCollidePairs.Value(int(ci), int(0)) + biB := BodyCollidePairs.Value(int(ci), int(1)) - xwAR := BodyDynamicPos(ba, params.Cur) - xwAQ := BodyDynamicQuat(ba, params.Cur) - xwBR := BodyDynamicPos(bb, params.Cur) + xwAR := BodyDynamicPos(biA, params.Cur) + xwAQ := BodyDynamicQuat(biA, params.Cur) + xwBR := BodyDynamicPos(biB, params.Cur) // xwBQ := BodyDynamicQuat(bb, params.Cur) - // note: at <= bt - sA := GetBodyShape(ba) - sB := GetBodyShape(bb) + // note: sA <= sB + sA := GetBodyShape(biA) + sB := GetBodyShape(biB) - rb := Bodies.Value(int(bb), int(BodyRadius)) + rb := Bodies.Value(int(biB), int(BodyRadius)) // if type_a == GeoType.PLANE and type_b == GeoType.PLANE: // return @@ -106,7 +174,7 @@ func CollisionBroad(i uint32) { //gosl:kernel // bounding sphere check infPlane := false if sA == Plane { - szA := BodySize(ba) + szA := BodySize(biA) if szA.X == 0 { infPlane = true } @@ -118,7 +186,7 @@ func CollisionBroad(i uint32) { //gosl:kernel } } else { d := slmath.Length3(xwAR.Sub(xwBR)) - ra := Bodies.Value(int(ba), int(BodyRadius)) + ra := Bodies.Value(int(biA), int(BodyRadius)) if d > ra+rb+margin { return } @@ -128,60 +196,37 @@ func CollisionBroad(i uint32) { //gosl:kernel var ncB int32 ncA := ShapePairContacts(sA, sB, infPlane, &ncB) - _ = ncA - - // ignoring this for now: - // if contact_point_limit: - // # assign a limit per contact pair, if max_per_pair is set - // if max_per_pair > 0: - // # distribute maximum number of contact per pair in both directions - // max_per_pair_half = max_per_pair // 2 - // if num_contacts_b > 0: - // contact_point_limit[pair_index_ab] = max_per_pair_half - // contact_point_limit[pair_index_ba] = max_per_pair_half - // else: - // contact_point_limit[pair_index_ab] = max_per_pair - // contact_point_limit[pair_index_ba] = 0 - // else: - // contact_point_limit[pair_index_ab] = 0 - // contact_point_limit[pair_index_ba] = 0 - - // now we just write results to big buffer of contact points. - - nci := atomic.AddInt32(&ContactCount.Values[int(params.Cur)], ncA+ncB) // returns previous? + + // note: ignoring contact_point_limit code for now + + enci := atomic.AddInt32(&ContactCount.Values[int(params.Cur)], ncA+ncB) + // Go returns post-added value, while WGSL returns pre-added value //gosl:wgsl - // nci += ncA + ncB // wgsl returns orig, Go returns new + // enci += ncA + ncB // wgsl now matches Go //gosl:end - if nci > params.ContactsMax { + nci := enci - (ncA + ncB) // starting index + if nci >= params.ContactsMax { // shouldn't happen! return } + AddContacts(biA, biB, ci, ncA, ncB) +} +// newton: geometry/kernels.py: allocate_contact_points + +// AddContacts adds contact records in prep for narrow phase. +func AddContacts(biA, biB, ci, ncA, ncB int32) { for i := range ncA { - // todo: accessors - Contacts.Set(ba, int(nci+i), int(ContactA)) - Contacts.Set(bb, int(nci+i), int(ContactB)) - // etc + SetContactA(ci+i, biA) + SetContactB(ci+i, biB) + SetContactPointIdx(ci+i, i) } for i := range ncB { - Contacts.Set(ba, int(nci+ncA+i), int(ContactA)) // flip?? - Contacts.Set(bb, int(nci+ncA+i), int(ContactA)) + SetContactA(ci+ncA+i, biB) // flipped + SetContactB(ci+ncA+i, biA) + SetContactPointIdx(ci+i, i) } - // # Allocate contact points using reusable method - // _success = allocate_contact_points( - // - // num_contacts_a, - // num_contacts_b, - // shape_a, - // shape_b, - // rigid_contact_max, - // contact_count, - // contact_shape0, - // contact_shape1, - // contact_point_id, - // - // ) } // ClosestPointPlane projects the point onto the quad in @@ -212,6 +257,8 @@ func (wl *World) IsChildDynamic(dip, dic int32) bool { return false } +// newton: sim/builder.py: find_shape_contact_pairs + // ConfigBodyCollidePairs compiles a list of body paris that could collide // based on world and group settings and not being direct parent // child relationship within a joint. Result has A with lower shape type, @@ -261,50 +308,40 @@ func (wl *World) ConfigBodyCollidePairs() { params.BodyCollidePairsN = int32(np) pt.SetShapeSizes(np, 2) wl.BodyCollidePairs = pt - wl.Contacts.SetShapeSizes(np, int(ContactVarsN)) fmt.Println("body pairs over alloc", nalc, np) } -// New adds a new contact to the list -// func (cs *Contacts) New(a, b Body) *Contact { -// c := &Contact{A: a, B: b} -// *cs = append(*cs, c) -// return c -// } -// -// // BodyVelBBoxIntersects returns the list of potential contact nodes between a and b -// // (could be the same or different groups) that have intersecting velocity-projected -// // bounding boxes. In general a should be dynamic bodies and b either dynamic or static. -// // This is the broad first-pass filtering. -// func BodyVelBBoxIntersects(a, b Node) Contacts { -// var cts Contacts -// a.AsTree().WalkDown(func(k tree.Node) bool { -// aii, ai := AsNode(k) -// if aii == nil { -// return false // going into a different type of thing, bail -// } -// abod := aii.AsBody() // only consider bodies for collision -// if abod == nil { -// return true -// } -// -// b.AsTree().WalkDown(func(k tree.Node) bool { -// bii, bi := AsNode(k) -// if bii == nil { -// return false // going into a different type of thing, bail -// } -// if !ai.BBox.IntersectsVelBox(&bi.BBox) { -// return false // done -// } -// bbod := bii.AsBody() // only consider bodies for collision -// if bbod == nil { -// return true -// } -// cts.New(abod, bbod) -// return false // done -// }) -// -// return false -// }) -// return cts -// } +// newton: geometry/kernels.py: count_contact_points + +// SetMaxContacts computes [Params.MaxContacts] based on current list of +// [BodyCollidePairs]. +func (wl *World) SetMaxContacts() { + params := &wl.Params[0] + + n := int32(0) + for ci := range params.BodyCollidePairsN { + biA := BodyCollidePairs.Value(int(ci), int(0)) + biB := BodyCollidePairs.Value(int(ci), int(1)) + + // note: sA <= sB + sA := GetBodyShape(biA) + sB := GetBodyShape(biB) + + infPlane := false + szA := BodySize(biA) + if szA.X == 0 { + infPlane = true + } + + var ncB int32 + ncA := ShapePairContacts(sA, sB, infPlane, &ncB) + n += ncA + ncB + } + // todo: this is a massive over-estimate, b/c there is no way everyone could be + // colliding at once. Except.. if it is a very small model. + if params.BodyCollidePairsN > 1000 { + n = n / 2 // todo: could do more of this as N gets larger + } + params.ContactsMax = n + wl.Contacts.SetShapeSizes(int(n), int(ContactVarsN)) +} diff --git a/physics/contact.goal b/physics/contact.goal index 7bf417f6..a24b3cd5 100644 --- a/physics/contact.goal +++ b/physics/contact.goal @@ -6,6 +6,7 @@ package physics import ( "fmt" + "math" "sync/atomic" "cogentcore.org/core/math32" @@ -27,6 +28,9 @@ const ( // the other body index ContactB + // contact point index for A-B pair + ContactPointIdx + // contact point on body A ContactAPointX ContactAPointY @@ -52,6 +56,71 @@ const ( ContactForceZ ) + +func SetContactA(idx, bodIdx int32) { + Contacts[idx, ContactA] = math.Float32frombits(uint32(bodIdx)) +} + +func GetContactA(idx int32) int32 { + return int32(math.Float32bits(Contacts[idx, ContactA])) +} + +func SetContactB(idx, bodIdx int32) { + Contacts[idx, ContactB] = math.Float32frombits(uint32(bodIdx)) +} + +func GetContactB(idx int32) int32 { + return int32(math.Float32bits(Contacts[idx, ContactB])) +} + +func SetContactPointIdx(idx, ptIdx int32) { + Contacts[idx, ContactPointIdx] = math.Float32frombits(uint32(ptIdx)) +} + +func GetContactPointIdx(idx int32) int32 { + return int32(math.Float32bits(Contacts[idx, ContactPointIdx])) +} + +func ContactAPoint(idx int32) math32.Vector3 { + return math32.Vec3(Contacts[idx, ContactAPointX], Contacts[idx, ContactAPointY], Contacts[idx, ContactAPointZ]) +} + +func SetContactAPoint(idx int32, pos math32.Vector3) { + Contacts[idx, ContactAPointX] = pos.X + Contacts[idx, ContactAPointY] = pos.Y + Contacts[idx, ContactAPointZ] = pos.Z +} + +func ContactBPoint(idx int32) math32.Vector3 { + return math32.Vec3(Contacts[idx, ContactBPointX], Contacts[idx, ContactBPointY], Contacts[idx, ContactBPointZ]) +} + +func SetContactBPoint(idx int32, pos math32.Vector3) { + Contacts[idx, ContactBPointX] = pos.X + Contacts[idx, ContactBPointY] = pos.Y + Contacts[idx, ContactBPointZ] = pos.Z +} + +func ContactNorm(idx int32) math32.Vector3 { + return math32.Vec3(Contacts[idx, ContactNormX], Contacts[idx, ContactNormY], Contacts[idx, ContactNormZ]) +} + +func SetContactNorm(idx int32, pos math32.Vector3) { + Contacts[idx, ContactNormX] = pos.X + Contacts[idx, ContactNormY] = pos.Y + Contacts[idx, ContactNormZ] = pos.Z +} + +func ContactForce(idx int32) math32.Vector3 { + return math32.Vec3(Contacts[idx, ContactForceX], Contacts[idx, ContactForceY], Contacts[idx, ContactForceZ]) +} + +func SetContactForce(idx int32, pos math32.Vector3) { + Contacts[idx, ContactForceX] = pos.X + Contacts[idx, ContactForceY] = pos.Y + Contacts[idx, ContactForceZ] = pos.Z +} + func WorldsCollide(wa, wb int32) bool { if wa != -1 && wb != -1 && wa != wb { return false @@ -72,7 +141,7 @@ func GroupsCollide(ga, gb int32) bool { return false } -// geometry/kernels.py/broadphase_collision_pairs +// newton: geometry/kernels.py: broadphase_collision_pairs // CollisionBroad performs broad-phase collision detection, generating Contacts. func CollisionBroad(i uint32) { //gosl:kernel @@ -81,19 +150,19 @@ func CollisionBroad(i uint32) { //gosl:kernel if ci >= params.BodyCollidePairsN { return } - ba := BodyCollidePairs[ci, 0] - bb := BodyCollidePairs[ci, 1] + biA := BodyCollidePairs[ci, 0] + biB := BodyCollidePairs[ci, 1] - xwAR := BodyDynamicPos(ba, params.Cur) - xwAQ := BodyDynamicQuat(ba, params.Cur) - xwBR := BodyDynamicPos(bb, params.Cur) + xwAR := BodyDynamicPos(biA, params.Cur) + xwAQ := BodyDynamicQuat(biA, params.Cur) + xwBR := BodyDynamicPos(biB, params.Cur) // xwBQ := BodyDynamicQuat(bb, params.Cur) - // note: at <= bt - sA := GetBodyShape(ba) - sB := GetBodyShape(bb) + // note: sA <= sB + sA := GetBodyShape(biA) + sB := GetBodyShape(biB) - rb := Bodies[bb, BodyRadius] + rb := Bodies[biB, BodyRadius] // if type_a == GeoType.PLANE and type_b == GeoType.PLANE: // return @@ -104,7 +173,7 @@ func CollisionBroad(i uint32) { //gosl:kernel // bounding sphere check infPlane := false if sA == Plane { - szA := BodySize(ba) + szA := BodySize(biA) if szA.X == 0 { infPlane = true } @@ -116,7 +185,7 @@ func CollisionBroad(i uint32) { //gosl:kernel } } else { d := slmath.Length3(xwAR.Sub(xwBR)) - ra := Bodies[ba, BodyRadius] + ra := Bodies[biA, BodyRadius] if d > ra+rb+margin { return } @@ -126,60 +195,40 @@ func CollisionBroad(i uint32) { //gosl:kernel var ncB int32 ncA := ShapePairContacts(sA, sB, infPlane, &ncB) - _ = ncA - - // ignoring this for now: - // if contact_point_limit: - // # assign a limit per contact pair, if max_per_pair is set - // if max_per_pair > 0: - // # distribute maximum number of contact per pair in both directions - // max_per_pair_half = max_per_pair // 2 - // if num_contacts_b > 0: - // contact_point_limit[pair_index_ab] = max_per_pair_half - // contact_point_limit[pair_index_ba] = max_per_pair_half - // else: - // contact_point_limit[pair_index_ab] = max_per_pair - // contact_point_limit[pair_index_ba] = 0 - // else: - // contact_point_limit[pair_index_ab] = 0 - // contact_point_limit[pair_index_ba] = 0 - - // now we just write results to big buffer of contact points. + + // note: ignoring contact_point_limit code for now - nci := atomic.AddInt32(&ContactCount.Values[int(params.Cur)], ncA + ncB) // returns previous? + enci := atomic.AddInt32(&ContactCount.Values[int(params.Cur)], ncA + ncB) + // Go returns post-added value, while WGSL returns pre-added value //gosl:wgsl - // nci += ncA + ncB // wgsl returns orig, Go returns new + // enci += ncA + ncB // wgsl now matches Go //gosl:end - if nci > params.ContactsMax { + nci := enci - (ncA + ncB) // starting index + if nci >= params.ContactsMax { // shouldn't happen! return } + AddContacts(biA, biB, ci, ncA, ncB) +} + +// newton: geometry/kernels.py: allocate_contact_points +// AddContacts adds contact records in prep for narrow phase. +func AddContacts(biA, biB, ci, ncA, ncB int32) { for i := range ncA { - // todo: accessors - Contacts[nci + i, ContactA] = ba - Contacts[nci + i, ContactB] = bb - // etc + SetContactA(ci + i, biA) + SetContactB(ci + i, biB) + SetContactPointIdx(ci+i, i) } for i := range ncB { - Contacts[nci + ncA + i, ContactA] = ba // flip?? - Contacts[nci + ncA + i, ContactA] = bb + SetContactA(ci + ncA + i, biB) // flipped + SetContactB(ci + ncA + i, biA) + SetContactPointIdx(ci+i, i) } - // # Allocate contact points using reusable method - // _success = allocate_contact_points( - // num_contacts_a, - // num_contacts_b, - // shape_a, - // shape_b, - // rigid_contact_max, - // contact_count, - // contact_shape0, - // contact_shape1, - // contact_point_id, - // ) } + // ClosestPointPlane projects the point onto the quad in // the xy plane (if size > 0.0, otherwise infinite. func ClosestPointPlane(sz, pt math32.Vector3) math32.Vector3 { @@ -208,6 +257,8 @@ func (wl *World) IsChildDynamic(dip, dic int32) bool { return false } +// newton: sim/builder.py: find_shape_contact_pairs + // ConfigBodyCollidePairs compiles a list of body paris that could collide // based on world and group settings and not being direct parent // child relationship within a joint. Result has A with lower shape type, @@ -257,50 +308,41 @@ func (wl *World) ConfigBodyCollidePairs() { params.BodyCollidePairsN = int32(np) pt.SetShapeSizes(np, 2) wl.BodyCollidePairs = pt - wl.Contacts.SetShapeSizes(np, int(ContactVarsN)) fmt.Println("body pairs over alloc", nalc, np) } -// New adds a new contact to the list -// func (cs *Contacts) New(a, b Body) *Contact { -// c := &Contact{A: a, B: b} -// *cs = append(*cs, c) -// return c -// } -// -// // BodyVelBBoxIntersects returns the list of potential contact nodes between a and b -// // (could be the same or different groups) that have intersecting velocity-projected -// // bounding boxes. In general a should be dynamic bodies and b either dynamic or static. -// // This is the broad first-pass filtering. -// func BodyVelBBoxIntersects(a, b Node) Contacts { -// var cts Contacts -// a.AsTree().WalkDown(func(k tree.Node) bool { -// aii, ai := AsNode(k) -// if aii == nil { -// return false // going into a different type of thing, bail -// } -// abod := aii.AsBody() // only consider bodies for collision -// if abod == nil { -// return true -// } -// -// b.AsTree().WalkDown(func(k tree.Node) bool { -// bii, bi := AsNode(k) -// if bii == nil { -// return false // going into a different type of thing, bail -// } -// if !ai.BBox.IntersectsVelBox(&bi.BBox) { -// return false // done -// } -// bbod := bii.AsBody() // only consider bodies for collision -// if bbod == nil { -// return true -// } -// cts.New(abod, bbod) -// return false // done -// }) -// -// return false -// }) -// return cts -// } +// newton: geometry/kernels.py: count_contact_points + +// SetMaxContacts computes [Params.MaxContacts] based on current list of +// [BodyCollidePairs]. +func (wl *World) SetMaxContacts() { + params := &wl.Params[0] + + n := int32(0) + for ci := range params.BodyCollidePairsN { + biA := BodyCollidePairs[ci, 0] + biB := BodyCollidePairs[ci, 1] + + // note: sA <= sB + sA := GetBodyShape(biA) + sB := GetBodyShape(biB) + + infPlane := false + szA := BodySize(biA) + if szA.X == 0 { + infPlane = true + } + + var ncB int32 + ncA := ShapePairContacts(sA, sB, infPlane, &ncB) + n += ncA + ncB + } + // todo: this is a massive over-estimate, b/c there is no way everyone could be + // colliding at once. Except.. if it is a very small model. + if params.BodyCollidePairsN > 1000 { + n = n/2 // todo: could do more of this as N gets larger + } + params.ContactsMax = n + wl.Contacts.SetShapeSizes(int(n), int(ContactVarsN)) +} + diff --git a/physics/enumgen.go b/physics/enumgen.go index 7ff33fcb..9560b835 100644 --- a/physics/enumgen.go +++ b/physics/enumgen.go @@ -51,20 +51,20 @@ func (i BodyVars) MarshalText() ([]byte, error) { return []byte(i.String()), nil // UnmarshalText implements the [encoding.TextUnmarshaler] interface. func (i *BodyVars) UnmarshalText(text []byte) error { return enums.UnmarshalText(i, text, "BodyVars") } -var _ContactVarsValues = []ContactVars{0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15} +var _ContactVarsValues = []ContactVars{0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16} // ContactVarsN is the highest valid value for type ContactVars, plus one. // //gosl:start -const ContactVarsN ContactVars = 16 +const ContactVarsN ContactVars = 17 //gosl:end -var _ContactVarsValueMap = map[string]ContactVars{`ContactA`: 0, `ContactB`: 1, `ContactAPointX`: 2, `ContactAPointY`: 3, `ContactAPointZ`: 4, `ContactBPointX`: 5, `ContactBPointY`: 6, `ContactBPointZ`: 7, `ContactADepth`: 8, `ContactBDepth`: 9, `ContactNormX`: 10, `ContactNormY`: 11, `ContactNormZ`: 12, `ContactForceX`: 13, `ContactForceY`: 14, `ContactForceZ`: 15} +var _ContactVarsValueMap = map[string]ContactVars{`ContactA`: 0, `ContactB`: 1, `ContactPointIdx`: 2, `ContactAPointX`: 3, `ContactAPointY`: 4, `ContactAPointZ`: 5, `ContactBPointX`: 6, `ContactBPointY`: 7, `ContactBPointZ`: 8, `ContactADepth`: 9, `ContactBDepth`: 10, `ContactNormX`: 11, `ContactNormY`: 12, `ContactNormZ`: 13, `ContactForceX`: 14, `ContactForceY`: 15, `ContactForceZ`: 16} -var _ContactVarsDescMap = map[ContactVars]string{0: `first body index`, 1: `the other body index`, 2: `contact point on body A`, 3: ``, 4: ``, 5: `contact point on body B`, 6: ``, 7: ``, 8: `Contact depths (thickness in newton)`, 9: ``, 10: `normal pointing from center of B to center of A`, 11: ``, 12: ``, 13: `computed contact force vector`, 14: ``, 15: ``} +var _ContactVarsDescMap = map[ContactVars]string{0: `first body index`, 1: `the other body index`, 2: `contact point index for A-B pair`, 3: `contact point on body A`, 4: ``, 5: ``, 6: `contact point on body B`, 7: ``, 8: ``, 9: `Contact depths (thickness in newton)`, 10: ``, 11: `normal pointing from center of B to center of A`, 12: ``, 13: ``, 14: `computed contact force vector`, 15: ``, 16: ``} -var _ContactVarsMap = map[ContactVars]string{0: `ContactA`, 1: `ContactB`, 2: `ContactAPointX`, 3: `ContactAPointY`, 4: `ContactAPointZ`, 5: `ContactBPointX`, 6: `ContactBPointY`, 7: `ContactBPointZ`, 8: `ContactADepth`, 9: `ContactBDepth`, 10: `ContactNormX`, 11: `ContactNormY`, 12: `ContactNormZ`, 13: `ContactForceX`, 14: `ContactForceY`, 15: `ContactForceZ`} +var _ContactVarsMap = map[ContactVars]string{0: `ContactA`, 1: `ContactB`, 2: `ContactPointIdx`, 3: `ContactAPointX`, 4: `ContactAPointY`, 5: `ContactAPointZ`, 6: `ContactBPointX`, 7: `ContactBPointY`, 8: `ContactBPointZ`, 9: `ContactADepth`, 10: `ContactBDepth`, 11: `ContactNormX`, 12: `ContactNormY`, 13: `ContactNormZ`, 14: `ContactForceX`, 15: `ContactForceY`, 16: `ContactForceZ`} // String returns the string representation of this ContactVars value. func (i ContactVars) String() string { return enums.String(i, _ContactVarsMap) } diff --git a/physics/params.go b/physics/params.go index c90cd54b..2d03aa90 100644 --- a/physics/params.go +++ b/physics/params.go @@ -50,6 +50,9 @@ type PhysParams struct { // around rigid bodies. ContactMargin float32 + // Maximum number of contacts to process at any given point. + ContactsMax int32 + // Index for the current state (0 or 1, alternates with Next). Cur int32 `edit:"-"` @@ -75,6 +78,8 @@ type PhysParams struct { // to examine. BodyCollidePairsN int32 `edit:"-"` + pad, pad1, pad2 int32 + // Gravity is the gravity acceleration function Gravity slvec.Vector3 } diff --git a/physics/shaders/CollisionBroad.wgsl b/physics/shaders/CollisionBroad.wgsl index 00df7821..16eef531 100644 --- a/physics/shaders/CollisionBroad.wgsl +++ b/physics/shaders/CollisionBroad.wgsl @@ -117,60 +117,87 @@ fn BodyDynamicQuat(idx: i32,cni: i32) -> vec4 { alias ContactVars = i32; //enums:enum const ContactA: ContactVars = 0; const ContactB: ContactVars = 1; -const ContactAPointX: ContactVars = 2; -const ContactAPointY: ContactVars = 3; -const ContactAPointZ: ContactVars = 4; -const ContactBPointX: ContactVars = 5; -const ContactBPointY: ContactVars = 6; -const ContactBPointZ: ContactVars = 7; -const ContactADepth: ContactVars = 8; -const ContactBDepth: ContactVars = 9; -const ContactNormX: ContactVars = 10; -const ContactNormY: ContactVars = 11; -const ContactNormZ: ContactVars = 12; -const ContactForceX: ContactVars = 13; -const ContactForceY: ContactVars = 14; -const ContactForceZ: ContactVars = 15; +const ContactPointIdx: ContactVars = 2; +const ContactAPointX: ContactVars = 3; +const ContactAPointY: ContactVars = 4; +const ContactAPointZ: ContactVars = 5; +const ContactBPointX: ContactVars = 6; +const ContactBPointY: ContactVars = 7; +const ContactBPointZ: ContactVars = 8; +const ContactADepth: ContactVars = 9; +const ContactBDepth: ContactVars = 10; +const ContactNormX: ContactVars = 11; +const ContactNormY: ContactVars = 12; +const ContactNormZ: ContactVars = 13; +const ContactForceX: ContactVars = 14; +const ContactForceY: ContactVars = 15; +const ContactForceZ: ContactVars = 16; +fn SetContactA(idx: i32,bodIdx: i32) { + Contacts[Index2D(TensorStrides[70], TensorStrides[71], u32(idx), u32(ContactA))] = bitcast(u32(bodIdx)); +} +fn SetContactB(idx: i32,bodIdx: i32) { + Contacts[Index2D(TensorStrides[70], TensorStrides[71], u32(idx), u32(ContactB))] = bitcast(u32(bodIdx)); +} +fn SetContactPointIdx(idx: i32,ptIdx: i32) { + Contacts[Index2D(TensorStrides[70], TensorStrides[71], u32(idx), u32(ContactPointIdx))] = bitcast(u32(ptIdx)); +} fn CollisionBroad(i: u32) { //gosl:kernel -let params = Params[0];; var ci = i32(i);; if (ci >= params.BodyCollidePairsN) { - return; -}; var ba = BodyCollidePairs[Index2D(TensorStrides[40], TensorStrides[41], u32(ci), u32(0))];; var bb = BodyCollidePairs[Index2D(TensorStrides[40], TensorStrides[41], u32(ci), u32(1))];; var xwAR = BodyDynamicPos(ba, params.Cur);; var xwAQ = BodyDynamicQuat(ba, params.Cur);; var xwBR = BodyDynamicPos(bb, params.Cur);; -var sA = GetBodyShape(ba);; var sB = GetBodyShape(bb);; var rb = Bodies[Index2D(TensorStrides[0], TensorStrides[1], -u32(bb), u32(BodyRadius))];; -var margin = params.ContactMargin;; -var infPlane = false;; if (sA == Plane) { - var szA = BodySize(ba); - if (szA.x == 0) { - infPlane = true; - } - var queryB = MulSpatialPoint(xwAR, xwAQ, xwBR); - var closest = ClosestPointPlane(szA, queryB); - var d = Length3(queryB-(closest)); - if (d > rb+margin) { + let params = Params[0]; + var ci = i32(i); + if (ci >= params.BodyCollidePairsN) { return; } -} else { - var d = Length3(xwAR-(xwBR)); - var ra = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(ba), u32(BodyRadius))]; - if (d > ra+rb+margin) { + var biA = BodyCollidePairs[Index2D(TensorStrides[40], TensorStrides[41], u32(ci), u32(0))]; + var biB = BodyCollidePairs[Index2D(TensorStrides[40], TensorStrides[41], u32(ci), u32(1))]; + var xwAR = BodyDynamicPos(biA, params.Cur); + var xwAQ = BodyDynamicQuat(biA, params.Cur); + var xwBR = BodyDynamicPos(biB, params.Cur); + var sA = GetBodyShape(biA); + var sB = GetBodyShape(biB); + var rb = Bodies[Index2D(TensorStrides[0], TensorStrides[1], + u32(biB), u32(BodyRadius))]; + var margin = params.ContactMargin; + var infPlane = false; + if (sA == Plane) { + var szA = BodySize(biA); + if (szA.x == 0) { + infPlane = true; + } + var queryB = MulSpatialPoint(xwAR, xwAQ, xwBR); + var closest = ClosestPointPlane(szA, queryB); + var d = Length3(queryB-(closest)); + if (d > rb+margin) { + return; + } + } else { + var d = Length3(xwAR-(xwBR)); + var ra = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(biA), u32(BodyRadius))]; + if (d > ra+rb+margin) { + return; + } + } + var ncB: i32; + var ncA = ShapePairContacts(sA, sB, infPlane, &ncB); + var enci = atomicAdd(&ContactCount[i32(params.Cur)], ncA+ncB); + enci += ncA + ncB; // wgsl now matches Go + var nci = enci - (ncA + ncB); // starting index + if (nci >= params.ContactsMax) { // shouldn't happen! return; } -}; -var ncB: i32;; var ncA = ShapePairContacts(sA, sB, infPlane, &ncB);; _ = ncA;; -var nci = atomicAdd(&ContactCount[i32(params.Cur)], ncA+ncB);; // returns previous? -nci += ncA + ncB;; // wgsl returns orig, Go returns new -if (nci > params.ContactsMax) { - return; -}; for (var i=0; i,pt: vec3) -> vec3 { var cp = pt; if (sz.x == 0.0) { @@ -229,7 +256,7 @@ fn DynamicQuat(idx: i32,cni: i32) -> vec4 { //////// import: "enumgen.go" const BodyVarsN: BodyVars = 40; -const ContactVarsN: ContactVars = 16; +const ContactVarsN: ContactVars = 17; const JointControlVarsN: JointControlVars = 3; const DynamicVarsN: DynamicVars = 32; const GPUVarsN: GPUVars = 10; @@ -322,6 +349,7 @@ struct PhysParams { ContactWeighting: i32, Restitution: i32, ContactMargin: f32, + ContactsMax: i32, Cur: i32, Next: i32, BodiesN: i32, @@ -330,6 +358,9 @@ struct PhysParams { JointDoFsN: i32, BodyJointsMax: i32, BodyCollidePairsN: i32, + pad: i32, + pad1: i32, + pad2: i32, Gravity: vec4, } diff --git a/physics/shaders/DeltasFromJoints.wgsl b/physics/shaders/DeltasFromJoints.wgsl index 68dc8d98..7724d9d6 100644 --- a/physics/shaders/DeltasFromJoints.wgsl +++ b/physics/shaders/DeltasFromJoints.wgsl @@ -82,20 +82,21 @@ const BodyRadius: BodyVars = 39; alias ContactVars = i32; //enums:enum const ContactA: ContactVars = 0; const ContactB: ContactVars = 1; -const ContactAPointX: ContactVars = 2; -const ContactAPointY: ContactVars = 3; -const ContactAPointZ: ContactVars = 4; -const ContactBPointX: ContactVars = 5; -const ContactBPointY: ContactVars = 6; -const ContactBPointZ: ContactVars = 7; -const ContactADepth: ContactVars = 8; -const ContactBDepth: ContactVars = 9; -const ContactNormX: ContactVars = 10; -const ContactNormY: ContactVars = 11; -const ContactNormZ: ContactVars = 12; -const ContactForceX: ContactVars = 13; -const ContactForceY: ContactVars = 14; -const ContactForceZ: ContactVars = 15; +const ContactPointIdx: ContactVars = 2; +const ContactAPointX: ContactVars = 3; +const ContactAPointY: ContactVars = 4; +const ContactAPointZ: ContactVars = 5; +const ContactBPointX: ContactVars = 6; +const ContactBPointY: ContactVars = 7; +const ContactBPointZ: ContactVars = 8; +const ContactADepth: ContactVars = 9; +const ContactBDepth: ContactVars = 10; +const ContactNormX: ContactVars = 11; +const ContactNormY: ContactVars = 12; +const ContactNormZ: ContactVars = 13; +const ContactForceX: ContactVars = 14; +const ContactForceY: ContactVars = 15; +const ContactForceZ: ContactVars = 16; //////// import: "control.go" alias JointControlVars = i32; //enums:enum @@ -150,7 +151,7 @@ fn SetDynamicAngDelta(idx: i32,cni: i32, angDelta: vec3) { //////// import: "enumgen.go" const BodyVarsN: BodyVars = 40; -const ContactVarsN: ContactVars = 16; +const ContactVarsN: ContactVars = 17; const JointControlVarsN: JointControlVars = 3; const DynamicVarsN: DynamicVars = 32; const GPUVarsN: GPUVars = 10; @@ -255,6 +256,7 @@ struct PhysParams { ContactWeighting: i32, Restitution: i32, ContactMargin: f32, + ContactsMax: i32, Cur: i32, Next: i32, BodiesN: i32, @@ -263,6 +265,9 @@ struct PhysParams { JointDoFsN: i32, BodyJointsMax: i32, BodyCollidePairsN: i32, + pad: i32, + pad1: i32, + pad2: i32, Gravity: vec4, } diff --git a/physics/shaders/DynamicsCurToNext.wgsl b/physics/shaders/DynamicsCurToNext.wgsl index c7b8e884..5218b681 100644 --- a/physics/shaders/DynamicsCurToNext.wgsl +++ b/physics/shaders/DynamicsCurToNext.wgsl @@ -74,20 +74,21 @@ const BodyRadius: BodyVars = 39; alias ContactVars = i32; //enums:enum const ContactA: ContactVars = 0; const ContactB: ContactVars = 1; -const ContactAPointX: ContactVars = 2; -const ContactAPointY: ContactVars = 3; -const ContactAPointZ: ContactVars = 4; -const ContactBPointX: ContactVars = 5; -const ContactBPointY: ContactVars = 6; -const ContactBPointZ: ContactVars = 7; -const ContactADepth: ContactVars = 8; -const ContactBDepth: ContactVars = 9; -const ContactNormX: ContactVars = 10; -const ContactNormY: ContactVars = 11; -const ContactNormZ: ContactVars = 12; -const ContactForceX: ContactVars = 13; -const ContactForceY: ContactVars = 14; -const ContactForceZ: ContactVars = 15; +const ContactPointIdx: ContactVars = 2; +const ContactAPointX: ContactVars = 3; +const ContactAPointY: ContactVars = 4; +const ContactAPointZ: ContactVars = 5; +const ContactBPointX: ContactVars = 6; +const ContactBPointY: ContactVars = 7; +const ContactBPointZ: ContactVars = 8; +const ContactADepth: ContactVars = 9; +const ContactBDepth: ContactVars = 10; +const ContactNormX: ContactVars = 11; +const ContactNormY: ContactVars = 12; +const ContactNormZ: ContactVars = 13; +const ContactForceX: ContactVars = 14; +const ContactForceY: ContactVars = 15; +const ContactForceZ: ContactVars = 16; //////// import: "control.go" alias JointControlVars = i32; //enums:enum @@ -132,7 +133,7 @@ const DynAngDeltaZ: DynamicVars = 31; //////// import: "enumgen.go" const BodyVarsN: BodyVars = 40; -const ContactVarsN: ContactVars = 16; +const ContactVarsN: ContactVars = 17; const JointControlVarsN: JointControlVars = 3; const DynamicVarsN: DynamicVars = 32; const GPUVarsN: GPUVars = 10; @@ -225,6 +226,7 @@ struct PhysParams { ContactWeighting: i32, Restitution: i32, ContactMargin: f32, + ContactsMax: i32, Cur: i32, Next: i32, BodiesN: i32, @@ -233,6 +235,9 @@ struct PhysParams { JointDoFsN: i32, BodyJointsMax: i32, BodyCollidePairsN: i32, + pad: i32, + pad1: i32, + pad2: i32, Gravity: vec4, } diff --git a/physics/shaders/ForcesFromJoints.wgsl b/physics/shaders/ForcesFromJoints.wgsl index 000dbbaa..3aea568e 100644 --- a/physics/shaders/ForcesFromJoints.wgsl +++ b/physics/shaders/ForcesFromJoints.wgsl @@ -82,20 +82,21 @@ const BodyRadius: BodyVars = 39; alias ContactVars = i32; //enums:enum const ContactA: ContactVars = 0; const ContactB: ContactVars = 1; -const ContactAPointX: ContactVars = 2; -const ContactAPointY: ContactVars = 3; -const ContactAPointZ: ContactVars = 4; -const ContactBPointX: ContactVars = 5; -const ContactBPointY: ContactVars = 6; -const ContactBPointZ: ContactVars = 7; -const ContactADepth: ContactVars = 8; -const ContactBDepth: ContactVars = 9; -const ContactNormX: ContactVars = 10; -const ContactNormY: ContactVars = 11; -const ContactNormZ: ContactVars = 12; -const ContactForceX: ContactVars = 13; -const ContactForceY: ContactVars = 14; -const ContactForceZ: ContactVars = 15; +const ContactPointIdx: ContactVars = 2; +const ContactAPointX: ContactVars = 3; +const ContactAPointY: ContactVars = 4; +const ContactAPointZ: ContactVars = 5; +const ContactBPointX: ContactVars = 6; +const ContactBPointY: ContactVars = 7; +const ContactBPointZ: ContactVars = 8; +const ContactADepth: ContactVars = 9; +const ContactBDepth: ContactVars = 10; +const ContactNormX: ContactVars = 11; +const ContactNormY: ContactVars = 12; +const ContactNormZ: ContactVars = 13; +const ContactForceX: ContactVars = 14; +const ContactForceY: ContactVars = 15; +const ContactForceZ: ContactVars = 16; //////// import: "control.go" alias JointControlVars = i32; //enums:enum @@ -150,7 +151,7 @@ fn SetDynamicTorque(idx: i32,cni: i32, torque: vec3) { //////// import: "enumgen.go" const BodyVarsN: BodyVars = 40; -const ContactVarsN: ContactVars = 16; +const ContactVarsN: ContactVars = 17; const JointControlVarsN: JointControlVars = 3; const DynamicVarsN: DynamicVars = 32; const GPUVarsN: GPUVars = 10; @@ -255,6 +256,7 @@ struct PhysParams { ContactWeighting: i32, Restitution: i32, ContactMargin: f32, + ContactsMax: i32, Cur: i32, Next: i32, BodiesN: i32, @@ -263,6 +265,9 @@ struct PhysParams { JointDoFsN: i32, BodyJointsMax: i32, BodyCollidePairsN: i32, + pad: i32, + pad1: i32, + pad2: i32, Gravity: vec4, } diff --git a/physics/shaders/InitDynamics.wgsl b/physics/shaders/InitDynamics.wgsl index df625a9d..fc23ff4c 100644 --- a/physics/shaders/InitDynamics.wgsl +++ b/physics/shaders/InitDynamics.wgsl @@ -80,20 +80,21 @@ const BodyRadius: BodyVars = 39; alias ContactVars = i32; //enums:enum const ContactA: ContactVars = 0; const ContactB: ContactVars = 1; -const ContactAPointX: ContactVars = 2; -const ContactAPointY: ContactVars = 3; -const ContactAPointZ: ContactVars = 4; -const ContactBPointX: ContactVars = 5; -const ContactBPointY: ContactVars = 6; -const ContactBPointZ: ContactVars = 7; -const ContactADepth: ContactVars = 8; -const ContactBDepth: ContactVars = 9; -const ContactNormX: ContactVars = 10; -const ContactNormY: ContactVars = 11; -const ContactNormZ: ContactVars = 12; -const ContactForceX: ContactVars = 13; -const ContactForceY: ContactVars = 14; -const ContactForceZ: ContactVars = 15; +const ContactPointIdx: ContactVars = 2; +const ContactAPointX: ContactVars = 3; +const ContactAPointY: ContactVars = 4; +const ContactAPointZ: ContactVars = 5; +const ContactBPointX: ContactVars = 6; +const ContactBPointY: ContactVars = 7; +const ContactBPointZ: ContactVars = 8; +const ContactADepth: ContactVars = 9; +const ContactBDepth: ContactVars = 10; +const ContactNormX: ContactVars = 11; +const ContactNormY: ContactVars = 12; +const ContactNormZ: ContactVars = 13; +const ContactForceX: ContactVars = 14; +const ContactForceY: ContactVars = 15; +const ContactForceZ: ContactVars = 16; //////// import: "control.go" alias JointControlVars = i32; //enums:enum @@ -141,7 +142,7 @@ fn DynamicBody(idx: i32) -> i32 { //////// import: "enumgen.go" const BodyVarsN: BodyVars = 40; -const ContactVarsN: ContactVars = 16; +const ContactVarsN: ContactVars = 17; const JointControlVarsN: JointControlVars = 3; const DynamicVarsN: DynamicVars = 32; const GPUVarsN: GPUVars = 10; @@ -234,6 +235,7 @@ struct PhysParams { ContactWeighting: i32, Restitution: i32, ContactMargin: f32, + ContactsMax: i32, Cur: i32, Next: i32, BodiesN: i32, @@ -242,6 +244,9 @@ struct PhysParams { JointDoFsN: i32, BodyJointsMax: i32, BodyCollidePairsN: i32, + pad: i32, + pad1: i32, + pad2: i32, Gravity: vec4, } diff --git a/physics/shaders/StepBodyDeltas.wgsl b/physics/shaders/StepBodyDeltas.wgsl index 7d71e1e2..bab3e14c 100644 --- a/physics/shaders/StepBodyDeltas.wgsl +++ b/physics/shaders/StepBodyDeltas.wgsl @@ -93,20 +93,21 @@ fn BodyInvInertia(idx: i32) -> mat3x3f { alias ContactVars = i32; //enums:enum const ContactA: ContactVars = 0; const ContactB: ContactVars = 1; -const ContactAPointX: ContactVars = 2; -const ContactAPointY: ContactVars = 3; -const ContactAPointZ: ContactVars = 4; -const ContactBPointX: ContactVars = 5; -const ContactBPointY: ContactVars = 6; -const ContactBPointZ: ContactVars = 7; -const ContactADepth: ContactVars = 8; -const ContactBDepth: ContactVars = 9; -const ContactNormX: ContactVars = 10; -const ContactNormY: ContactVars = 11; -const ContactNormZ: ContactVars = 12; -const ContactForceX: ContactVars = 13; -const ContactForceY: ContactVars = 14; -const ContactForceZ: ContactVars = 15; +const ContactPointIdx: ContactVars = 2; +const ContactAPointX: ContactVars = 3; +const ContactAPointY: ContactVars = 4; +const ContactAPointZ: ContactVars = 5; +const ContactBPointX: ContactVars = 6; +const ContactBPointY: ContactVars = 7; +const ContactBPointZ: ContactVars = 8; +const ContactADepth: ContactVars = 9; +const ContactBDepth: ContactVars = 10; +const ContactNormX: ContactVars = 11; +const ContactNormY: ContactVars = 12; +const ContactNormZ: ContactVars = 13; +const ContactForceX: ContactVars = 14; +const ContactForceY: ContactVars = 15; +const ContactForceZ: ContactVars = 16; //////// import: "control.go" alias JointControlVars = i32; //enums:enum @@ -187,7 +188,7 @@ fn SetDynamicAngDelta(idx: i32,cni: i32, angDelta: vec3) { //////// import: "enumgen.go" const BodyVarsN: BodyVars = 40; -const ContactVarsN: ContactVars = 16; +const ContactVarsN: ContactVars = 17; const JointControlVarsN: JointControlVars = 3; const DynamicVarsN: DynamicVars = 32; const GPUVarsN: GPUVars = 10; @@ -280,6 +281,7 @@ struct PhysParams { ContactWeighting: i32, Restitution: i32, ContactMargin: f32, + ContactsMax: i32, Cur: i32, Next: i32, BodiesN: i32, @@ -288,6 +290,9 @@ struct PhysParams { JointDoFsN: i32, BodyJointsMax: i32, BodyCollidePairsN: i32, + pad: i32, + pad1: i32, + pad2: i32, Gravity: vec4, } diff --git a/physics/shaders/StepIntegrateBodies.wgsl b/physics/shaders/StepIntegrateBodies.wgsl index c725fc08..54b6e0ec 100644 --- a/physics/shaders/StepIntegrateBodies.wgsl +++ b/physics/shaders/StepIntegrateBodies.wgsl @@ -93,20 +93,21 @@ fn BodyInvInertia(idx: i32) -> mat3x3f { alias ContactVars = i32; //enums:enum const ContactA: ContactVars = 0; const ContactB: ContactVars = 1; -const ContactAPointX: ContactVars = 2; -const ContactAPointY: ContactVars = 3; -const ContactAPointZ: ContactVars = 4; -const ContactBPointX: ContactVars = 5; -const ContactBPointY: ContactVars = 6; -const ContactBPointZ: ContactVars = 7; -const ContactADepth: ContactVars = 8; -const ContactBDepth: ContactVars = 9; -const ContactNormX: ContactVars = 10; -const ContactNormY: ContactVars = 11; -const ContactNormZ: ContactVars = 12; -const ContactForceX: ContactVars = 13; -const ContactForceY: ContactVars = 14; -const ContactForceZ: ContactVars = 15; +const ContactPointIdx: ContactVars = 2; +const ContactAPointX: ContactVars = 3; +const ContactAPointY: ContactVars = 4; +const ContactAPointZ: ContactVars = 5; +const ContactBPointX: ContactVars = 6; +const ContactBPointY: ContactVars = 7; +const ContactBPointZ: ContactVars = 8; +const ContactADepth: ContactVars = 9; +const ContactBDepth: ContactVars = 10; +const ContactNormX: ContactVars = 11; +const ContactNormY: ContactVars = 12; +const ContactNormZ: ContactVars = 13; +const ContactForceX: ContactVars = 14; +const ContactForceY: ContactVars = 15; +const ContactForceZ: ContactVars = 16; //////// import: "control.go" alias JointControlVars = i32; //enums:enum @@ -193,7 +194,7 @@ fn SetDynamicAngDelta(idx: i32,cni: i32, angDelta: vec3) { //////// import: "enumgen.go" const BodyVarsN: BodyVars = 40; -const ContactVarsN: ContactVars = 16; +const ContactVarsN: ContactVars = 17; const JointControlVarsN: JointControlVars = 3; const DynamicVarsN: DynamicVars = 32; const GPUVarsN: GPUVars = 10; @@ -286,6 +287,7 @@ struct PhysParams { ContactWeighting: i32, Restitution: i32, ContactMargin: f32, + ContactsMax: i32, Cur: i32, Next: i32, BodiesN: i32, @@ -294,6 +296,9 @@ struct PhysParams { JointDoFsN: i32, BodyJointsMax: i32, BodyCollidePairsN: i32, + pad: i32, + pad1: i32, + pad2: i32, Gravity: vec4, } diff --git a/physics/shaders/StepJointForces.wgsl b/physics/shaders/StepJointForces.wgsl index cc7aba72..08e3c52e 100644 --- a/physics/shaders/StepJointForces.wgsl +++ b/physics/shaders/StepJointForces.wgsl @@ -89,20 +89,21 @@ fn BodyCom(idx: i32) -> vec3 { alias ContactVars = i32; //enums:enum const ContactA: ContactVars = 0; const ContactB: ContactVars = 1; -const ContactAPointX: ContactVars = 2; -const ContactAPointY: ContactVars = 3; -const ContactAPointZ: ContactVars = 4; -const ContactBPointX: ContactVars = 5; -const ContactBPointY: ContactVars = 6; -const ContactBPointZ: ContactVars = 7; -const ContactADepth: ContactVars = 8; -const ContactBDepth: ContactVars = 9; -const ContactNormX: ContactVars = 10; -const ContactNormY: ContactVars = 11; -const ContactNormZ: ContactVars = 12; -const ContactForceX: ContactVars = 13; -const ContactForceY: ContactVars = 14; -const ContactForceZ: ContactVars = 15; +const ContactPointIdx: ContactVars = 2; +const ContactAPointX: ContactVars = 3; +const ContactAPointY: ContactVars = 4; +const ContactAPointZ: ContactVars = 5; +const ContactBPointX: ContactVars = 6; +const ContactBPointY: ContactVars = 7; +const ContactBPointZ: ContactVars = 8; +const ContactADepth: ContactVars = 9; +const ContactBDepth: ContactVars = 10; +const ContactNormX: ContactVars = 11; +const ContactNormY: ContactVars = 12; +const ContactNormZ: ContactVars = 13; +const ContactForceX: ContactVars = 14; +const ContactForceY: ContactVars = 15; +const ContactForceZ: ContactVars = 16; //////// import: "control.go" alias JointControlVars = i32; //enums:enum @@ -159,7 +160,7 @@ fn DynamicQuat(idx: i32,cni: i32) -> vec4 { //////// import: "enumgen.go" const BodyVarsN: BodyVars = 40; -const ContactVarsN: ContactVars = 16; +const ContactVarsN: ContactVars = 17; const JointControlVarsN: JointControlVars = 3; const DynamicVarsN: DynamicVars = 32; const GPUVarsN: GPUVars = 10; @@ -305,6 +306,7 @@ struct PhysParams { ContactWeighting: i32, Restitution: i32, ContactMargin: f32, + ContactsMax: i32, Cur: i32, Next: i32, BodiesN: i32, @@ -313,6 +315,9 @@ struct PhysParams { JointDoFsN: i32, BodyJointsMax: i32, BodyCollidePairsN: i32, + pad: i32, + pad1: i32, + pad2: i32, Gravity: vec4, } diff --git a/physics/shaders/StepSolveJoints.wgsl b/physics/shaders/StepSolveJoints.wgsl index 94449356..f112c05a 100644 --- a/physics/shaders/StepSolveJoints.wgsl +++ b/physics/shaders/StepSolveJoints.wgsl @@ -94,20 +94,21 @@ fn BodyInvInertia(idx: i32) -> mat3x3f { alias ContactVars = i32; //enums:enum const ContactA: ContactVars = 0; const ContactB: ContactVars = 1; -const ContactAPointX: ContactVars = 2; -const ContactAPointY: ContactVars = 3; -const ContactAPointZ: ContactVars = 4; -const ContactBPointX: ContactVars = 5; -const ContactBPointY: ContactVars = 6; -const ContactBPointZ: ContactVars = 7; -const ContactADepth: ContactVars = 8; -const ContactBDepth: ContactVars = 9; -const ContactNormX: ContactVars = 10; -const ContactNormY: ContactVars = 11; -const ContactNormZ: ContactVars = 12; -const ContactForceX: ContactVars = 13; -const ContactForceY: ContactVars = 14; -const ContactForceZ: ContactVars = 15; +const ContactPointIdx: ContactVars = 2; +const ContactAPointX: ContactVars = 3; +const ContactAPointY: ContactVars = 4; +const ContactAPointZ: ContactVars = 5; +const ContactBPointX: ContactVars = 6; +const ContactBPointY: ContactVars = 7; +const ContactBPointZ: ContactVars = 8; +const ContactADepth: ContactVars = 9; +const ContactBDepth: ContactVars = 10; +const ContactNormX: ContactVars = 11; +const ContactNormY: ContactVars = 12; +const ContactNormZ: ContactVars = 13; +const ContactForceX: ContactVars = 14; +const ContactForceY: ContactVars = 15; +const ContactForceZ: ContactVars = 16; //////// import: "control.go" alias JointControlVars = i32; //enums:enum @@ -170,7 +171,7 @@ fn DynamicAngDelta(idx: i32,cni: i32) -> vec3 { //////// import: "enumgen.go" const BodyVarsN: BodyVars = 40; -const ContactVarsN: ContactVars = 16; +const ContactVarsN: ContactVars = 17; const JointControlVarsN: JointControlVars = 3; const DynamicVarsN: DynamicVars = 32; const GPUVarsN: GPUVars = 10; @@ -325,6 +326,7 @@ struct PhysParams { ContactWeighting: i32, Restitution: i32, ContactMargin: f32, + ContactsMax: i32, Cur: i32, Next: i32, BodiesN: i32, @@ -333,6 +335,9 @@ struct PhysParams { JointDoFsN: i32, BodyJointsMax: i32, BodyCollidePairsN: i32, + pad: i32, + pad1: i32, + pad2: i32, Gravity: vec4, } diff --git a/physics/shapes.go b/physics/shapes.go index 484717df..722a9491 100644 --- a/physics/shapes.go +++ b/physics/shapes.go @@ -45,6 +45,8 @@ const ( Box ) +// newton: geometry/kernels.py: count_contact_points_for_pair + // ShapePairContacts returns the number of contact points possible // for given pair of shapes. a <= b ordering. returns from a to b, // ba is from b to a (mostly 0). diff --git a/physics/typegen.go b/physics/typegen.go index 9e59bc14..201609e4 100644 --- a/physics/typegen.go +++ b/physics/typegen.go @@ -24,7 +24,7 @@ var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.JointVars", var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.JointDoFVars", IDName: "joint-do-f-vars", Doc: "JointDoFVars are joint DoF state variables stored in tensor.Float32,\none for each DoF."}) -var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.PhysParams", IDName: "phys-params", Doc: "PhysParams are the physics parameters", Directives: []types.Directive{{Tool: "gosl", Directive: "start"}}, Fields: []types.Field{{Name: "Iterations", Doc: "Iterations is the number of iterations to perform."}, {Name: "Dt", Doc: "Dt is the integration stepsize."}, {Name: "SoftRelax", Doc: "SoftRelax is soft-body relaxation constant."}, {Name: "JointLinearRelax", Doc: "JointLinearRelax is joint linear relaxation constant."}, {Name: "JointAngularRelax", Doc: "JointAngularRelax is joint angular relaxation constant."}, {Name: "JointLinearComply", Doc: "JointLinearComply is joint linear compliance constant."}, {Name: "JointAngularComply", Doc: "JointAngularComply is joint angular compliance constant."}, {Name: "ContactRelax", Doc: "ContactRelax is rigid contact relaxation constant."}, {Name: "AngularDamping", Doc: "AngularDamping is damping of angular motion."}, {Name: "ContactWeighting", Doc: "Contact weighting"}, {Name: "Restitution", Doc: "Restitution"}, {Name: "ContactMargin", Doc: "Contact margin is the extra distance for broadphase collision\naround rigid bodies."}, {Name: "Cur", Doc: "Index for the current state (0 or 1, alternates with Next)."}, {Name: "Next", Doc: "Index for the next state (1 or 0, alternates with Cur)."}, {Name: "BodiesN", Doc: "BodiesN is number of rigid bodies."}, {Name: "DynamicsN", Doc: "DynamicsN is number of dynamics bodies."}, {Name: "JointsN", Doc: "JointsN is number of joints."}, {Name: "JointDoFsN", Doc: "JointDoFsN is number of joint DoFs."}, {Name: "BodyJointsMax", Doc: "BodyJointsMax is max number of joints per body + 1 for actual n."}, {Name: "BodyCollidePairsN", Doc: "BodyCollidePairsN is the total number of pre-compiled collision pairs\nto examine."}, {Name: "Gravity", Doc: "Gravity is the gravity acceleration function"}}}) +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.PhysParams", IDName: "phys-params", Doc: "PhysParams are the physics parameters", Directives: []types.Directive{{Tool: "gosl", Directive: "start"}}, Fields: []types.Field{{Name: "Iterations", Doc: "Iterations is the number of iterations to perform."}, {Name: "Dt", Doc: "Dt is the integration stepsize."}, {Name: "SoftRelax", Doc: "SoftRelax is soft-body relaxation constant."}, {Name: "JointLinearRelax", Doc: "JointLinearRelax is joint linear relaxation constant."}, {Name: "JointAngularRelax", Doc: "JointAngularRelax is joint angular relaxation constant."}, {Name: "JointLinearComply", Doc: "JointLinearComply is joint linear compliance constant."}, {Name: "JointAngularComply", Doc: "JointAngularComply is joint angular compliance constant."}, {Name: "ContactRelax", Doc: "ContactRelax is rigid contact relaxation constant."}, {Name: "AngularDamping", Doc: "AngularDamping is damping of angular motion."}, {Name: "ContactWeighting", Doc: "Contact weighting"}, {Name: "Restitution", Doc: "Restitution"}, {Name: "ContactMargin", Doc: "Contact margin is the extra distance for broadphase collision\naround rigid bodies."}, {Name: "ContactsMax", Doc: "Maximum number of contacts to process at any given point."}, {Name: "Cur", Doc: "Index for the current state (0 or 1, alternates with Next)."}, {Name: "Next", Doc: "Index for the next state (1 or 0, alternates with Cur)."}, {Name: "BodiesN", Doc: "BodiesN is number of rigid bodies."}, {Name: "DynamicsN", Doc: "DynamicsN is number of dynamics bodies."}, {Name: "JointsN", Doc: "JointsN is number of joints."}, {Name: "JointDoFsN", Doc: "JointDoFsN is number of joint DoFs."}, {Name: "BodyJointsMax", Doc: "BodyJointsMax is max number of joints per body + 1 for actual n."}, {Name: "BodyCollidePairsN", Doc: "BodyCollidePairsN is the total number of pre-compiled collision pairs\nto examine."}, {Name: "pad"}, {Name: "pad1"}, {Name: "pad2"}, {Name: "Gravity", Doc: "Gravity is the gravity acceleration function"}}}) var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.Shapes", IDName: "shapes", Doc: "Shapes are elemental shapes for rigid bodies.\nIn general, size dimensions are half values\n(e.g., radius, half-height, etc), which is natural for\ncenter-based body coordinates."}) diff --git a/physics/vars.go b/physics/vars.go index 5737f9a3..fb2f5ccd 100644 --- a/physics/vars.go +++ b/physics/vars.go @@ -6,7 +6,8 @@ package physics import "cogentcore.org/lab/tensor" -//go:generate gosl -keep -exclude=Update,Defaults,ShouldDisplay -max-buffer-size=2147483616 +// note: add -keep to inspect intermediate .go code +//go:generate gosl -exclude=Update,Defaults,ShouldDisplay -max-buffer-size=2147483616 //gosl:start @@ -66,7 +67,7 @@ var ( ContactCount *tensor.Int32 // Contacts are points of contact between bodies. - // [contact][ContactVarsN] + // [ContactsMax][ContactVarsN] //gosl:dims 2 Contacts *tensor.Float32 From a10df39bbfa36e71ce1bc1f95fa791a0d10cd601 Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Sat, 20 Dec 2025 16:35:48 +0100 Subject: [PATCH 29/97] physics: previous had major update to gosl docs. added more docs for locations of newton source code --- physics/step_body.goal | 4 ++++ physics/step_joint.goal | 4 ++++ 2 files changed, 8 insertions(+) diff --git a/physics/step_body.goal b/physics/step_body.goal index 1dfe7fe7..052899da 100644 --- a/physics/step_body.goal +++ b/physics/step_body.goal @@ -112,6 +112,8 @@ func DeltasFromJoints(i uint32) { //gosl:kernel SetDynamicAngDelta(di, params.Next, ta) } +// newton: solvers/solver.py: integrate_rigid_body + // StepIntegrateBodies applies forces to update pos and deltas func StepIntegrateBodies(i uint32) { //gosl:kernel params := GetParams(0) @@ -164,6 +166,8 @@ func StepIntegrateBodies(i uint32) { //gosl:kernel SetDynamicAngDelta(di, params.Next, w1) } +// newton: solvers/xpbd/kernels.py: apply_body_deltas + // StepBodyDeltas updates Next position with deltas. func StepBodyDeltas(i uint32) { //gosl:kernel params := GetParams(0) diff --git a/physics/step_joint.goal b/physics/step_joint.goal index 10e92543..7d556bff 100644 --- a/physics/step_joint.goal +++ b/physics/step_joint.goal @@ -25,6 +25,8 @@ import ( //gosl:start //gosl:import "cogentcore.org/lab/gosl/slmath" +// newton: solvers/xpbd/kernels.py: apply_joint_forces + // StepJointForces computes joint forces. func StepJointForces(i uint32) { //gosl:kernel params := GetParams(0) @@ -106,6 +108,8 @@ func StepJointForces(i uint32) { //gosl:kernel SetJointCTorque(ji, t.Add(slmath.Cross3(dC, f))) } +// newton: solvers/xpbd/kernels.py: solve_body_joints + // StepSolveJoints fixes joints after updating bodies. func StepSolveJoints(i uint32) { //gosl:kernel params := GetParams(0) From 30b7fc21e82cee34d7bcf923e4acbb09020ea03a Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Sat, 20 Dec 2025 19:01:51 +0100 Subject: [PATCH 30/97] physics: gosl doesn't align check structs that are not global vars, and allows such vars to be passed as pointers to other functions. Allows purely internal structs to be more effectively used, as long as not the first arg! --- docs/content/gosl.md | 18 +- gosl/alignsl/alignsl.go | 36 +- gosl/gotosl/gotosl.go | 6 + gosl/gotosl/nodes.go | 11 +- gosl/gotosl/translate.go | 18 +- physics/contact.go | 40 ++ physics/contact.goal | 41 +++ physics/gosl.go | 48 +++ physics/shaders/CollisionBroad.wgsl | 12 + physics/shaders/CollisionNarrow.wgsl | 442 +++++++++++++++++++++++ physics/shaders/DeltasFromJoints.wgsl | 12 + physics/shaders/DynamicsCurToNext.wgsl | 12 + physics/shaders/ForcesFromJoints.wgsl | 12 + physics/shaders/InitDynamics.wgsl | 12 + physics/shaders/StepBodyDeltas.wgsl | 12 + physics/shaders/StepIntegrateBodies.wgsl | 12 + physics/shaders/StepJointForces.wgsl | 12 + physics/shaders/StepSolveJoints.wgsl | 12 + physics/shapes.go | 66 +++- physics/step_body.go | 4 + physics/step_joint.go | 4 + physics/typegen.go | 2 + 22 files changed, 818 insertions(+), 26 deletions(-) create mode 100644 physics/shaders/CollisionNarrow.wgsl diff --git a/docs/content/gosl.md b/docs/content/gosl.md index 9f0ec270..c03fb54b 100644 --- a/docs/content/gosl.md +++ b/docs/content/gosl.md @@ -202,29 +202,31 @@ The flags are: `gosl` always operates on the current directory, looking for all files with `//gosl:` tags, and accumulating all the `import` files that they include, etc. -Any `struct` types encountered will be checked for 16-byte alignment of sub-types and overall sizes as an even multiple of 16 bytes (4 `float32` or `int32` values), which is the alignment used in WGSL and glsl shader languages, and the underlying GPU hardware presumably. Look for error messages on the output from the gosl run. This ensures that direct byte-wise copies of data between CPU and GPU will be successful. The fact that `gosl` operates directly on the original CPU-side Go code uniquely enables it to perform these alignment checks, which are otherwise a major source of difficult-to-diagnose bugs. +Any `struct` types encountered will be checked for 16-byte alignment of sub-types and overall sizes as an even multiple of 16 bytes (4 `float32` or `int32` values), which is the alignment used in WGSL and glsl shader languages, and the underlying GPU hardware presumably. Look for error messages on the output from the gosl run. This ensures that direct byte-wise copies of data between CPU and GPU will be successful. The fact that `gosl` operates directly on the original CPU-side Go code uniquely enables it to perform these alignment checks, which are otherwise a major source of difficult-to-diagnose bugs. ## Restrictions -In general shader code should be simple mathematical expressions and data types, with minimal control logic via `if`, `for` statements, and only using the subset of Go that is consistent with C. Here are specific restrictions: +In general shader code should be simple mathematical expressions and data types, with minimal control logic via `if`, `for` statements, and only using the subset of Go that is consistent with C. Here are specific restrictions: -* Can only use `float32`, `[u]int32` for basic types (`int` is converted to `int32` automatically), and `struct` types composed of these same types -- no other Go types (i.e., `map`, slices, `string`, etc) are compatible. There are strict alignment restrictions on 16 byte (e.g., 4 `float32`'s) intervals that are enforced via the `alignsl` sub-package. +* Can only use `float32`, `[u]int32` for basic types (`int` is converted to `int32` automatically), and `struct` types composed of these same types -- no other Go types (i.e., `map`, slices, `string`, etc) are compatible. There are strict alignment restrictions on 16 byte (e.g., 4 `float32`'s) intervals that are enforced via the `alignsl` sub-package. * WGSL does _not_ support 64 bit float or int. * Use `slbool.Bool` instead of `bool` -- it defines a Go-friendly interface based on a `int32` basic type. -* Alignment and padding of `struct` fields is key -- this is automatically checked by `gosl`. +* Alignment and padding of `struct` fields is key -- this is automatically checked by `gosl`. Any purely internally-used struct that is not used for variables passed between CPU and GPU is not checked and can use non-auto-aligned types, because we don't care about bit-wise identity between CPU and GPU. -* WGSL does not support enum types, but standard go `const` declarations will be converted. Use an `int32` or `uint32` data type. It will automatically deal with the simple incrementing `iota` values, but not more complex cases. Also, for bitflags, define explicitly, not using `bitflags` package, and use `0x01`, `0x02`, `0x04` etc instead of `1<<2` -- in theory the latter should be ok but in practice it complains. +* Cannot use a pointer type for the first argument to any function. This is because all methods with pointer method receivers are automatically converted into functions where the receiver is the first argument, which cannot be a pointer. It is too hard to distinguish all of these cases. + +* WGSL does not support enum types, but standard go `const` declarations will be converted. Use an `int32` or `uint32` data type. It will automatically deal with the simple incrementing `iota` values, but not more complex cases. Also, for bitflags, define explicitly, not using `bitflags` package, and use `0x01`, `0x02`, `0x04` etc instead of `1<<2` -- in theory the latter should be ok but in practice it complains. * Cannot use multiple return values, or multiple assignment of variables in a single `=` expression. * *Can* use multiple variable names with the same type (e.g., `min, max float32`) -- this will be properly converted to the more redundant form with the type repeated, for WGSL. -* `switch` `case` statements are _purely_ self-contained -- no `fallthrough` allowed! does support multiple items per `case` however. Every `switch` _must_ have a `default` case. +* `switch` `case` statements are _purely_ self-contained -- no `fallthrough` allowed! Does support multiple items per `case` however. Every `switch` _must_ have a `default` case. -* WGSL does specify that new variables are initialized to 0, like Go, but also somehow discourages that use-case. It is safer to initialize directly: +* WGSL does specify that new variables are initialized to 0, like Go, but also somehow discourages that use-case. It is safer to initialize directly: ```go val := float32(0) // guaranteed 0 value var val float32 // ok but generally avoid @@ -243,7 +245,7 @@ This automatically does the right thing on GPU while returning a pointer to the * [tour-of-wgsl](https://google.github.io/tour-of-wgsl/types/pointers/passing_pointers/) is a good reference to explain things more directly than the spec. * `ptr` provides a pointer arg -* `private` scope = within the shader code "module", i.e., one thread. +* `private` scope = within the shader code "module", i.e., one thread. * `function` = within the function, not outside it. * `workgroup` = shared across workgroup -- coudl be powerful (but slow!) -- need to learn more. diff --git a/gosl/alignsl/alignsl.go b/gosl/alignsl/alignsl.go index d774d144..9f2ecf1d 100644 --- a/gosl/alignsl/alignsl.go +++ b/gosl/alignsl/alignsl.go @@ -25,16 +25,18 @@ import ( // Context for given package run type Context struct { - Sizes types.Sizes // from package - Structs map[*types.Struct]string // structs that have been processed already -- value is name - Stack map[*types.Struct]string // structs to process in a second pass -- structs encountered during processing of other structs - Errs []string // accumulating list of error strings -- empty if all good + Sizes types.Sizes // from package + Structs map[*types.Struct]string // structs that have been processed already -- value is name + Stack map[*types.Struct]string // structs to process in a second pass -- structs encountered during processing of other structs + StructTypes map[string]bool // top level list of struct types to examine -- skip anything at a top-level that is not in this list. + Errs []string // accumulating list of error strings -- empty if all good } -func NewContext(sz types.Sizes) *Context { +func NewContext(sz types.Sizes, structTypes map[string]bool) *Context { cx := &Context{Sizes: sz} cx.Structs = make(map[*types.Struct]string) cx.Stack = make(map[*types.Struct]string) + cx.StructTypes = structTypes return cx } @@ -62,10 +64,20 @@ func TypeName(tp types.Type) string { return tp.String() } -// CheckStruct is the primary checker -- returns hasErr = true if there +// CheckStruct is the top-level checker -- returns hasErr = true if there // are any mis-aligned fields or total size of struct is not an -// even multiple of 16 bytes -- adds details to Errs +// even multiple of 16 bytes -- adds details to Errs. +// If struct is not on the cx.StructTypes list, it is skipped. func CheckStruct(cx *Context, st *types.Struct, stName string) bool { + if _, ok := cx.StructTypes[stName]; !ok { + return false + } + return CheckStructImpl(cx, st, stName) +} + +// CheckStructImpl can be used for CheckStack -- doesn't check for +// top-level StructTypes membership. +func CheckStructImpl(cx *Context, st *types.Struct, stName string) bool { if !cx.IsNewStruct(st) { return false } @@ -124,8 +136,11 @@ func CheckStruct(cx *Context, st *types.Struct, stName string) bool { // CheckPackage is main entry point for checking a package // returns error string if any errors found. -func CheckPackage(pkg *packages.Package) error { - cx := NewContext(pkg.TypesSizes) +// structTypes is a map of struct type names to check for alignment. +// any other struct types are purely internal not used for variables, so +// they don't need to be checked. +func CheckPackage(pkg *packages.Package, structTypes map[string]bool) error { + cx := NewContext(pkg.TypesSizes, structTypes) sc := pkg.Types.Scope() hasErr := CheckScope(cx, sc, 0) er := CheckStack(cx) @@ -136,6 +151,7 @@ WARNING: in struct type alignment checking: and fields are 32 bit types: [U]Int32, Float32 or other struct, and that fields that are other struct types are aligned at even 16 byte multiples. List of errors found follow below, by struct type name: + ` + strings.Join(cx.Errs, "\n") return errors.New(str) } @@ -151,7 +167,7 @@ func CheckStack(cx *Context) bool { st := cx.Stack cx.Stack = make(map[*types.Struct]string) // new stack for st, nm := range st { - er := CheckStruct(cx, st, nm) + er := CheckStructImpl(cx, st, nm) if er { hasErr = true } diff --git a/gosl/gotosl/gotosl.go b/gosl/gotosl/gotosl.go index 140bed50..37529e8a 100644 --- a/gosl/gotosl/gotosl.go +++ b/gosl/gotosl/gotosl.go @@ -232,6 +232,9 @@ type State struct { // GetFuncs is a map of GetVar, SetVar function names for global vars. GetFuncs map[string]*Var + // VarStructTypes is a map of struct type names to vars that use them. + VarStructTypes map[string]*Var + // SLImportFiles are all the extracted and translated WGSL files in shaders/imports, // which are copied into the generated shader kernel files. SLImportFiles []*File @@ -365,6 +368,7 @@ func (st *State) GetTempVar(vrnm string) *GetGlobalVar { // VarsAdded is called when a set of vars has been added; update relevant maps etc. func (st *State) VarsAdded() { st.GetFuncs = make(map[string]*Var) + st.VarStructTypes = make(map[string]*Var) for _, sy := range st.Systems { tensorIdx := 0 for gi, gp := range sy.Groups { @@ -386,6 +390,8 @@ func (st *State) VarsAdded() { continue } st.GetFuncs["Get"+vr.Name] = vr + jtyp := strings.TrimPrefix(vr.Type, "[]") + st.VarStructTypes[jtyp] = vr vn++ } } diff --git a/gosl/gotosl/nodes.go b/gosl/gotosl/nodes.go index ca6f0217..60457347 100644 --- a/gosl/gotosl/nodes.go +++ b/gosl/gotosl/nodes.go @@ -439,6 +439,7 @@ func (p *printer) parameters(fields *ast.FieldList, mode paramMode) { p.expr(atyp) if isPtr { p.print(">") + p.curPtrArgs = append(p.curPtrArgs, par.Names[0]) } } else { atyp, isPtr := p.ptrParamType(stripParensAlways(par.Type)) @@ -700,7 +701,15 @@ func (p *printer) ptrParamType(x ast.Expr) (ast.Expr, bool) { typ := p.getIdType(pt) if typ != nil { if _, ok := typ.Underlying().(*types.Struct); ok { - return u.X, false + tn := getLocalTypeName(typ) + pi := strings.Index(tn, ".") + if pi > 0 { + tn = tn[pi+1:] + } + // fmt.Printf("struct typ: %s\n", tn) + if _, ok := p.GoToSL.VarStructTypes[tn]; ok { + return u.X, false // no pointer, else ok + } } } p.print("ptr 0 { fmt.Printf("WARNING: %d shaders exceed maxStorageBuffersPerShaderStage min of 10\n", nOverBase) } + + //////// check types + + structTypes := make(map[string]bool) + for nm := range st.VarStructTypes { + structTypes[nm] = true + } + + serr := alignsl.CheckPackage(pkg, structTypes) + if serr != nil { + fmt.Println(serr) + } return nil } diff --git a/physics/contact.go b/physics/contact.go index 755de6b5..31f760a6 100644 --- a/physics/contact.go +++ b/physics/contact.go @@ -229,6 +229,46 @@ func AddContacts(biA, biB, ci, ncA, ncB int32) { } } +// newton: geometry/kernels.py: generate_handle_contact_pairs / handle_contact_pairs + +// CollisionNarrow performs narrow-phase collision on Contacts. +func CollisionNarrow(i uint32) { //gosl:kernel + params := GetParams(0) + ci := int32(i) + if ci >= params.ContactsMax { + return + } + biA := GetContactA(ci) + biB := GetContactB(ci) + cpi := GetContactPointIdx(ci) + + sA := GetBodyShape(biA) + sB := GetBodyShape(biB) + + gdA := NewGeomData(biA, params.Cur, sA) + gdB := NewGeomData(biB, params.Cur, sB) + + // could be per-shape + // margin = wp.max(shape_contact_margin[shape_a], shape_contact_margin[shape_b]) + margin := params.ContactMargin + thickness := gdA.Thickness + gdB.Thickness // todo + + dist := float32(1.0e6) + cni := params.Cur + var ptA, ptB, norm math32.Vector3 + switch gdA.Shape { + case Sphere: + switch gdB.Shape { + case Sphere: + dist = ColSphereSphere(cni, &gdA, &gdB, &ptA, &ptB, &norm) + case Box: + case Capsule: + default: + } + default: + } +} + // ClosestPointPlane projects the point onto the quad in // the xy plane (if size > 0.0, otherwise infinite. func ClosestPointPlane(sz, pt math32.Vector3) math32.Vector3 { diff --git a/physics/contact.goal b/physics/contact.goal index a24b3cd5..58637761 100644 --- a/physics/contact.goal +++ b/physics/contact.goal @@ -229,6 +229,47 @@ func AddContacts(biA, biB, ci, ncA, ncB int32) { } +// newton: geometry/kernels.py: generate_handle_contact_pairs / handle_contact_pairs + +// CollisionNarrow performs narrow-phase collision on Contacts. +func CollisionNarrow(i uint32) { //gosl:kernel + params := GetParams(0) + ci := int32(i) + if ci >= params.ContactsMax { + return + } + biA := GetContactA(ci) + biB := GetContactB(ci) + cpi := GetContactPointIdx(ci) + + sA := GetBodyShape(biA) + sB := GetBodyShape(biB) + + gdA := NewGeomData(biA, params.Cur, sA) + gdB := NewGeomData(biB, params.Cur, sB) + + // could be per-shape + // margin = wp.max(shape_contact_margin[shape_a], shape_contact_margin[shape_b]) + margin := params.ContactMargin + thickness := gdA.Thickness + gdB.Thickness // todo + + dist := float32(1.0e6) + cni := params.Cur + var ptA, ptB, norm math32.Vector3 + switch gdA.Shape { + case Sphere: + switch gdB.Shape { + case Sphere: + dist = ColSphereSphere(cni, &gdA, &gdB, &ptA, &ptB, &norm) + case Box: + case Capsule: + default: + } + default: + } +} + + // ClosestPointPlane projects the point onto the quad in // the xy plane (if size > 0.0, otherwise infinite. func ClosestPointPlane(sz, pt math32.Vector3) math32.Vector3 { diff --git a/physics/gosl.go b/physics/gosl.go index d0fba392..e7ca6827 100644 --- a/physics/gosl.go +++ b/physics/gosl.go @@ -120,6 +120,12 @@ func GPUInit() { pl.AddVarUsed(2, "Contacts") pl.AddVarUsed(2, "Dynamics") pl.AddVarUsed(0, "Params") + pl = gpu.NewComputePipelineShaderFS(shaders, "shaders/CollisionNarrow.wgsl", sy) + pl.AddVarUsed(0, "TensorStrides") + pl.AddVarUsed(1, "Bodies") + pl.AddVarUsed(2, "Contacts") + pl.AddVarUsed(2, "Dynamics") + pl.AddVarUsed(0, "Params") pl = gpu.NewComputePipelineShaderFS(shaders, "shaders/DeltasFromJoints.wgsl", sy) pl.AddVarUsed(0, "TensorStrides") pl.AddVarUsed(1, "BodyJoints") @@ -228,6 +234,48 @@ func RunOneCollisionBroad(n int, syncVars ...GPUVars) { RunCollisionBroadCPU(n) } } +// RunCollisionNarrow runs the CollisionNarrow kernel with given number of elements, +// on either the CPU or GPU depending on the UseGPU variable. +// Can call multiple Run* kernels in a row, which are then all launched +// in the same command submission on the GPU, which is by far the most efficient. +// MUST call RunDone (with optional vars to sync) after all Run calls. +// Alternatively, a single-shot RunOneCollisionNarrow call does Run and Done for a +// single run-and-sync case. +func RunCollisionNarrow(n int) { + if UseGPU { + RunCollisionNarrowGPU(n) + } else { + RunCollisionNarrowCPU(n) + } +} + +// RunCollisionNarrowGPU runs the CollisionNarrow kernel on the GPU. See [RunCollisionNarrow] for more info. +func RunCollisionNarrowGPU(n int) { + sy := GPUSystem + pl := sy.ComputePipelines["CollisionNarrow"] + ce, _ := sy.BeginComputePass() + pl.Dispatch1D(ce, n, 64) +} + +// RunCollisionNarrowCPU runs the CollisionNarrow kernel on the CPU. +func RunCollisionNarrowCPU(n int) { + gpu.VectorizeFunc(0, n, CollisionNarrow) +} + +// RunOneCollisionNarrow runs the CollisionNarrow kernel with given number of elements, +// on either the CPU or GPU depending on the UseGPU variable. +// This version then calls RunDone with the given variables to sync +// after the Run, for a single-shot Run-and-Done call. If multiple kernels +// can be run in sequence, it is much more efficient to do multiple Run* +// calls followed by a RunDone call. +func RunOneCollisionNarrow(n int, syncVars ...GPUVars) { + if UseGPU { + RunCollisionNarrowGPU(n) + RunDone(syncVars...) + } else { + RunCollisionNarrowCPU(n) + } +} // RunDeltasFromJoints runs the DeltasFromJoints kernel with given number of elements, // on either the CPU or GPU depending on the UseGPU variable. // Can call multiple Run* kernels in a row, which are then all launched diff --git a/physics/shaders/CollisionBroad.wgsl b/physics/shaders/CollisionBroad.wgsl index 16eef531..924b86de 100644 --- a/physics/shaders/CollisionBroad.wgsl +++ b/physics/shaders/CollisionBroad.wgsl @@ -433,6 +433,18 @@ fn ShapePairContacts(a: Shapes,b: Shapes, infPlane: bool, ba: ptr) } } } +struct GeomData { + BodyIdx: i32, + Shape: Shapes, + MinSize: f32, + Thickness: f32, + Radius: f32, + Size: vec3, + WtoBR: vec3, + WtoBQ: vec4, + BtoWR: vec3, + BtoWQ: vec4, +} //////// import: "slmath-matrix3.go" diff --git a/physics/shaders/CollisionNarrow.wgsl b/physics/shaders/CollisionNarrow.wgsl new file mode 100644 index 00000000..ed06f1fc --- /dev/null +++ b/physics/shaders/CollisionNarrow.wgsl @@ -0,0 +1,442 @@ +// Code generated by "gosl"; DO NOT EDIT +// kernel: CollisionNarrow + +// // Params are global parameters. +@group(0) @binding(0) +var TensorStrides: array; +@group(0) @binding(1) +var Params: array; +// // Bodies are the rigid body elements (dynamic and static), // specifying the constant, non-dynamic properties, // which is initial state for dynamics. // [body][BodyVarsN] +@group(1) @binding(0) +var Bodies: array; +// // Dynamics are the dynamic rigid body elements: these actually move. // Two alternating states are used: Params.Cur and Params.Next. // [dyn body][cur/next][DynamicVarsN] +@group(2) @binding(0) +var Dynamics: array; +@group(2) @binding(2) +var Contacts: array; +// // JointControls are dynamic joint control inputs, per joint DoF // (in correspondence with [JointDoFs]). This can be uploaded to the // GPU at every step. // [dof][JointControlVarsN] + +alias GPUVars = i32; + +@compute @workgroup_size(64, 1, 1) +fn main(@builtin(workgroup_id) wgid: vec3, @builtin(num_workgroups) nwg: vec3, @builtin(local_invocation_index) loci: u32) { + let idx = loci + (wgid.x + wgid.y * nwg.x + wgid.z * nwg.x * nwg.y) * 64; + CollisionNarrow(idx); +} + +fn Index2D(s0: u32, s1: u32, i0: u32, i1: u32) -> u32 { + return s0 * i0 + s1 * i1; +} + +fn Index3D(s0: u32, s1: u32, s2: u32, i0: u32, i1: u32, i2: u32) -> u32 { + return s0 * i0 + s1 * i1 + s2 * i2; +} + + +//////// import: "vars.go" + +//////// import: "body.go" +alias BodyVars = i32; //enums:enum +const BodyShape: BodyVars = 0; +const BodyDynamic: BodyVars = 1; +const BodyWorld: BodyVars = 2; +const BodyGroup: BodyVars = 3; +const BodySizeX: BodyVars = 4; +const BodySizeY: BodyVars = 5; +const BodySizeZ: BodyVars = 6; +const BodyMass: BodyVars = 7; +const BodyInvMass: BodyVars = 8; +const BodyBounce: BodyVars = 9; +const BodyFriction: BodyVars = 10; +const BodyPosX: BodyVars = 11; +const BodyPosY: BodyVars = 12; +const BodyPosZ: BodyVars = 13; +const BodyQuatX: BodyVars = 14; +const BodyQuatY: BodyVars = 15; +const BodyQuatZ: BodyVars = 16; +const BodyQuatW: BodyVars = 17; +const BodyComX: BodyVars = 18; +const BodyComY: BodyVars = 19; +const BodyComZ: BodyVars = 20; +const BodyInertiaXX: BodyVars = 21; +const BodyInertiaYX: BodyVars = 22; +const BodyInertiaZX: BodyVars = 23; +const BodyInertiaXY: BodyVars = 24; +const BodyInertiaYY: BodyVars = 25; +const BodyInertiaZY: BodyVars = 26; +const BodyInertiaXZ: BodyVars = 27; +const BodyInertiaYZ: BodyVars = 28; +const BodyInertiaZZ: BodyVars = 29; +const BodyInvInertiaXX: BodyVars = 30; +const BodyInvInertiaYX: BodyVars = 31; +const BodyInvInertiaZX: BodyVars = 32; +const BodyInvInertiaXY: BodyVars = 33; +const BodyInvInertiaYY: BodyVars = 34; +const BodyInvInertiaZY: BodyVars = 35; +const BodyInvInertiaXZ: BodyVars = 36; +const BodyInvInertiaYZ: BodyVars = 37; +const BodyInvInertiaZZ: BodyVars = 38; +const BodyRadius: BodyVars = 39; +fn GetBodyShape(idx: i32) -> Shapes { + return Shapes(bitcast(Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyShape))])); +} +fn GetBodyDynamic(idx: i32) -> i32 { + return i32(bitcast(Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyDynamic))])); +} +fn BodySize(idx: i32) -> vec3 { + return vec3(Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodySizeX))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodySizeY))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodySizeZ))]); +} +fn BodyPos(idx: i32) -> vec3 { + return vec3(Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyPosX))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyPosY))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyPosZ))]); +} +fn BodyQuat(idx: i32) -> vec4 { + return vec4(Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyQuatX))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyQuatY))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyQuatZ))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyQuatW))]); +} +fn BodyDynamicPos(idx: i32,cni: i32) -> vec3 { + var didx = GetBodyDynamic(idx); + if (didx < 0) { + return BodyPos(idx); + }return DynamicPos(didx, cni); +} +fn BodyDynamicQuat(idx: i32,cni: i32) -> vec4 { + var didx = GetBodyDynamic(idx); + if (didx < 0) { + return BodyQuat(idx); + }return DynamicQuat(didx, cni); +} + +//////// import: "contact.go" +alias ContactVars = i32; //enums:enum +const ContactA: ContactVars = 0; +const ContactB: ContactVars = 1; +const ContactPointIdx: ContactVars = 2; +const ContactAPointX: ContactVars = 3; +const ContactAPointY: ContactVars = 4; +const ContactAPointZ: ContactVars = 5; +const ContactBPointX: ContactVars = 6; +const ContactBPointY: ContactVars = 7; +const ContactBPointZ: ContactVars = 8; +const ContactADepth: ContactVars = 9; +const ContactBDepth: ContactVars = 10; +const ContactNormX: ContactVars = 11; +const ContactNormY: ContactVars = 12; +const ContactNormZ: ContactVars = 13; +const ContactForceX: ContactVars = 14; +const ContactForceY: ContactVars = 15; +const ContactForceZ: ContactVars = 16; +fn GetContactA(idx: i32) -> i32 { + return i32(bitcast(Contacts[Index2D(TensorStrides[70], TensorStrides[71], u32(idx), u32(ContactA))])); +} +fn GetContactB(idx: i32) -> i32 { + return i32(bitcast(Contacts[Index2D(TensorStrides[70], TensorStrides[71], u32(idx), u32(ContactB))])); +} +fn GetContactPointIdx(idx: i32) -> i32 { + return i32(bitcast(Contacts[Index2D(TensorStrides[70], TensorStrides[71], u32(idx), u32(ContactPointIdx))])); +} +fn CollisionNarrow(i: u32) { //gosl:kernel + let params = Params[0]; + var ci = i32(i); + if (ci >= params.ContactsMax) { + return; + } + var biA = GetContactA(ci); + var biB = GetContactB(ci); + var cpi = GetContactPointIdx(ci); + var sA = GetBodyShape(biA); + var sB = GetBodyShape(biB); + var gdA = NewGeomData(biA, params.Cur, sA); + var gdB = NewGeomData(biB, params.Cur, sB); + var margin = params.ContactMargin; + var thickness = gdA.Thickness + gdB.Thickness; // todo + var dist = f32(1.0e6); + var cni = params.Cur; + var ptA: vec3; + var ptB: vec3; + var norm: vec3; + switch (gdA.Shape) { + case Sphere: { + switch (gdB.Shape) { + case Sphere: { + dist = ColSphereSphere(cni, &gdA, &gdB, &ptA, &ptB, &norm); + } + case Box: { + } + case Capsule: { + } + default: { + } + } + } + default: { + } + } +} + +//////// import: "control.go" +alias JointControlVars = i32; //enums:enum +const JointControlForce: JointControlVars = 0; +const JointTargetPos: JointControlVars = 1; +const JointTargetVel: JointControlVars = 2; + +//////// import: "dynamics.go" +alias DynamicVars = i32; //enums:enum +const DynBody: DynamicVars = 0; +const DynPosX: DynamicVars = 1; +const DynPosY: DynamicVars = 2; +const DynPosZ: DynamicVars = 3; +const DynQuatX: DynamicVars = 4; +const DynQuatY: DynamicVars = 5; +const DynQuatZ: DynamicVars = 6; +const DynQuatW: DynamicVars = 7; +const DynVelX: DynamicVars = 8; +const DynVelY: DynamicVars = 9; +const DynVelZ: DynamicVars = 10; +const DynAngVelX: DynamicVars = 11; +const DynAngVelY: DynamicVars = 12; +const DynAngVelZ: DynamicVars = 13; +const DynAccX: DynamicVars = 14; +const DynAccY: DynamicVars = 15; +const DynAccZ: DynamicVars = 16; +const DynAngAccX: DynamicVars = 17; +const DynAngAccY: DynamicVars = 18; +const DynAngAccZ: DynamicVars = 19; +const DynForceX: DynamicVars = 20; +const DynForceY: DynamicVars = 21; +const DynForceZ: DynamicVars = 22; +const DynTorqueX: DynamicVars = 23; +const DynTorqueY: DynamicVars = 24; +const DynTorqueZ: DynamicVars = 25; +const DynDeltaX: DynamicVars = 26; +const DynDeltaY: DynamicVars = 27; +const DynDeltaZ: DynamicVars = 28; +const DynAngDeltaX: DynamicVars = 29; +const DynAngDeltaY: DynamicVars = 30; +const DynAngDeltaZ: DynamicVars = 31; +fn DynamicPos(idx: i32,cni: i32) -> vec3 { + return vec3(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosZ))]); +} +fn DynamicQuat(idx: i32,cni: i32) -> vec4 { + return vec4(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatZ))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatW))]); +} + +//////// import: "enumgen.go" +const BodyVarsN: BodyVars = 40; +const ContactVarsN: ContactVars = 17; +const JointControlVarsN: JointControlVars = 3; +const DynamicVarsN: DynamicVars = 32; +const GPUVarsN: GPUVars = 10; +const JointTypesN: JointTypes = 7; +const JointVarsN: JointVars = 50; +const JointDoFVarsN: JointDoFVars = 7; +const ShapesN: Shapes = 5; + +//////// import: "joint.go" +const JointLimitUnlimited = 1e10; +alias JointTypes = i32; //enums:enum +const Prismatic: JointTypes = 0; +const Revolute: JointTypes = 1; +const Ball: JointTypes = 2; +const Fixed: JointTypes = 3; +const Free: JointTypes = 4; +const Distance: JointTypes = 5; +const D6: JointTypes = 6; +alias JointVars = i32; //enums:enum +const JointType: JointVars = 0; +const JointEnabled: JointVars = 1; +const JointParent: JointVars = 2; +const JointChild: JointVars = 3; +const JointPPosX: JointVars = 4; +const JointPPosY: JointVars = 5; +const JointPPosZ: JointVars = 6; +const JointPQuatX: JointVars = 7; +const JointPQuatY: JointVars = 8; +const JointPQuatZ: JointVars = 9; +const JointPQuatW: JointVars = 10; +const JointCPosX: JointVars = 11; +const JointCPosY: JointVars = 12; +const JointCPosZ: JointVars = 13; +const JointCQuatX: JointVars = 14; +const JointCQuatY: JointVars = 15; +const JointCQuatZ: JointVars = 16; +const JointCQuatW: JointVars = 17; +const JointLinearDoFN: JointVars = 18; +const JointAngularDoFN: JointVars = 19; +const JointDoF1: JointVars = 20; +const JointDoF2: JointVars = 21; +const JointDoF3: JointVars = 22; +const JointDoF4: JointVars = 23; +const JointDoF5: JointVars = 24; +const JointDoF6: JointVars = 25; +const JointPForceX: JointVars = 26; +const JointPForceY: JointVars = 27; +const JointPForceZ: JointVars = 28; +const JointPTorqueX: JointVars = 29; +const JointPTorqueY: JointVars = 30; +const JointPTorqueZ: JointVars = 31; +const JointCForceX: JointVars = 32; +const JointCForceY: JointVars = 33; +const JointCForceZ: JointVars = 34; +const JointCTorqueX: JointVars = 35; +const JointCTorqueY: JointVars = 36; +const JointCTorqueZ: JointVars = 37; +const JointPDeltaX: JointVars = 38; +const JointPDeltaY: JointVars = 39; +const JointPDeltaZ: JointVars = 40; +const JointPAngDeltaX: JointVars = 41; +const JointPAngDeltaY: JointVars = 42; +const JointPAngDeltaZ: JointVars = 43; +const JointCDeltaX: JointVars = 44; +const JointCDeltaY: JointVars = 45; +const JointCDeltaZ: JointVars = 46; +const JointCAngDeltaX: JointVars = 47; +const JointCAngDeltaY: JointVars = 48; +const JointCAngDeltaZ: JointVars = 49; +alias JointDoFVars = i32; //enums:enum +const JointAxisX: JointDoFVars = 0; +const JointAxisY: JointDoFVars = 1; +const JointAxisZ: JointDoFVars = 2; +const JointLimitLower: JointDoFVars = 3; +const JointLimitUpper: JointDoFVars = 4; +const JointStiff: JointDoFVars = 5; +const JointDamp: JointDoFVars = 6; + +//////// import: "params.go" +struct PhysParams { + Iterations: i32, + Dt: f32, + SoftRelax: f32, + JointLinearRelax: f32, + JointAngularRelax: f32, + JointLinearComply: f32, + JointAngularComply: f32, + ContactRelax: f32, + AngularDamping: f32, + ContactWeighting: i32, + Restitution: i32, + ContactMargin: f32, + ContactsMax: i32, + Cur: i32, + Next: i32, + BodiesN: i32, + DynamicsN: i32, + JointsN: i32, + JointDoFsN: i32, + BodyJointsMax: i32, + BodyCollidePairsN: i32, + pad: i32, + pad1: i32, + pad2: i32, + Gravity: vec4, +} + +//////// import: "shapes.go" +alias Shapes = i32; //enums:enum +const Plane: Shapes = 0; +const Sphere: Shapes = 1; +const Capsule: Shapes = 2; +const Cylinder: Shapes = 3; +const Box: Shapes = 4; +struct GeomData { + BodyIdx: i32, + Shape: Shapes, + MinSize: f32, + Thickness: f32, + Radius: f32, + Size: vec3, + WtoBR: vec3, + WtoBQ: vec4, + BtoWR: vec3, + BtoWQ: vec4, +} +fn NewGeomData(bi: i32,cni: i32, shp: Shapes) -> GeomData { + var gd: GeomData; + gd.BodyIdx = bi; + gd.Shape = shp; + gd.Size = BodySize(bi); + gd.MinSize = min(gd.Size.x, gd.Size.y); + gd.MinSize = min(gd.MinSize, gd.Size.z); + gd.WtoBR = BodyDynamicPos(bi, cni); + gd.WtoBQ = BodyDynamicQuat(bi, cni); + var bwR: vec3; + var bwQ: vec4; + SpatialTransformInverse(gd.WtoBR, gd.WtoBQ, &bwR, &bwQ); + gd.BtoWR = bwR; + gd.BtoWQ = bwQ; + gd.Radius = f32(0); + if (shp == Sphere || shp == Capsule) { // todo: cone is separate + gd.Radius = gd.Size.x; + }return gd; +} +fn ColSphereSphere(cni: i32, gdA: ptr, gdB: ptr, ptA: ptr>,ptB: ptr>,norm: ptr>) -> f32 { + *ptA = (*gdA).WtoBR; + *ptB = (*gdB).WtoBR; + var diff = (*ptA)-(*ptB); + *norm = Normal3(diff);return Dot3(diff, *norm); +} + +//////// import: "slmath-matrix3.go" + +//////// import: "slmath-quaternion.go" +fn QuatLength(q: vec4) -> f32 { + return sqrt(q.x*q.x + q.y*q.y + q.z*q.z + q.w*q.w); +} +fn QuatNormalize(q: vec4) -> vec4 { + var nq = q; + var l = QuatLength(q); + if (l == 0) { + nq.x = f32(0); + nq.y = f32(0); + nq.z = f32(0); + nq.w = f32(1); + } else { + l = 1 / l; + nq.x *= l; + nq.y *= l; + nq.z *= l; + nq.w *= l; + }return nq; +} +fn MulQuatVector(q: vec4, v: vec3) -> vec3 { + var xyz = vec3(q.x, q.y, q.z); + var t = MulScalar3(Cross3(xyz, v), f32(f32(2)));return v+(MulScalar3(t, q.w))+(Cross3(xyz, t)); +} +fn SpatialTransformInverse(p: vec3, q: vec4, oP: ptr>, oQ: ptr>) { + var qi = QuatInverse(q); + *oP = Negate3(MulQuatVector(qi, p)); + *oQ = qi; +} +fn QuatInverse(q: vec4) -> vec4 { + var nq = q; + nq.x *= f32(-1); + nq.y *= f32(-1); + nq.z *= f32(-1);return QuatNormalize(nq); +} + +//////// import: "slmath-vector3.go" +fn MulScalar3(v: vec3, s: f32) -> vec3 { + return vec3(v.x*s, v.y*s, v.z*s); +} +fn DivScalar3(v: vec3, s: f32) -> vec3 { + return vec3(v.x/s, v.y/s, v.z/s); +} +fn Negate3(v: vec3) -> vec3 { + return vec3(-v.x, -v.y, -v.z); +} +fn Length3(v: vec3) -> f32 { + return sqrt(v.x*v.x + v.y*v.y + v.z*v.z); +} +fn Dot3(v: vec3,o: vec3) -> f32 { + return v.x*o.x + v.y*o.y + v.z*o.z; +} +fn Normal3(v: vec3) -> vec3 { + return DivScalar3(v, Length3(v)); +} +fn Cross3(v: vec3,o: vec3) -> vec3 { + return vec3(v.y*o.z-v.z*o.y, v.z*o.x-v.x*o.z, v.x*o.y-v.y*o.x); +} + +//////// import: "step.go" + +//////// import: "step_body.go" + +//////// import: "step_joint.go" \ No newline at end of file diff --git a/physics/shaders/DeltasFromJoints.wgsl b/physics/shaders/DeltasFromJoints.wgsl index 7724d9d6..4b061d87 100644 --- a/physics/shaders/DeltasFromJoints.wgsl +++ b/physics/shaders/DeltasFromJoints.wgsl @@ -278,6 +278,18 @@ const Sphere: Shapes = 1; const Capsule: Shapes = 2; const Cylinder: Shapes = 3; const Box: Shapes = 4; +struct GeomData { + BodyIdx: i32, + Shape: Shapes, + MinSize: f32, + Thickness: f32, + Radius: f32, + Size: vec3, + WtoBR: vec3, + WtoBQ: vec4, + BtoWR: vec3, + BtoWQ: vec4, +} //////// import: "slmath-matrix3.go" diff --git a/physics/shaders/DynamicsCurToNext.wgsl b/physics/shaders/DynamicsCurToNext.wgsl index 5218b681..3e58c7d0 100644 --- a/physics/shaders/DynamicsCurToNext.wgsl +++ b/physics/shaders/DynamicsCurToNext.wgsl @@ -248,6 +248,18 @@ const Sphere: Shapes = 1; const Capsule: Shapes = 2; const Cylinder: Shapes = 3; const Box: Shapes = 4; +struct GeomData { + BodyIdx: i32, + Shape: Shapes, + MinSize: f32, + Thickness: f32, + Radius: f32, + Size: vec3, + WtoBR: vec3, + WtoBQ: vec4, + BtoWR: vec3, + BtoWQ: vec4, +} //////// import: "slmath-matrix3.go" diff --git a/physics/shaders/ForcesFromJoints.wgsl b/physics/shaders/ForcesFromJoints.wgsl index 3aea568e..23f6e212 100644 --- a/physics/shaders/ForcesFromJoints.wgsl +++ b/physics/shaders/ForcesFromJoints.wgsl @@ -278,6 +278,18 @@ const Sphere: Shapes = 1; const Capsule: Shapes = 2; const Cylinder: Shapes = 3; const Box: Shapes = 4; +struct GeomData { + BodyIdx: i32, + Shape: Shapes, + MinSize: f32, + Thickness: f32, + Radius: f32, + Size: vec3, + WtoBR: vec3, + WtoBQ: vec4, + BtoWR: vec3, + BtoWQ: vec4, +} //////// import: "slmath-matrix3.go" diff --git a/physics/shaders/InitDynamics.wgsl b/physics/shaders/InitDynamics.wgsl index fc23ff4c..f6e06c03 100644 --- a/physics/shaders/InitDynamics.wgsl +++ b/physics/shaders/InitDynamics.wgsl @@ -257,6 +257,18 @@ const Sphere: Shapes = 1; const Capsule: Shapes = 2; const Cylinder: Shapes = 3; const Box: Shapes = 4; +struct GeomData { + BodyIdx: i32, + Shape: Shapes, + MinSize: f32, + Thickness: f32, + Radius: f32, + Size: vec3, + WtoBR: vec3, + WtoBQ: vec4, + BtoWR: vec3, + BtoWQ: vec4, +} //////// import: "slmath-matrix3.go" diff --git a/physics/shaders/StepBodyDeltas.wgsl b/physics/shaders/StepBodyDeltas.wgsl index bab3e14c..8527d0da 100644 --- a/physics/shaders/StepBodyDeltas.wgsl +++ b/physics/shaders/StepBodyDeltas.wgsl @@ -303,6 +303,18 @@ const Sphere: Shapes = 1; const Capsule: Shapes = 2; const Cylinder: Shapes = 3; const Box: Shapes = 4; +struct GeomData { + BodyIdx: i32, + Shape: Shapes, + MinSize: f32, + Thickness: f32, + Radius: f32, + Size: vec3, + WtoBR: vec3, + WtoBQ: vec4, + BtoWR: vec3, + BtoWQ: vec4, +} //////// import: "slmath-matrix3.go" diff --git a/physics/shaders/StepIntegrateBodies.wgsl b/physics/shaders/StepIntegrateBodies.wgsl index 54b6e0ec..5ad07099 100644 --- a/physics/shaders/StepIntegrateBodies.wgsl +++ b/physics/shaders/StepIntegrateBodies.wgsl @@ -309,6 +309,18 @@ const Sphere: Shapes = 1; const Capsule: Shapes = 2; const Cylinder: Shapes = 3; const Box: Shapes = 4; +struct GeomData { + BodyIdx: i32, + Shape: Shapes, + MinSize: f32, + Thickness: f32, + Radius: f32, + Size: vec3, + WtoBR: vec3, + WtoBQ: vec4, + BtoWR: vec3, + BtoWQ: vec4, +} //////// import: "slmath-matrix3.go" diff --git a/physics/shaders/StepJointForces.wgsl b/physics/shaders/StepJointForces.wgsl index 08e3c52e..fcd73673 100644 --- a/physics/shaders/StepJointForces.wgsl +++ b/physics/shaders/StepJointForces.wgsl @@ -328,6 +328,18 @@ const Sphere: Shapes = 1; const Capsule: Shapes = 2; const Cylinder: Shapes = 3; const Box: Shapes = 4; +struct GeomData { + BodyIdx: i32, + Shape: Shapes, + MinSize: f32, + Thickness: f32, + Radius: f32, + Size: vec3, + WtoBR: vec3, + WtoBQ: vec4, + BtoWR: vec3, + BtoWQ: vec4, +} //////// import: "slmath-matrix3.go" diff --git a/physics/shaders/StepSolveJoints.wgsl b/physics/shaders/StepSolveJoints.wgsl index f112c05a..6b5acca9 100644 --- a/physics/shaders/StepSolveJoints.wgsl +++ b/physics/shaders/StepSolveJoints.wgsl @@ -348,6 +348,18 @@ const Sphere: Shapes = 1; const Capsule: Shapes = 2; const Cylinder: Shapes = 3; const Box: Shapes = 4; +struct GeomData { + BodyIdx: i32, + Shape: Shapes, + MinSize: f32, + Thickness: f32, + Radius: f32, + Size: vec3, + WtoBR: vec3, + WtoBQ: vec4, + BtoWR: vec3, + BtoWQ: vec4, +} //////// import: "slmath-matrix3.go" diff --git a/physics/shapes.go b/physics/shapes.go index 722a9491..b9c870f6 100644 --- a/physics/shapes.go +++ b/physics/shapes.go @@ -4,7 +4,10 @@ package physics -import "cogentcore.org/core/math32" +import ( + "cogentcore.org/core/math32" + "cogentcore.org/lab/gosl/slmath" +) // see: newton/geometry for lots of helpful methods. @@ -98,6 +101,67 @@ func ShapePairContacts(a, b Shapes, infPlane bool, ba *int32) int32 { } } +// newton: geometry/kernels.py class GeoData + +// GeomData contains all geometric data for narrow-phase collision. +type GeomData struct { + BodyIdx int32 + + Shape Shapes + + // MinSize is the min of the Size dimensions. + MinSize float32 + + Thickness float32 + + // Radius is the effective radius for sphere-like elements (Sphere, Capsule, Cone) + Radius float32 + + Size math32.Vector3 + + // World-to-Body transform + // Position (R) (i.e., BodyPos) + WtoBR math32.Vector3 + // Quaternion (Q) (i.e., BodyQuat) + WtoBQ math32.Quat + + // Body-to-World transform (inverse) + // Position (R) + BtoWR math32.Vector3 + // Quaternion (Q) + BtoWQ math32.Quat +} + +func NewGeomData(bi, cni int32, shp Shapes) GeomData { + var gd GeomData + gd.BodyIdx = bi + gd.Shape = shp + gd.Size = BodySize(bi) + gd.MinSize = min(gd.Size.X, gd.Size.Y) + gd.MinSize = min(gd.MinSize, gd.Size.Z) + gd.WtoBR = BodyDynamicPos(bi, cni) + gd.WtoBQ = BodyDynamicQuat(bi, cni) + var bwR math32.Vector3 + var bwQ math32.Quat + slmath.SpatialTransformInverse(gd.WtoBR, gd.WtoBQ, &bwR, &bwQ) + gd.BtoWR = bwR + gd.BtoWQ = bwQ + gd.Radius = 0 + if shp == Sphere || shp == Capsule { // todo: cone is separate + gd.Radius = gd.Size.X + } + return gd +} + +// note: have to pass a non-pointer arg as first arg, due to gosl issue. +func ColSphereSphere(cni int32, gdA *GeomData, gdB *GeomData, ptA, ptB, norm *math32.Vector3) float32 { + *ptA = gdA.WtoBR + *ptB = gdB.WtoBR + diff := (*ptA).Sub(*ptB) + *norm = slmath.Normal3(diff) + return slmath.Dot3(diff, *norm) +} + //gosl:end // Radius returns the shape radius for given size. diff --git a/physics/step_body.go b/physics/step_body.go index 7faabd46..534c6bcd 100644 --- a/physics/step_body.go +++ b/physics/step_body.go @@ -114,6 +114,8 @@ func DeltasFromJoints(i uint32) { //gosl:kernel SetDynamicAngDelta(di, params.Next, ta) } +// newton: solvers/solver.py: integrate_rigid_body + // StepIntegrateBodies applies forces to update pos and deltas func StepIntegrateBodies(i uint32) { //gosl:kernel params := GetParams(0) @@ -166,6 +168,8 @@ func StepIntegrateBodies(i uint32) { //gosl:kernel SetDynamicAngDelta(di, params.Next, w1) } +// newton: solvers/xpbd/kernels.py: apply_body_deltas + // StepBodyDeltas updates Next position with deltas. func StepBodyDeltas(i uint32) { //gosl:kernel params := GetParams(0) diff --git a/physics/step_joint.go b/physics/step_joint.go index 2c59c2ef..15c9e640 100644 --- a/physics/step_joint.go +++ b/physics/step_joint.go @@ -27,6 +27,8 @@ import ( //gosl:start //gosl:import "cogentcore.org/lab/gosl/slmath" +// newton: solvers/xpbd/kernels.py: apply_joint_forces + // StepJointForces computes joint forces. func StepJointForces(i uint32) { //gosl:kernel params := GetParams(0) @@ -108,6 +110,8 @@ func StepJointForces(i uint32) { //gosl:kernel SetJointCTorque(ji, t.Add(slmath.Cross3(dC, f))) } +// newton: solvers/xpbd/kernels.py: solve_body_joints + // StepSolveJoints fixes joints after updating bodies. func StepSolveJoints(i uint32) { //gosl:kernel params := GetParams(0) diff --git a/physics/typegen.go b/physics/typegen.go index 201609e4..c4a08ff6 100644 --- a/physics/typegen.go +++ b/physics/typegen.go @@ -28,6 +28,8 @@ var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.PhysParams", var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.Shapes", IDName: "shapes", Doc: "Shapes are elemental shapes for rigid bodies.\nIn general, size dimensions are half values\n(e.g., radius, half-height, etc), which is natural for\ncenter-based body coordinates."}) +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.GeomData", IDName: "geom-data", Doc: "GeomData contains all geometric data for narrow-phase collision.", Fields: []types.Field{{Name: "BodyIdx"}, {Name: "Shape"}, {Name: "MinSize", Doc: "MinSize is the min of the Size dimensions."}, {Name: "Thickness"}, {Name: "Radius", Doc: "Radius is the effective radius for sphere-like elements (Sphere, Capsule, Cone)"}, {Name: "Size"}, {Name: "WtoBR", Doc: "World-to-Body transform\nPosition (R) (i.e., BodyPos)"}, {Name: "WtoBQ", Doc: "Quaternion (Q) (i.e., BodyQuat)"}, {Name: "BtoWR", Doc: "Body-to-World transform (inverse)\nPosition (R)"}, {Name: "BtoWQ", Doc: "Quaternion (Q)"}}}) + var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.State", IDName: "state", Doc: "State contains the basic physical state including position, orientation, velocity.\nThese are only the values that can be either relative or absolute -- other physical\nstate values such as Mass should go in Rigid.", Methods: []types.Method{{Name: "MoveOnAxis", Doc: "MoveOnAxis moves (translates) the specified distance on the specified local axis,\nrelative to the current rotation orientation.\nThe axis is normalized prior to aplying the distance factor.\nSets the LinVel to motion vector.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z", "dist"}}, {Name: "MoveOnAxisAbs", Doc: "MoveOnAxisAbs moves (translates) the specified distance on the specified local axis,\nin absolute X,Y,Z coordinates (does not apply the Quat rotation factor.\nThe axis is normalized prior to aplying the distance factor.\nSets the LinVel to motion vector.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z", "dist"}}, {Name: "SetEulerRotation", Doc: "SetEulerRotation sets the rotation in Euler angles (degrees).", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z"}}, {Name: "EulerRotation", Doc: "EulerRotation returns the current rotation in Euler angles (degrees).", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Returns: []string{"Vector3"}}, {Name: "SetAxisRotation", Doc: "SetAxisRotation sets rotation from local axis and angle in degrees.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z", "angle"}}, {Name: "RotateOnAxis", Doc: "RotateOnAxis rotates around the specified local axis the specified angle in degrees.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z", "angle"}}, {Name: "RotateEuler", Doc: "RotateEuler rotates by given Euler angles (in degrees) relative to existing rotation.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z"}}}, Fields: []types.Field{{Name: "Pos", Doc: "Pos is the position of center of mass of object."}, {Name: "Quat", Doc: "Quat is the rotation specified as a quaternion."}, {Name: "LinVel", Doc: "LinVel is the linear velocity."}, {Name: "AngVel", Doc: "AngVel is the angular velocity."}}}) var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.World", IDName: "world", Doc: "World contains and manages all of the physics elements.", Directives: []types.Directive{{Tool: "go", Directive: "generate", Args: []string{"core", "generate", "-add-types", "-gosl"}}}, Fields: []types.Field{{Name: "GPU", Doc: "GPU determines whether to use GPU (else CPU)."}, {Name: "Params", Doc: "Params are global parameters."}, {Name: "Bodies", Doc: "Bodies are the rigid body elements (dynamic and static),\nspecifying the constant, non-dynamic properties,\nwhich is initial state for dynamics.\n[body][BodyVarsN]"}, {Name: "Joints", Doc: "Joints is a list of permanent joints connecting bodies,\nwhich do not change (no dynamic variables).\n[joint][JointVarsN]"}, {Name: "JointDoFs", Doc: "JointDoFs is a list of joint DoF parameters, allocated per joint.\n[dof][JointDoFVars]"}, {Name: "BodyJoints", Doc: "BodyJoints is a list of joint indexes for each dynamic body, for aggregating.\n[dyn body][parent, child][Params.BodyJointsMax]"}, {Name: "BodyCollidePairs", Doc: "BodyCollidePairs are pairs of Body indexes that could potentially collide\nbased on precomputed collision logic, using World, Group, and Joint indexes.\n[BodyCollidePairsN][2]"}, {Name: "Dynamics", Doc: "Dynamics are the dynamic rigid body elements: these actually move.\nThe first set of variables are for initial values, and the second current.\n[body][cur/next][DynamicVarsN]"}, {Name: "ContactCount", Doc: "ContactCount has number of points of contact between bodies.\nparams.Cur is written to and params.Next is zeroed in\nnarrow-phase contacts processing, so it is ready for next time.\n[2]"}, {Name: "Contacts", Doc: "Contacts are points of contact between bodies. Max possible size\nis BodyCollidePairsN, but actual size on given run is updated\ninto ContactCount.\n[BodyCollidePairsN][ContactVarsN]"}, {Name: "JointControls", Doc: "JointControls are dynamic joint control inputs, per joint DoF\n(in correspondence with [JointDoFs]). This can be uploaded to the\nGPU at every step.\n[dof][JointControlVarsN]"}}}) From 52db49b48f702327c45a1d95d85e9d6f2bf9702b Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Sat, 20 Dec 2025 20:51:50 +0100 Subject: [PATCH 31/97] physics: update gosl test --- gosl/gotosl/testdata/Compute.golden | 5 +- physics/shaders/CollisionBroad.wgsl | 26 ++++---- physics/shaders/CollisionNarrow.wgsl | 16 ++--- physics/shaders/DeltasFromJoints.wgsl | 16 ++--- physics/shaders/DynamicsCurToNext.wgsl | 16 ++--- physics/shaders/ForcesFromJoints.wgsl | 16 ++--- physics/shaders/InitDynamics.wgsl | 16 ++--- physics/shaders/StepBodyDeltas.wgsl | 16 ++--- physics/shaders/StepIntegrateBodies.wgsl | 16 ++--- physics/shaders/StepJointForces.wgsl | 16 ++--- physics/shaders/StepSolveJoints.wgsl | 16 ++--- physics/shapecollide.go | 77 ++++++++++++++++++++++++ physics/shapes.go | 65 +------------------- physics/typegen.go | 4 +- 14 files changed, 181 insertions(+), 140 deletions(-) create mode 100644 physics/shapecollide.go diff --git a/gosl/gotosl/testdata/Compute.golden b/gosl/gotosl/testdata/Compute.golden index 1f2682b5..e0eebc6c 100644 --- a/gosl/gotosl/testdata/Compute.golden +++ b/gosl/gotosl/testdata/Compute.golden @@ -177,6 +177,7 @@ fn ParamStruct_IntegFromRaw(ps: ParamStruct, idx: i32) -> f32 { integ += newVal; Data[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(Integ))] = integ; Data[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(Exp))] = exp(-integ); + Data[idx*2] = 55.0; var a = ps.VXYf.x; var b = vec2(ps.VXYf.x,ps.VXYf.y); var c = b*(b+(b)); // converted to direct ops @@ -187,7 +188,9 @@ fn ParamStruct_IntegFromRaw(ps: ParamStruct, idx: i32) -> f32 { d = TransformPoint(op, oq, d); let ctx = Ctx[0]; ParamStruct_AnotherMeth(ps, ctx, idx, &a); - var bv = BigGet(Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(Integ))); + var bv = BigGet(Index2D(TensorStrides[10], TensorStrides[11], + u32(idx), u32(Integ))); + bv *= f32(2); BigSet(bv*2 + c.y + d.z, Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(Exp)));return Data[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(Exp))]; } diff --git a/physics/shaders/CollisionBroad.wgsl b/physics/shaders/CollisionBroad.wgsl index 924b86de..d01fc147 100644 --- a/physics/shaders/CollisionBroad.wgsl +++ b/physics/shaders/CollisionBroad.wgsl @@ -364,6 +364,20 @@ struct PhysParams { Gravity: vec4, } +//////// import: "shapecollide.go" +struct GeomData { + BodyIdx: i32, + Shape: Shapes, + MinSize: f32, + Thickness: f32, + Radius: f32, + Size: vec3, + WtoBR: vec3, + WtoBQ: vec4, + BtoWR: vec3, + BtoWQ: vec4, +} + //////// import: "shapes.go" alias Shapes = i32; //enums:enum const Plane: Shapes = 0; @@ -433,18 +447,6 @@ fn ShapePairContacts(a: Shapes,b: Shapes, infPlane: bool, ba: ptr) } } } -struct GeomData { - BodyIdx: i32, - Shape: Shapes, - MinSize: f32, - Thickness: f32, - Radius: f32, - Size: vec3, - WtoBR: vec3, - WtoBQ: vec4, - BtoWR: vec3, - BtoWQ: vec4, -} //////// import: "slmath-matrix3.go" diff --git a/physics/shaders/CollisionNarrow.wgsl b/physics/shaders/CollisionNarrow.wgsl index ed06f1fc..ce48d930 100644 --- a/physics/shaders/CollisionNarrow.wgsl +++ b/physics/shaders/CollisionNarrow.wgsl @@ -329,13 +329,7 @@ struct PhysParams { Gravity: vec4, } -//////// import: "shapes.go" -alias Shapes = i32; //enums:enum -const Plane: Shapes = 0; -const Sphere: Shapes = 1; -const Capsule: Shapes = 2; -const Cylinder: Shapes = 3; -const Box: Shapes = 4; +//////// import: "shapecollide.go" struct GeomData { BodyIdx: i32, Shape: Shapes, @@ -374,6 +368,14 @@ fn ColSphereSphere(cni: i32, gdA: ptr, gdB: ptr, } -//////// import: "shapes.go" -alias Shapes = i32; //enums:enum -const Plane: Shapes = 0; -const Sphere: Shapes = 1; -const Capsule: Shapes = 2; -const Cylinder: Shapes = 3; -const Box: Shapes = 4; +//////// import: "shapecollide.go" struct GeomData { BodyIdx: i32, Shape: Shapes, @@ -291,6 +285,14 @@ struct GeomData { BtoWQ: vec4, } +//////// import: "shapes.go" +alias Shapes = i32; //enums:enum +const Plane: Shapes = 0; +const Sphere: Shapes = 1; +const Capsule: Shapes = 2; +const Cylinder: Shapes = 3; +const Box: Shapes = 4; + //////// import: "slmath-matrix3.go" //////// import: "slmath-quaternion.go" diff --git a/physics/shaders/DynamicsCurToNext.wgsl b/physics/shaders/DynamicsCurToNext.wgsl index 3e58c7d0..b7a5b89a 100644 --- a/physics/shaders/DynamicsCurToNext.wgsl +++ b/physics/shaders/DynamicsCurToNext.wgsl @@ -241,13 +241,7 @@ struct PhysParams { Gravity: vec4, } -//////// import: "shapes.go" -alias Shapes = i32; //enums:enum -const Plane: Shapes = 0; -const Sphere: Shapes = 1; -const Capsule: Shapes = 2; -const Cylinder: Shapes = 3; -const Box: Shapes = 4; +//////// import: "shapecollide.go" struct GeomData { BodyIdx: i32, Shape: Shapes, @@ -261,6 +255,14 @@ struct GeomData { BtoWQ: vec4, } +//////// import: "shapes.go" +alias Shapes = i32; //enums:enum +const Plane: Shapes = 0; +const Sphere: Shapes = 1; +const Capsule: Shapes = 2; +const Cylinder: Shapes = 3; +const Box: Shapes = 4; + //////// import: "slmath-matrix3.go" //////// import: "slmath-quaternion.go" diff --git a/physics/shaders/ForcesFromJoints.wgsl b/physics/shaders/ForcesFromJoints.wgsl index 23f6e212..af72b0d8 100644 --- a/physics/shaders/ForcesFromJoints.wgsl +++ b/physics/shaders/ForcesFromJoints.wgsl @@ -271,13 +271,7 @@ struct PhysParams { Gravity: vec4, } -//////// import: "shapes.go" -alias Shapes = i32; //enums:enum -const Plane: Shapes = 0; -const Sphere: Shapes = 1; -const Capsule: Shapes = 2; -const Cylinder: Shapes = 3; -const Box: Shapes = 4; +//////// import: "shapecollide.go" struct GeomData { BodyIdx: i32, Shape: Shapes, @@ -291,6 +285,14 @@ struct GeomData { BtoWQ: vec4, } +//////// import: "shapes.go" +alias Shapes = i32; //enums:enum +const Plane: Shapes = 0; +const Sphere: Shapes = 1; +const Capsule: Shapes = 2; +const Cylinder: Shapes = 3; +const Box: Shapes = 4; + //////// import: "slmath-matrix3.go" //////// import: "slmath-quaternion.go" diff --git a/physics/shaders/InitDynamics.wgsl b/physics/shaders/InitDynamics.wgsl index f6e06c03..57327838 100644 --- a/physics/shaders/InitDynamics.wgsl +++ b/physics/shaders/InitDynamics.wgsl @@ -250,13 +250,7 @@ struct PhysParams { Gravity: vec4, } -//////// import: "shapes.go" -alias Shapes = i32; //enums:enum -const Plane: Shapes = 0; -const Sphere: Shapes = 1; -const Capsule: Shapes = 2; -const Cylinder: Shapes = 3; -const Box: Shapes = 4; +//////// import: "shapecollide.go" struct GeomData { BodyIdx: i32, Shape: Shapes, @@ -270,6 +264,14 @@ struct GeomData { BtoWQ: vec4, } +//////// import: "shapes.go" +alias Shapes = i32; //enums:enum +const Plane: Shapes = 0; +const Sphere: Shapes = 1; +const Capsule: Shapes = 2; +const Cylinder: Shapes = 3; +const Box: Shapes = 4; + //////// import: "slmath-matrix3.go" //////// import: "slmath-quaternion.go" diff --git a/physics/shaders/StepBodyDeltas.wgsl b/physics/shaders/StepBodyDeltas.wgsl index 8527d0da..86cea64b 100644 --- a/physics/shaders/StepBodyDeltas.wgsl +++ b/physics/shaders/StepBodyDeltas.wgsl @@ -296,13 +296,7 @@ struct PhysParams { Gravity: vec4, } -//////// import: "shapes.go" -alias Shapes = i32; //enums:enum -const Plane: Shapes = 0; -const Sphere: Shapes = 1; -const Capsule: Shapes = 2; -const Cylinder: Shapes = 3; -const Box: Shapes = 4; +//////// import: "shapecollide.go" struct GeomData { BodyIdx: i32, Shape: Shapes, @@ -316,6 +310,14 @@ struct GeomData { BtoWQ: vec4, } +//////// import: "shapes.go" +alias Shapes = i32; //enums:enum +const Plane: Shapes = 0; +const Sphere: Shapes = 1; +const Capsule: Shapes = 2; +const Cylinder: Shapes = 3; +const Box: Shapes = 4; + //////// import: "slmath-matrix3.go" //////// import: "slmath-quaternion.go" diff --git a/physics/shaders/StepIntegrateBodies.wgsl b/physics/shaders/StepIntegrateBodies.wgsl index 5ad07099..015ec080 100644 --- a/physics/shaders/StepIntegrateBodies.wgsl +++ b/physics/shaders/StepIntegrateBodies.wgsl @@ -302,13 +302,7 @@ struct PhysParams { Gravity: vec4, } -//////// import: "shapes.go" -alias Shapes = i32; //enums:enum -const Plane: Shapes = 0; -const Sphere: Shapes = 1; -const Capsule: Shapes = 2; -const Cylinder: Shapes = 3; -const Box: Shapes = 4; +//////// import: "shapecollide.go" struct GeomData { BodyIdx: i32, Shape: Shapes, @@ -322,6 +316,14 @@ struct GeomData { BtoWQ: vec4, } +//////// import: "shapes.go" +alias Shapes = i32; //enums:enum +const Plane: Shapes = 0; +const Sphere: Shapes = 1; +const Capsule: Shapes = 2; +const Cylinder: Shapes = 3; +const Box: Shapes = 4; + //////// import: "slmath-matrix3.go" //////// import: "slmath-quaternion.go" diff --git a/physics/shaders/StepJointForces.wgsl b/physics/shaders/StepJointForces.wgsl index fcd73673..a2fb72d1 100644 --- a/physics/shaders/StepJointForces.wgsl +++ b/physics/shaders/StepJointForces.wgsl @@ -321,13 +321,7 @@ struct PhysParams { Gravity: vec4, } -//////// import: "shapes.go" -alias Shapes = i32; //enums:enum -const Plane: Shapes = 0; -const Sphere: Shapes = 1; -const Capsule: Shapes = 2; -const Cylinder: Shapes = 3; -const Box: Shapes = 4; +//////// import: "shapecollide.go" struct GeomData { BodyIdx: i32, Shape: Shapes, @@ -341,6 +335,14 @@ struct GeomData { BtoWQ: vec4, } +//////// import: "shapes.go" +alias Shapes = i32; //enums:enum +const Plane: Shapes = 0; +const Sphere: Shapes = 1; +const Capsule: Shapes = 2; +const Cylinder: Shapes = 3; +const Box: Shapes = 4; + //////// import: "slmath-matrix3.go" //////// import: "slmath-quaternion.go" diff --git a/physics/shaders/StepSolveJoints.wgsl b/physics/shaders/StepSolveJoints.wgsl index 6b5acca9..3474abd8 100644 --- a/physics/shaders/StepSolveJoints.wgsl +++ b/physics/shaders/StepSolveJoints.wgsl @@ -341,13 +341,7 @@ struct PhysParams { Gravity: vec4, } -//////// import: "shapes.go" -alias Shapes = i32; //enums:enum -const Plane: Shapes = 0; -const Sphere: Shapes = 1; -const Capsule: Shapes = 2; -const Cylinder: Shapes = 3; -const Box: Shapes = 4; +//////// import: "shapecollide.go" struct GeomData { BodyIdx: i32, Shape: Shapes, @@ -361,6 +355,14 @@ struct GeomData { BtoWQ: vec4, } +//////// import: "shapes.go" +alias Shapes = i32; //enums:enum +const Plane: Shapes = 0; +const Sphere: Shapes = 1; +const Capsule: Shapes = 2; +const Cylinder: Shapes = 3; +const Box: Shapes = 4; + //////// import: "slmath-matrix3.go" //////// import: "slmath-quaternion.go" diff --git a/physics/shapecollide.go b/physics/shapecollide.go new file mode 100644 index 00000000..0dfa5636 --- /dev/null +++ b/physics/shapecollide.go @@ -0,0 +1,77 @@ +// Copyright (c) 2025, Cogent Core. All rights reserved. +// Use of this source code is governed by a BSD-style +// license that can be found in the LICENSE file. + +package physics + +import ( + "cogentcore.org/core/math32" + "cogentcore.org/lab/gosl/slmath" +) + +//gosl:start + +// newton: geometry/kernels.py class GeoData + +// GeomData contains all geometric data for narrow-phase collision. +type GeomData struct { + BodyIdx int32 + + Shape Shapes + + // MinSize is the min of the Size dimensions. + MinSize float32 + + Thickness float32 + + // Radius is the effective radius for sphere-like elements (Sphere, Capsule, Cone) + Radius float32 + + Size math32.Vector3 + + // World-to-Body transform + // Position (R) (i.e., BodyPos) + WtoBR math32.Vector3 + // Quaternion (Q) (i.e., BodyQuat) + WtoBQ math32.Quat + + // Body-to-World transform (inverse) + // Position (R) + BtoWR math32.Vector3 + // Quaternion (Q) + BtoWQ math32.Quat +} + +func NewGeomData(bi, cni int32, shp Shapes) GeomData { + var gd GeomData + gd.BodyIdx = bi + gd.Shape = shp + gd.Size = BodySize(bi) + gd.MinSize = min(gd.Size.X, gd.Size.Y) + gd.MinSize = min(gd.MinSize, gd.Size.Z) + gd.WtoBR = BodyDynamicPos(bi, cni) + gd.WtoBQ = BodyDynamicQuat(bi, cni) + var bwR math32.Vector3 + var bwQ math32.Quat + slmath.SpatialTransformInverse(gd.WtoBR, gd.WtoBQ, &bwR, &bwQ) + gd.BtoWR = bwR + gd.BtoWQ = bwQ + gd.Radius = 0 + if shp == Sphere || shp == Capsule { // todo: cone is separate + gd.Radius = gd.Size.X + } + return gd +} + +/////// Collision methods: +// note: have to pass a non-pointer arg as first arg, due to gosl issue. + +func ColSphereSphere(cni int32, gdA *GeomData, gdB *GeomData, ptA, ptB, norm *math32.Vector3) float32 { + *ptA = gdA.WtoBR + *ptB = gdB.WtoBR + diff := (*ptA).Sub(*ptB) + *norm = slmath.Normal3(diff) + return slmath.Dot3(diff, *norm) +} + +//gosl:end diff --git a/physics/shapes.go b/physics/shapes.go index b9c870f6..63c71a3e 100644 --- a/physics/shapes.go +++ b/physics/shapes.go @@ -6,7 +6,6 @@ package physics import ( "cogentcore.org/core/math32" - "cogentcore.org/lab/gosl/slmath" ) // see: newton/geometry for lots of helpful methods. @@ -42,6 +41,8 @@ const ( // Cylinder can not collide with a Box. Cylinder + // todo: add separate Cone + // Box is a 3D rectalinear shape. // The sizes are _half_ sizes along each dimension, // relative to the center. @@ -101,67 +102,6 @@ func ShapePairContacts(a, b Shapes, infPlane bool, ba *int32) int32 { } } -// newton: geometry/kernels.py class GeoData - -// GeomData contains all geometric data for narrow-phase collision. -type GeomData struct { - BodyIdx int32 - - Shape Shapes - - // MinSize is the min of the Size dimensions. - MinSize float32 - - Thickness float32 - - // Radius is the effective radius for sphere-like elements (Sphere, Capsule, Cone) - Radius float32 - - Size math32.Vector3 - - // World-to-Body transform - // Position (R) (i.e., BodyPos) - WtoBR math32.Vector3 - // Quaternion (Q) (i.e., BodyQuat) - WtoBQ math32.Quat - - // Body-to-World transform (inverse) - // Position (R) - BtoWR math32.Vector3 - // Quaternion (Q) - BtoWQ math32.Quat -} - -func NewGeomData(bi, cni int32, shp Shapes) GeomData { - var gd GeomData - gd.BodyIdx = bi - gd.Shape = shp - gd.Size = BodySize(bi) - gd.MinSize = min(gd.Size.X, gd.Size.Y) - gd.MinSize = min(gd.MinSize, gd.Size.Z) - gd.WtoBR = BodyDynamicPos(bi, cni) - gd.WtoBQ = BodyDynamicQuat(bi, cni) - var bwR math32.Vector3 - var bwQ math32.Quat - slmath.SpatialTransformInverse(gd.WtoBR, gd.WtoBQ, &bwR, &bwQ) - gd.BtoWR = bwR - gd.BtoWQ = bwQ - gd.Radius = 0 - if shp == Sphere || shp == Capsule { // todo: cone is separate - gd.Radius = gd.Size.X - } - return gd -} - -// note: have to pass a non-pointer arg as first arg, due to gosl issue. -func ColSphereSphere(cni int32, gdA *GeomData, gdB *GeomData, ptA, ptB, norm *math32.Vector3) float32 { - *ptA = gdA.WtoBR - *ptB = gdB.WtoBR - diff := (*ptA).Sub(*ptB) - *norm = slmath.Normal3(diff) - return slmath.Dot3(diff, *norm) -} - //gosl:end // Radius returns the shape radius for given size. @@ -204,6 +144,7 @@ func (sh Shapes) BBox(sz math32.Vector3) math32.Box3 { func (sh Shapes) Inertia(sz math32.Vector3, mass float32) math32.Matrix3 { var inertia math32.Matrix3 switch sh { + // todo: other shapes!! see below. case Sphere: r := sz.X // v := 4.0 / 3.0 * math32.Pi * r * r * r diff --git a/physics/typegen.go b/physics/typegen.go index c4a08ff6..149ebb39 100644 --- a/physics/typegen.go +++ b/physics/typegen.go @@ -26,9 +26,9 @@ var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.JointDoFVars var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.PhysParams", IDName: "phys-params", Doc: "PhysParams are the physics parameters", Directives: []types.Directive{{Tool: "gosl", Directive: "start"}}, Fields: []types.Field{{Name: "Iterations", Doc: "Iterations is the number of iterations to perform."}, {Name: "Dt", Doc: "Dt is the integration stepsize."}, {Name: "SoftRelax", Doc: "SoftRelax is soft-body relaxation constant."}, {Name: "JointLinearRelax", Doc: "JointLinearRelax is joint linear relaxation constant."}, {Name: "JointAngularRelax", Doc: "JointAngularRelax is joint angular relaxation constant."}, {Name: "JointLinearComply", Doc: "JointLinearComply is joint linear compliance constant."}, {Name: "JointAngularComply", Doc: "JointAngularComply is joint angular compliance constant."}, {Name: "ContactRelax", Doc: "ContactRelax is rigid contact relaxation constant."}, {Name: "AngularDamping", Doc: "AngularDamping is damping of angular motion."}, {Name: "ContactWeighting", Doc: "Contact weighting"}, {Name: "Restitution", Doc: "Restitution"}, {Name: "ContactMargin", Doc: "Contact margin is the extra distance for broadphase collision\naround rigid bodies."}, {Name: "ContactsMax", Doc: "Maximum number of contacts to process at any given point."}, {Name: "Cur", Doc: "Index for the current state (0 or 1, alternates with Next)."}, {Name: "Next", Doc: "Index for the next state (1 or 0, alternates with Cur)."}, {Name: "BodiesN", Doc: "BodiesN is number of rigid bodies."}, {Name: "DynamicsN", Doc: "DynamicsN is number of dynamics bodies."}, {Name: "JointsN", Doc: "JointsN is number of joints."}, {Name: "JointDoFsN", Doc: "JointDoFsN is number of joint DoFs."}, {Name: "BodyJointsMax", Doc: "BodyJointsMax is max number of joints per body + 1 for actual n."}, {Name: "BodyCollidePairsN", Doc: "BodyCollidePairsN is the total number of pre-compiled collision pairs\nto examine."}, {Name: "pad"}, {Name: "pad1"}, {Name: "pad2"}, {Name: "Gravity", Doc: "Gravity is the gravity acceleration function"}}}) -var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.Shapes", IDName: "shapes", Doc: "Shapes are elemental shapes for rigid bodies.\nIn general, size dimensions are half values\n(e.g., radius, half-height, etc), which is natural for\ncenter-based body coordinates."}) +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.GeomData", IDName: "geom-data", Doc: "GeomData contains all geometric data for narrow-phase collision.", Directives: []types.Directive{{Tool: "gosl", Directive: "start"}}, Fields: []types.Field{{Name: "BodyIdx"}, {Name: "Shape"}, {Name: "MinSize", Doc: "MinSize is the min of the Size dimensions."}, {Name: "Thickness"}, {Name: "Radius", Doc: "Radius is the effective radius for sphere-like elements (Sphere, Capsule, Cone)"}, {Name: "Size"}, {Name: "WtoBR", Doc: "World-to-Body transform\nPosition (R) (i.e., BodyPos)"}, {Name: "WtoBQ", Doc: "Quaternion (Q) (i.e., BodyQuat)"}, {Name: "BtoWR", Doc: "Body-to-World transform (inverse)\nPosition (R)"}, {Name: "BtoWQ", Doc: "Quaternion (Q)"}}}) -var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.GeomData", IDName: "geom-data", Doc: "GeomData contains all geometric data for narrow-phase collision.", Fields: []types.Field{{Name: "BodyIdx"}, {Name: "Shape"}, {Name: "MinSize", Doc: "MinSize is the min of the Size dimensions."}, {Name: "Thickness"}, {Name: "Radius", Doc: "Radius is the effective radius for sphere-like elements (Sphere, Capsule, Cone)"}, {Name: "Size"}, {Name: "WtoBR", Doc: "World-to-Body transform\nPosition (R) (i.e., BodyPos)"}, {Name: "WtoBQ", Doc: "Quaternion (Q) (i.e., BodyQuat)"}, {Name: "BtoWR", Doc: "Body-to-World transform (inverse)\nPosition (R)"}, {Name: "BtoWQ", Doc: "Quaternion (Q)"}}}) +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.Shapes", IDName: "shapes", Doc: "Shapes are elemental shapes for rigid bodies.\nIn general, size dimensions are half values\n(e.g., radius, half-height, etc), which is natural for\ncenter-based body coordinates."}) var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.State", IDName: "state", Doc: "State contains the basic physical state including position, orientation, velocity.\nThese are only the values that can be either relative or absolute -- other physical\nstate values such as Mass should go in Rigid.", Methods: []types.Method{{Name: "MoveOnAxis", Doc: "MoveOnAxis moves (translates) the specified distance on the specified local axis,\nrelative to the current rotation orientation.\nThe axis is normalized prior to aplying the distance factor.\nSets the LinVel to motion vector.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z", "dist"}}, {Name: "MoveOnAxisAbs", Doc: "MoveOnAxisAbs moves (translates) the specified distance on the specified local axis,\nin absolute X,Y,Z coordinates (does not apply the Quat rotation factor.\nThe axis is normalized prior to aplying the distance factor.\nSets the LinVel to motion vector.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z", "dist"}}, {Name: "SetEulerRotation", Doc: "SetEulerRotation sets the rotation in Euler angles (degrees).", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z"}}, {Name: "EulerRotation", Doc: "EulerRotation returns the current rotation in Euler angles (degrees).", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Returns: []string{"Vector3"}}, {Name: "SetAxisRotation", Doc: "SetAxisRotation sets rotation from local axis and angle in degrees.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z", "angle"}}, {Name: "RotateOnAxis", Doc: "RotateOnAxis rotates around the specified local axis the specified angle in degrees.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z", "angle"}}, {Name: "RotateEuler", Doc: "RotateEuler rotates by given Euler angles (in degrees) relative to existing rotation.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z"}}}, Fields: []types.Field{{Name: "Pos", Doc: "Pos is the position of center of mass of object."}, {Name: "Quat", Doc: "Quat is the rotation specified as a quaternion."}, {Name: "LinVel", Doc: "LinVel is the linear velocity."}, {Name: "AngVel", Doc: "AngVel is the angular velocity."}}}) From 4202cf8d244eb6f88aa3b59ddd7c7a65312590b5 Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Sun, 21 Dec 2025 00:37:17 +0100 Subject: [PATCH 32/97] physics: major progress on shape collide code, shapegeom, etc --- docs/content/physics.md | 8 +- gosl/slmath/quaternion.go | 8 +- gosl/slmath/vector2.go | 102 ++++++++ gosl/slmath/vector3.go | 18 +- physics/README.md | 4 + physics/body.go | 4 + physics/body.goal | 4 + physics/contact.go | 19 +- physics/contact.goal | 19 +- physics/enumgen.go | 20 +- physics/shaders/CollisionBroad.wgsl | 111 ++++----- physics/shaders/CollisionNarrow.wgsl | 122 +++++----- physics/shaders/DeltasFromJoints.wgsl | 86 +++---- physics/shaders/DynamicsCurToNext.wgsl | 86 +++---- physics/shaders/ForcesFromJoints.wgsl | 86 +++---- physics/shaders/InitDynamics.wgsl | 86 +++---- physics/shaders/StepBodyDeltas.wgsl | 93 ++++---- physics/shaders/StepIntegrateBodies.wgsl | 93 ++++---- physics/shaders/StepJointForces.wgsl | 99 ++++---- physics/shaders/StepSolveJoints.wgsl | 98 ++++---- physics/shapecollide.go | 79 +++++-- physics/shapecollide.goal | 124 ++++++++++ physics/shapegeom.go | 282 +++++++++++++++++++++++ physics/shapes.go | 22 +- physics/step_joint.go | 8 +- physics/step_joint.goal | 8 +- physics/typegen.go | 2 +- 27 files changed, 1134 insertions(+), 557 deletions(-) create mode 100644 gosl/slmath/vector2.go create mode 100644 physics/shapecollide.goal create mode 100644 physics/shapegeom.go diff --git a/docs/content/physics.md b/docs/content/physics.md index 168af003..b7df7321 100644 --- a/docs/content/physics.md +++ b/docs/content/physics.md @@ -28,7 +28,11 @@ There is a special constraint where the parent and child on a same joint do not ## Shapes -The elemental shapes are a `Box`, `Sphere`, `Cylinder` (Cone if one radius is 0), and `Capsule`: [[doc:physics.Shapes]]. The `Size` property on bodies is always the _half_ size, such as the radius or the half-height of a cylinder or capsule. This is used in `newton-physics` and makes more sense for center-based computations: physics operates on the center-of-mass of a body. Consistent with the overall coordinate system, the `Cylinder` and `Capsule` are oriented with `Y` as the height dimension, which is unfortunately inconsistent with the Z=up convention in `newton-physics`. +The elemental shapes are a `Plane`, `Sphere`, `Capsule`, `Cylinder`, `Cone`, and `Box`: [[doc:physics.Shapes]]. The `Size` property on bodies is always the _half_ size, such as the radius or the half-height of a cylinder or capsule. This is used in `newton-physics` and makes more sense for center-based computations: physics operates on the center-of-mass of a body. Consistent with the overall coordinate system, the `Cylinder` and `Capsule` are oriented with `Y` as the height dimension, which is unfortunately inconsistent with the Z=up convention in `newton-physics`. + +### Multi-shape bodies + +The newton-physics framework, and MuJoCo upon which it is based, support multiple shapes per body, which can then be integrated to produce an aggregate inertia. This adds an additional level of complexity and management overhead, which we are currently avoiding in favor of putting the shapes directly on the body, so each body has 1 and only 1 shape. This simplifies collision considerably as well. It would not be difficult to add a shape layer at some point in the future. The same goes for Mesh, SDF, and HeightField types. ## Joints @@ -56,7 +60,7 @@ Typically, bodies are created using the enhanced functions in the [[doc:physics/ ## Physics solver algorithms -This section provides a brief overview of different physics solver algorithms, and motivates why we're using XPBD (see [[@MacklinMullerChentanez16]] and [[@MullerMacklinChentanezEtAl20]] for full info). There are two main categories of mathematical problems that these engines solve: +This section provides a brief overview of different physics solver algorithms, and motivates why we're using XPBD (see [[@MacklinMullerChentanez16]] and [[@MullerMacklinChentanezEtAl20]] for full info). See [[@CollinsChandVanderkopEtAl21]] for a recent review of relevant software and approaches. There are two main categories of mathematical problems that these engines solve: * Impacts from contact / collisions among bodies. When two billiard balls hit each other, they rebound in an _elastic_ collision, for example. There are also forces of friction and graded levels of inelasticity in these dynamics. The primary problem here is that the instantaneous forces involved in these impacts can be huge (this is why objects tend to shatter when you drop them on a hard surface), because the momentum reverses within a very short period of time. Numerical integration techniques tend to perform poorly when dealing with such huge forces and resulting accelerations. diff --git a/gosl/slmath/quaternion.go b/gosl/slmath/quaternion.go index c368babe..68f978f1 100644 --- a/gosl/slmath/quaternion.go +++ b/gosl/slmath/quaternion.go @@ -36,16 +36,16 @@ func QuatNormalize(q math32.Quat) math32.Quat { // to the [math32.Vector3]. func MulQuatVector(q math32.Quat, v math32.Vector3) math32.Vector3 { xyz := math32.Vec3(q.X, q.Y, q.Z) - t := MulScalar3(Cross3(xyz, v), 2) - return v.Add(MulScalar3(t, q.W)).Add(Cross3(xyz, t)) + t := Cross3(xyz, v).MulScalar(2) + return v.Add(t.MulScalar(q.W)).Add(Cross3(xyz, t)) } // MulQuatVectorInverse applies the inverse of the rotation encoded // in the [math32.Quat] to the [math32.Vector3]. func MulQuatVectorInverse(q math32.Quat, v math32.Vector3) math32.Vector3 { xyz := math32.Vec3(q.X, q.Y, q.Z) - t := MulScalar3(Cross3(xyz, v), 2) - return v.Sub(MulScalar3(t, q.W)).Add(Cross3(xyz, t)) + t := Cross3(xyz, v).MulScalar(2) + return v.Sub(t.MulScalar(q.W)).Add(Cross3(xyz, t)) } // MulQuats returns multiplication of a by b quaternions. diff --git a/gosl/slmath/vector2.go b/gosl/slmath/vector2.go new file mode 100644 index 00000000..c17a3314 --- /dev/null +++ b/gosl/slmath/vector2.go @@ -0,0 +1,102 @@ +// Copyright (c) 2025, Cogent Core. All rights reserved. +// Use of this source code is governed by a BSD-style +// license that can be found in the LICENSE file. + +package slmath + +import "cogentcore.org/core/math32" + +//gosl:start + +// DivSafe2 divides v by o elementwise, only where o != 0 +func DivSafe2(v math32.Vector2, o math32.Vector2) math32.Vector2 { + nv := v + if o.X != 0 { + nv.X /= o.X + } + if o.Y != 0 { + nv.Y /= o.Y + } + return nv +} + +func Negate2(v math32.Vector2) math32.Vector2 { + return math32.Vec2(-v.X, -v.Y) +} + +// Length2 returns the length (magnitude) of this vector. +func Length2(v math32.Vector2) float32 { + return math32.Sqrt(v.X*v.X + v.Y*v.Y) +} + +// LengthSquared2 returns the length squared of this vector. +func LengthSquared2(v math32.Vector2) float32 { + return v.X*v.X + v.Y*v.Y +} + +func Dot2(v, o math32.Vector2) float32 { + return v.X*o.X + v.Y*o.Y +} + +// Max2 returns max of this vector components vs. other vector. +func Max2(v, o math32.Vector2) math32.Vector2 { + return math32.Vec2(max(v.X, o.X), max(v.Y, o.Y)) +} + +// Min2 returns min of this vector components vs. other vector. +func Min2(v, o math32.Vector2) math32.Vector2 { + return math32.Vec2(min(v.X, o.X), min(v.Y, o.Y)) +} + +// Abs2 returns abs of this vector components. +func Abs2(v math32.Vector2) math32.Vector2 { + return math32.Vec2(math32.Abs(v.X), math32.Abs(v.Y)) +} + +func Clamp2(v, min, max math32.Vector2) math32.Vector2 { + r := v + if r.X < min.X { + r.X = min.X + } else if r.X > max.X { + r.X = max.X + } + if r.Y < min.Y { + r.Y = min.Y + } else if r.Y > max.Y { + r.Y = max.Y + } + return r +} + +// Normal2 returns this vector divided by its length (its unit vector). +func Normal2(v math32.Vector2) math32.Vector2 { + return v.DivScalar(Length2(v)) +} + +// Cross2 returns the cross product of this vector with other. +func Cross2(v, o math32.Vector2) float32 { + return v.X*o.Y - v.Y*o.X +} + +func Dim2(v math32.Vector2, dim int32) float32 { + if dim == 0 { + return v.X + } + if dim == 1 { + return v.Y + } + return 0 +} + +func SetDim2(v math32.Vector2, dim int32, val float32) math32.Vector2 { + nv := v + if dim == 0 { + nv.X = val + } + if dim == 1 { + nv.Y = val + } + return nv +} + +//gosl:end diff --git a/gosl/slmath/vector3.go b/gosl/slmath/vector3.go index 2a23dbdc..888e316a 100644 --- a/gosl/slmath/vector3.go +++ b/gosl/slmath/vector3.go @@ -8,22 +8,6 @@ import "cogentcore.org/core/math32" //gosl:start -func AddScalar3(v math32.Vector3, s float32) math32.Vector3 { - return math32.Vec3(v.X+s, v.Y+s, v.Z+s) -} - -func SubScalar3(v math32.Vector3, s float32) math32.Vector3 { - return math32.Vec3(v.X-s, v.Y-s, v.Z-s) -} - -func MulScalar3(v math32.Vector3, s float32) math32.Vector3 { - return math32.Vec3(v.X*s, v.Y*s, v.Z*s) -} - -func DivScalar3(v math32.Vector3, s float32) math32.Vector3 { - return math32.Vec3(v.X/s, v.Y/s, v.Z/s) -} - // DivSafe3 divides v by o elementwise, only where o != 0 func DivSafe3(v math32.Vector3, o math32.Vector3) math32.Vector3 { nv := v @@ -94,7 +78,7 @@ func Clamp3(v, min, max math32.Vector3) math32.Vector3 { // Normal3 returns this vector divided by its length (its unit vector). func Normal3(v math32.Vector3) math32.Vector3 { - return DivScalar3(v, Length3(v)) + return v.DivScalar(Length3(v)) } // Cross3 returns the cross product of this vector with other. diff --git a/physics/README.md b/physics/README.md index 07a1c314..8f911d49 100644 --- a/physics/README.md +++ b/physics/README.md @@ -6,3 +6,7 @@ See [physics docs](https://cogentcore.org/lab/physics) for the main docs. The [world](world) visualization sub-package manages a `View` element that links to physics bodies and generates an [xyz](https://cogentcore.org/core/xyz) 3D scenegraph based on the physics bodies, and updates this visualization efficiently as the physics is updated. +## TODO + +* Muscles: https://mujoco.readthedocs.io/en/stable/modeling.html#muscles + diff --git a/physics/body.go b/physics/body.go index 597296c4..a18f6984 100644 --- a/physics/body.go +++ b/physics/body.go @@ -50,6 +50,10 @@ const ( BodySizeY BodySizeZ + // BodyThick is the thickness of the body, as a hollow shape. + // If 0, then it is a solid shape (default). + BodyThick + // physical properties // BodyMass is the mass of the object. diff --git a/physics/body.goal b/physics/body.goal index 4f77f9e3..fb0ecaf2 100644 --- a/physics/body.goal +++ b/physics/body.goal @@ -48,6 +48,10 @@ const ( BodySizeY BodySizeZ + // BodyThick is the thickness of the body, as a hollow shape. + // If 0, then it is a solid shape (default). + BodyThick + // physical properties // BodyMass is the mass of the object. diff --git a/physics/contact.go b/physics/contact.go index 31f760a6..bb65c72d 100644 --- a/physics/contact.go +++ b/physics/contact.go @@ -179,7 +179,7 @@ func CollisionBroad(i uint32) { //gosl:kernel infPlane = true } queryB := slmath.MulSpatialPoint(xwAR, xwAQ, xwBR) - closest := ClosestPointPlane(szA, queryB) + closest := ClosestPointPlane(szA.X, szA.Z, queryB) d := slmath.Length3(queryB.Sub(closest)) if d > rb+margin { return @@ -251,16 +251,15 @@ func CollisionNarrow(i uint32) { //gosl:kernel // could be per-shape // margin = wp.max(shape_contact_margin[shape_a], shape_contact_margin[shape_b]) margin := params.ContactMargin - thickness := gdA.Thickness + gdB.Thickness // todo + thick := gdA.Thick + gdB.Thick dist := float32(1.0e6) - cni := params.Cur var ptA, ptB, norm math32.Vector3 switch gdA.Shape { case Sphere: switch gdB.Shape { case Sphere: - dist = ColSphereSphere(cni, &gdA, &gdB, &ptA, &ptB, &norm) + dist = ColSphereSphere(cpi, &gdA, &gdB, &ptA, &ptB, &norm) case Box: case Capsule: default: @@ -269,18 +268,6 @@ func CollisionNarrow(i uint32) { //gosl:kernel } } -// ClosestPointPlane projects the point onto the quad in -// the xy plane (if size > 0.0, otherwise infinite. -func ClosestPointPlane(sz, pt math32.Vector3) math32.Vector3 { - cp := pt - if sz.X == 0.0 { - return cp - } - cp.X = math32.Clamp(pt.X, -sz.X, sz.X) - cp.Y = math32.Clamp(pt.Y, -sz.Y, sz.Y) - return cp -} - //gosl:end // IsChildDynamic returns true if dic is a direct child diff --git a/physics/contact.goal b/physics/contact.goal index 58637761..75f40b54 100644 --- a/physics/contact.goal +++ b/physics/contact.goal @@ -178,7 +178,7 @@ func CollisionBroad(i uint32) { //gosl:kernel infPlane = true } queryB := slmath.MulSpatialPoint(xwAR, xwAQ, xwBR) - closest := ClosestPointPlane(szA, queryB) + closest := ClosestPointPlane(szA.X, szA.Z, queryB) d := slmath.Length3(queryB.Sub(closest)) if d > rb+margin { return @@ -251,16 +251,15 @@ func CollisionNarrow(i uint32) { //gosl:kernel // could be per-shape // margin = wp.max(shape_contact_margin[shape_a], shape_contact_margin[shape_b]) margin := params.ContactMargin - thickness := gdA.Thickness + gdB.Thickness // todo + thick := gdA.Thick + gdB.Thick dist := float32(1.0e6) - cni := params.Cur var ptA, ptB, norm math32.Vector3 switch gdA.Shape { case Sphere: switch gdB.Shape { case Sphere: - dist = ColSphereSphere(cni, &gdA, &gdB, &ptA, &ptB, &norm) + dist = ColSphereSphere(cpi, &gdA, &gdB, &ptA, &ptB, &norm) case Box: case Capsule: default: @@ -270,18 +269,6 @@ func CollisionNarrow(i uint32) { //gosl:kernel } -// ClosestPointPlane projects the point onto the quad in -// the xy plane (if size > 0.0, otherwise infinite. -func ClosestPointPlane(sz, pt math32.Vector3) math32.Vector3 { - cp := pt - if sz.X == 0.0 { - return cp - } - cp.X = math32.Clamp(pt.X, -sz.X, sz.X) - cp.Y = math32.Clamp(pt.Y, -sz.Y, sz.Y) - return cp -} - //gosl:end // IsChildDynamic returns true if dic is a direct child diff --git a/physics/enumgen.go b/physics/enumgen.go index 9560b835..68078cbd 100644 --- a/physics/enumgen.go +++ b/physics/enumgen.go @@ -6,20 +6,20 @@ import ( "cogentcore.org/core/enums" ) -var _BodyVarsValues = []BodyVars{0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39} +var _BodyVarsValues = []BodyVars{0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40} // BodyVarsN is the highest valid value for type BodyVars, plus one. // //gosl:start -const BodyVarsN BodyVars = 40 +const BodyVarsN BodyVars = 41 //gosl:end -var _BodyVarsValueMap = map[string]BodyVars{`BodyShape`: 0, `BodyDynamic`: 1, `BodyWorld`: 2, `BodyGroup`: 3, `BodySizeX`: 4, `BodySizeY`: 5, `BodySizeZ`: 6, `BodyMass`: 7, `BodyInvMass`: 8, `BodyBounce`: 9, `BodyFriction`: 10, `BodyPosX`: 11, `BodyPosY`: 12, `BodyPosZ`: 13, `BodyQuatX`: 14, `BodyQuatY`: 15, `BodyQuatZ`: 16, `BodyQuatW`: 17, `BodyComX`: 18, `BodyComY`: 19, `BodyComZ`: 20, `BodyInertiaXX`: 21, `BodyInertiaYX`: 22, `BodyInertiaZX`: 23, `BodyInertiaXY`: 24, `BodyInertiaYY`: 25, `BodyInertiaZY`: 26, `BodyInertiaXZ`: 27, `BodyInertiaYZ`: 28, `BodyInertiaZZ`: 29, `BodyInvInertiaXX`: 30, `BodyInvInertiaYX`: 31, `BodyInvInertiaZX`: 32, `BodyInvInertiaXY`: 33, `BodyInvInertiaYY`: 34, `BodyInvInertiaZY`: 35, `BodyInvInertiaXZ`: 36, `BodyInvInertiaYZ`: 37, `BodyInvInertiaZZ`: 38, `BodyRadius`: 39} +var _BodyVarsValueMap = map[string]BodyVars{`BodyShape`: 0, `BodyDynamic`: 1, `BodyWorld`: 2, `BodyGroup`: 3, `BodySizeX`: 4, `BodySizeY`: 5, `BodySizeZ`: 6, `BodyThick`: 7, `BodyMass`: 8, `BodyInvMass`: 9, `BodyBounce`: 10, `BodyFriction`: 11, `BodyPosX`: 12, `BodyPosY`: 13, `BodyPosZ`: 14, `BodyQuatX`: 15, `BodyQuatY`: 16, `BodyQuatZ`: 17, `BodyQuatW`: 18, `BodyComX`: 19, `BodyComY`: 20, `BodyComZ`: 21, `BodyInertiaXX`: 22, `BodyInertiaYX`: 23, `BodyInertiaZX`: 24, `BodyInertiaXY`: 25, `BodyInertiaYY`: 26, `BodyInertiaZY`: 27, `BodyInertiaXZ`: 28, `BodyInertiaYZ`: 29, `BodyInertiaZZ`: 30, `BodyInvInertiaXX`: 31, `BodyInvInertiaYX`: 32, `BodyInvInertiaZX`: 33, `BodyInvInertiaXY`: 34, `BodyInvInertiaYY`: 35, `BodyInvInertiaZY`: 36, `BodyInvInertiaXZ`: 37, `BodyInvInertiaYZ`: 38, `BodyInvInertiaZZ`: 39, `BodyRadius`: 40} -var _BodyVarsDescMap = map[BodyVars]string{0: `BodyShape is the shape type of the object, as a Shapes type.`, 1: `BodyDynamic is the index into Dynamics for this body, which is -1 for static bodies. Use this to get current Pos and Quat values for a dynamic body.`, 2: `BodyWorld partitions bodies into different worlds for collision detection: Global bodies = -1 can collide with everything; otherwise only items within the same world collide.`, 3: `BodyGroup partitions bodies within worlds into different groups for collision detection. 0 does not collide with anything. Negative numbers are global within a world, except they don't collide amongst themselves (all non-dynamic bodies should go in -1 because they don't collide amongst each-other, but do potentially collide with dynamics). Positive numbers only collide amongst themselves, and with negative groups, but not other positive groups. This is for more special-purpose dynamics: in general use 1 for all dynamic bodies. There is an automatic constraint that the two objects within a single joint do not collide with each other, so this does not need to be handled here.`, 4: `BodySize is the size of the object (values depend on shape type).`, 5: ``, 6: ``, 7: `BodyMass is the mass of the object.`, 8: `BodyInvMass is 1/mass of the object or 0 if no mass.`, 9: `BodyBounce specifies the COR or coefficient of restitution (0..1), which determines how elastic the collision is, i.e., final velocity / initial velocity.`, 10: `BodyFriction coefficient: how much friction is generated by transverse motion. Additive across the two surfaces.`, 11: `3D position of body (structural center).`, 12: ``, 13: ``, 14: `Quaternion rotation of body.`, 15: ``, 16: ``, 17: ``, 18: `Relative center-of-mass offset from 3D position of body.`, 19: ``, 20: ``, 21: `Inertia 3x3 matrix (column matrix organization, r,c labels).`, 22: ``, 23: ``, 24: ``, 25: ``, 26: ``, 27: ``, 28: ``, 29: ``, 30: `InvInertia inverse inertia 3x3 matrix (column matrix organization, r,c labels).`, 31: ``, 32: ``, 33: ``, 34: ``, 35: ``, 36: ``, 37: ``, 38: ``, 39: `radius for broadphase collision`} +var _BodyVarsDescMap = map[BodyVars]string{0: `BodyShape is the shape type of the object, as a Shapes type.`, 1: `BodyDynamic is the index into Dynamics for this body, which is -1 for static bodies. Use this to get current Pos and Quat values for a dynamic body.`, 2: `BodyWorld partitions bodies into different worlds for collision detection: Global bodies = -1 can collide with everything; otherwise only items within the same world collide.`, 3: `BodyGroup partitions bodies within worlds into different groups for collision detection. 0 does not collide with anything. Negative numbers are global within a world, except they don't collide amongst themselves (all non-dynamic bodies should go in -1 because they don't collide amongst each-other, but do potentially collide with dynamics). Positive numbers only collide amongst themselves, and with negative groups, but not other positive groups. This is for more special-purpose dynamics: in general use 1 for all dynamic bodies. There is an automatic constraint that the two objects within a single joint do not collide with each other, so this does not need to be handled here.`, 4: `BodySize is the size of the object (values depend on shape type).`, 5: ``, 6: ``, 7: `BodyThick is the thickness of the body, as a hollow shape. If 0, then it is a solid shape (default).`, 8: `BodyMass is the mass of the object.`, 9: `BodyInvMass is 1/mass of the object or 0 if no mass.`, 10: `BodyBounce specifies the COR or coefficient of restitution (0..1), which determines how elastic the collision is, i.e., final velocity / initial velocity.`, 11: `BodyFriction coefficient: how much friction is generated by transverse motion. Additive across the two surfaces.`, 12: `3D position of body (structural center).`, 13: ``, 14: ``, 15: `Quaternion rotation of body.`, 16: ``, 17: ``, 18: ``, 19: `Relative center-of-mass offset from 3D position of body.`, 20: ``, 21: ``, 22: `Inertia 3x3 matrix (column matrix organization, r,c labels).`, 23: ``, 24: ``, 25: ``, 26: ``, 27: ``, 28: ``, 29: ``, 30: ``, 31: `InvInertia inverse inertia 3x3 matrix (column matrix organization, r,c labels).`, 32: ``, 33: ``, 34: ``, 35: ``, 36: ``, 37: ``, 38: ``, 39: ``, 40: `radius for broadphase collision`} -var _BodyVarsMap = map[BodyVars]string{0: `BodyShape`, 1: `BodyDynamic`, 2: `BodyWorld`, 3: `BodyGroup`, 4: `BodySizeX`, 5: `BodySizeY`, 6: `BodySizeZ`, 7: `BodyMass`, 8: `BodyInvMass`, 9: `BodyBounce`, 10: `BodyFriction`, 11: `BodyPosX`, 12: `BodyPosY`, 13: `BodyPosZ`, 14: `BodyQuatX`, 15: `BodyQuatY`, 16: `BodyQuatZ`, 17: `BodyQuatW`, 18: `BodyComX`, 19: `BodyComY`, 20: `BodyComZ`, 21: `BodyInertiaXX`, 22: `BodyInertiaYX`, 23: `BodyInertiaZX`, 24: `BodyInertiaXY`, 25: `BodyInertiaYY`, 26: `BodyInertiaZY`, 27: `BodyInertiaXZ`, 28: `BodyInertiaYZ`, 29: `BodyInertiaZZ`, 30: `BodyInvInertiaXX`, 31: `BodyInvInertiaYX`, 32: `BodyInvInertiaZX`, 33: `BodyInvInertiaXY`, 34: `BodyInvInertiaYY`, 35: `BodyInvInertiaZY`, 36: `BodyInvInertiaXZ`, 37: `BodyInvInertiaYZ`, 38: `BodyInvInertiaZZ`, 39: `BodyRadius`} +var _BodyVarsMap = map[BodyVars]string{0: `BodyShape`, 1: `BodyDynamic`, 2: `BodyWorld`, 3: `BodyGroup`, 4: `BodySizeX`, 5: `BodySizeY`, 6: `BodySizeZ`, 7: `BodyThick`, 8: `BodyMass`, 9: `BodyInvMass`, 10: `BodyBounce`, 11: `BodyFriction`, 12: `BodyPosX`, 13: `BodyPosY`, 14: `BodyPosZ`, 15: `BodyQuatX`, 16: `BodyQuatY`, 17: `BodyQuatZ`, 18: `BodyQuatW`, 19: `BodyComX`, 20: `BodyComY`, 21: `BodyComZ`, 22: `BodyInertiaXX`, 23: `BodyInertiaYX`, 24: `BodyInertiaZX`, 25: `BodyInertiaXY`, 26: `BodyInertiaYY`, 27: `BodyInertiaZY`, 28: `BodyInertiaXZ`, 29: `BodyInertiaYZ`, 30: `BodyInertiaZZ`, 31: `BodyInvInertiaXX`, 32: `BodyInvInertiaYX`, 33: `BodyInvInertiaZX`, 34: `BodyInvInertiaXY`, 35: `BodyInvInertiaYY`, 36: `BodyInvInertiaZY`, 37: `BodyInvInertiaXZ`, 38: `BodyInvInertiaYZ`, 39: `BodyInvInertiaZZ`, 40: `BodyRadius`} // String returns the string representation of this BodyVars value. func (i BodyVars) String() string { return enums.String(i, _BodyVarsMap) } @@ -378,20 +378,20 @@ func (i *JointDoFVars) UnmarshalText(text []byte) error { return enums.UnmarshalText(i, text, "JointDoFVars") } -var _ShapesValues = []Shapes{0, 1, 2, 3, 4} +var _ShapesValues = []Shapes{0, 1, 2, 3, 4, 5} // ShapesN is the highest valid value for type Shapes, plus one. // //gosl:start -const ShapesN Shapes = 5 +const ShapesN Shapes = 6 //gosl:end -var _ShapesValueMap = map[string]Shapes{`Plane`: 0, `Sphere`: 1, `Capsule`: 2, `Cylinder`: 3, `Box`: 4} +var _ShapesValueMap = map[string]Shapes{`Plane`: 0, `Sphere`: 1, `Capsule`: 2, `Cylinder`: 3, `Box`: 4, `Cone`: 5} -var _ShapesDescMap = map[Shapes]string{0: `Plane cannot be a dynamic shape, but is most efficient for collision computations. Use size = 0 for an infinite plane.`, 1: `Sphere. SizeX is the radius.`, 2: `Capsule, which is a cylinder with half-spheres on the ends. Natively oriented vertically along the Y axis. SizeX = bottom radius, SizeY = half-height, SizeZ = top radius.`, 3: `Cylinder, natively oriented vertically along the Y axis. If one radius is 0, then it is a cone. SizeX = bottom radius, SizeY = half-height in Y axis, SizeZ = top radius. Cylinder can not collide with a Box.`, 4: `Box is a 3D rectalinear shape. The sizes are _half_ sizes along each dimension, relative to the center.`} +var _ShapesDescMap = map[Shapes]string{0: `Plane cannot be a dynamic shape, but is most efficient for collision computations. Use size = 0 for an infinite plane. Natively extends in the X-Z plane: SizeX x SizeZ.`, 1: `Sphere. SizeX is the radius.`, 2: `Capsule is a cylinder with half-spheres on the ends. Natively oriented vertically along the Y axis. SizeX = radius, SizeY = half-height.`, 3: `Cylinder, natively oriented vertically along the Y axis. SizeX = radius, SizeY = half-height in Y axis. Cylinder can not collide with a Box.`, 4: `Box is a 3D rectalinear shape. The sizes are _half_ sizes along each dimension, relative to the center.`, 5: `Cone is like a cylinder with the top radius = 0, oriented up. SizeX = bottom radius, SizeY = half-height in Y. Cone does not support any collisions and is not recommended for interacting bodies.`} -var _ShapesMap = map[Shapes]string{0: `Plane`, 1: `Sphere`, 2: `Capsule`, 3: `Cylinder`, 4: `Box`} +var _ShapesMap = map[Shapes]string{0: `Plane`, 1: `Sphere`, 2: `Capsule`, 3: `Cylinder`, 4: `Box`, 5: `Cone`} // String returns the string representation of this Shapes value. func (i Shapes) String() string { return enums.String(i, _ShapesMap) } diff --git a/physics/shaders/CollisionBroad.wgsl b/physics/shaders/CollisionBroad.wgsl index d01fc147..879dc5d9 100644 --- a/physics/shaders/CollisionBroad.wgsl +++ b/physics/shaders/CollisionBroad.wgsl @@ -52,39 +52,40 @@ const BodyGroup: BodyVars = 3; const BodySizeX: BodyVars = 4; const BodySizeY: BodyVars = 5; const BodySizeZ: BodyVars = 6; -const BodyMass: BodyVars = 7; -const BodyInvMass: BodyVars = 8; -const BodyBounce: BodyVars = 9; -const BodyFriction: BodyVars = 10; -const BodyPosX: BodyVars = 11; -const BodyPosY: BodyVars = 12; -const BodyPosZ: BodyVars = 13; -const BodyQuatX: BodyVars = 14; -const BodyQuatY: BodyVars = 15; -const BodyQuatZ: BodyVars = 16; -const BodyQuatW: BodyVars = 17; -const BodyComX: BodyVars = 18; -const BodyComY: BodyVars = 19; -const BodyComZ: BodyVars = 20; -const BodyInertiaXX: BodyVars = 21; -const BodyInertiaYX: BodyVars = 22; -const BodyInertiaZX: BodyVars = 23; -const BodyInertiaXY: BodyVars = 24; -const BodyInertiaYY: BodyVars = 25; -const BodyInertiaZY: BodyVars = 26; -const BodyInertiaXZ: BodyVars = 27; -const BodyInertiaYZ: BodyVars = 28; -const BodyInertiaZZ: BodyVars = 29; -const BodyInvInertiaXX: BodyVars = 30; -const BodyInvInertiaYX: BodyVars = 31; -const BodyInvInertiaZX: BodyVars = 32; -const BodyInvInertiaXY: BodyVars = 33; -const BodyInvInertiaYY: BodyVars = 34; -const BodyInvInertiaZY: BodyVars = 35; -const BodyInvInertiaXZ: BodyVars = 36; -const BodyInvInertiaYZ: BodyVars = 37; -const BodyInvInertiaZZ: BodyVars = 38; -const BodyRadius: BodyVars = 39; +const BodyThick: BodyVars = 7; +const BodyMass: BodyVars = 8; +const BodyInvMass: BodyVars = 9; +const BodyBounce: BodyVars = 10; +const BodyFriction: BodyVars = 11; +const BodyPosX: BodyVars = 12; +const BodyPosY: BodyVars = 13; +const BodyPosZ: BodyVars = 14; +const BodyQuatX: BodyVars = 15; +const BodyQuatY: BodyVars = 16; +const BodyQuatZ: BodyVars = 17; +const BodyQuatW: BodyVars = 18; +const BodyComX: BodyVars = 19; +const BodyComY: BodyVars = 20; +const BodyComZ: BodyVars = 21; +const BodyInertiaXX: BodyVars = 22; +const BodyInertiaYX: BodyVars = 23; +const BodyInertiaZX: BodyVars = 24; +const BodyInertiaXY: BodyVars = 25; +const BodyInertiaYY: BodyVars = 26; +const BodyInertiaZY: BodyVars = 27; +const BodyInertiaXZ: BodyVars = 28; +const BodyInertiaYZ: BodyVars = 29; +const BodyInertiaZZ: BodyVars = 30; +const BodyInvInertiaXX: BodyVars = 31; +const BodyInvInertiaYX: BodyVars = 32; +const BodyInvInertiaZX: BodyVars = 33; +const BodyInvInertiaXY: BodyVars = 34; +const BodyInvInertiaYY: BodyVars = 35; +const BodyInvInertiaZY: BodyVars = 36; +const BodyInvInertiaXZ: BodyVars = 37; +const BodyInvInertiaYZ: BodyVars = 38; +const BodyInvInertiaZZ: BodyVars = 39; +const BodyRadius: BodyVars = 40; fn GetBodyShape(idx: i32) -> Shapes { return Shapes(bitcast(Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyShape))])); } @@ -164,7 +165,7 @@ fn CollisionBroad(i: u32) { //gosl:kernel infPlane = true; } var queryB = MulSpatialPoint(xwAR, xwAQ, xwBR); - var closest = ClosestPointPlane(szA, queryB); + var closest = ClosestPointPlane(szA.x, szA.z, queryB); var d = Length3(queryB-(closest)); if (d > rb+margin) { return; @@ -198,14 +199,6 @@ fn AddContacts(biA: i32,biB: i32,ci: i32,ncA: i32,ncB: i32) { SetContactPointIdx(ci+i, i); } } -fn ClosestPointPlane(sz: vec3,pt: vec3) -> vec3 { - var cp = pt; - if (sz.x == 0.0) { - return cp; - } - cp.x = clamp(pt.x, -sz.x, sz.x); - cp.y = clamp(pt.y, -sz.y, sz.y);return cp; -} //////// import: "control.go" alias JointControlVars = i32; //enums:enum @@ -255,7 +248,7 @@ fn DynamicQuat(idx: i32,cni: i32) -> vec4 { } //////// import: "enumgen.go" -const BodyVarsN: BodyVars = 40; +const BodyVarsN: BodyVars = 41; const ContactVarsN: ContactVars = 17; const JointControlVarsN: JointControlVars = 3; const DynamicVarsN: DynamicVars = 32; @@ -263,7 +256,7 @@ const GPUVarsN: GPUVars = 10; const JointTypesN: JointTypes = 7; const JointVarsN: JointVars = 50; const JointDoFVarsN: JointDoFVars = 7; -const ShapesN: Shapes = 5; +const ShapesN: Shapes = 6; //////// import: "joint.go" const JointLimitUnlimited = 1e10; @@ -369,13 +362,23 @@ struct GeomData { BodyIdx: i32, Shape: Shapes, MinSize: f32, - Thickness: f32, + Thick: f32, Radius: f32, Size: vec3, - WtoBR: vec3, - WtoBQ: vec4, - BtoWR: vec3, - BtoWQ: vec4, + WbR: vec3, + WbQ: vec4, + BwR: vec3, + BwQ: vec4, +} + +//////// import: "shapegeom.go" +fn ClosestPointPlane(width: f32,length: f32, pt: vec3) -> vec3 { + var cp = pt; + if (width == 0.0) { + cp.y = f32(0);return cp; + } + cp.x = clamp(pt.x, -width, width); + cp.z = clamp(pt.z, -length, length);return cp; } //////// import: "shapes.go" @@ -385,6 +388,7 @@ const Sphere: Shapes = 1; const Capsule: Shapes = 2; const Cylinder: Shapes = 3; const Box: Shapes = 4; +const Cone: Shapes = 5; fn ShapePairContacts(a: Shapes,b: Shapes, infPlane: bool, ba: ptr) -> i32 { *ba = i32(0); switch (a) { @@ -442,7 +446,7 @@ fn ShapePairContacts(a: Shapes,b: Shapes, infPlane: bool, ba: ptr) *ba = i32(12); return i32(12); } - default: { + default: { // note: Cone has no collision points! return i32(0); } } @@ -453,16 +457,15 @@ fn ShapePairContacts(a: Shapes,b: Shapes, infPlane: bool, ba: ptr) //////// import: "slmath-quaternion.go" fn MulQuatVector(q: vec4, v: vec3) -> vec3 { var xyz = vec3(q.x, q.y, q.z); - var t = MulScalar3(Cross3(xyz, v), f32(f32(2)));return v+(MulScalar3(t, q.w))+(Cross3(xyz, t)); + var t = Cross3(xyz, v)*(2);return v+(t*(q.w))+(Cross3(xyz, t)); } fn MulSpatialPoint(xP: vec3, xQ: vec4, p: vec3) -> vec3 { var dp = MulQuatVector(xQ, p);return dp+(xP); } +//////// import: "slmath-vector2.go" + //////// import: "slmath-vector3.go" -fn MulScalar3(v: vec3, s: f32) -> vec3 { - return vec3(v.x*s, v.y*s, v.z*s); -} fn Length3(v: vec3) -> f32 { return sqrt(v.x*v.x + v.y*v.y + v.z*v.z); } diff --git a/physics/shaders/CollisionNarrow.wgsl b/physics/shaders/CollisionNarrow.wgsl index ce48d930..3ad2aeac 100644 --- a/physics/shaders/CollisionNarrow.wgsl +++ b/physics/shaders/CollisionNarrow.wgsl @@ -44,39 +44,40 @@ const BodyGroup: BodyVars = 3; const BodySizeX: BodyVars = 4; const BodySizeY: BodyVars = 5; const BodySizeZ: BodyVars = 6; -const BodyMass: BodyVars = 7; -const BodyInvMass: BodyVars = 8; -const BodyBounce: BodyVars = 9; -const BodyFriction: BodyVars = 10; -const BodyPosX: BodyVars = 11; -const BodyPosY: BodyVars = 12; -const BodyPosZ: BodyVars = 13; -const BodyQuatX: BodyVars = 14; -const BodyQuatY: BodyVars = 15; -const BodyQuatZ: BodyVars = 16; -const BodyQuatW: BodyVars = 17; -const BodyComX: BodyVars = 18; -const BodyComY: BodyVars = 19; -const BodyComZ: BodyVars = 20; -const BodyInertiaXX: BodyVars = 21; -const BodyInertiaYX: BodyVars = 22; -const BodyInertiaZX: BodyVars = 23; -const BodyInertiaXY: BodyVars = 24; -const BodyInertiaYY: BodyVars = 25; -const BodyInertiaZY: BodyVars = 26; -const BodyInertiaXZ: BodyVars = 27; -const BodyInertiaYZ: BodyVars = 28; -const BodyInertiaZZ: BodyVars = 29; -const BodyInvInertiaXX: BodyVars = 30; -const BodyInvInertiaYX: BodyVars = 31; -const BodyInvInertiaZX: BodyVars = 32; -const BodyInvInertiaXY: BodyVars = 33; -const BodyInvInertiaYY: BodyVars = 34; -const BodyInvInertiaZY: BodyVars = 35; -const BodyInvInertiaXZ: BodyVars = 36; -const BodyInvInertiaYZ: BodyVars = 37; -const BodyInvInertiaZZ: BodyVars = 38; -const BodyRadius: BodyVars = 39; +const BodyThick: BodyVars = 7; +const BodyMass: BodyVars = 8; +const BodyInvMass: BodyVars = 9; +const BodyBounce: BodyVars = 10; +const BodyFriction: BodyVars = 11; +const BodyPosX: BodyVars = 12; +const BodyPosY: BodyVars = 13; +const BodyPosZ: BodyVars = 14; +const BodyQuatX: BodyVars = 15; +const BodyQuatY: BodyVars = 16; +const BodyQuatZ: BodyVars = 17; +const BodyQuatW: BodyVars = 18; +const BodyComX: BodyVars = 19; +const BodyComY: BodyVars = 20; +const BodyComZ: BodyVars = 21; +const BodyInertiaXX: BodyVars = 22; +const BodyInertiaYX: BodyVars = 23; +const BodyInertiaZX: BodyVars = 24; +const BodyInertiaXY: BodyVars = 25; +const BodyInertiaYY: BodyVars = 26; +const BodyInertiaZY: BodyVars = 27; +const BodyInertiaXZ: BodyVars = 28; +const BodyInertiaYZ: BodyVars = 29; +const BodyInertiaZZ: BodyVars = 30; +const BodyInvInertiaXX: BodyVars = 31; +const BodyInvInertiaYX: BodyVars = 32; +const BodyInvInertiaZX: BodyVars = 33; +const BodyInvInertiaXY: BodyVars = 34; +const BodyInvInertiaYY: BodyVars = 35; +const BodyInvInertiaZY: BodyVars = 36; +const BodyInvInertiaXZ: BodyVars = 37; +const BodyInvInertiaYZ: BodyVars = 38; +const BodyInvInertiaZZ: BodyVars = 39; +const BodyRadius: BodyVars = 40; fn GetBodyShape(idx: i32) -> Shapes { return Shapes(bitcast(Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyShape))])); } @@ -147,9 +148,8 @@ fn CollisionNarrow(i: u32) { //gosl:kernel var gdA = NewGeomData(biA, params.Cur, sA); var gdB = NewGeomData(biB, params.Cur, sB); var margin = params.ContactMargin; - var thickness = gdA.Thickness + gdB.Thickness; // todo + var thick = gdA.Thick + gdB.Thick; var dist = f32(1.0e6); - var cni = params.Cur; var ptA: vec3; var ptB: vec3; var norm: vec3; @@ -157,7 +157,7 @@ fn CollisionNarrow(i: u32) { //gosl:kernel case Sphere: { switch (gdB.Shape) { case Sphere: { - dist = ColSphereSphere(cni, &gdA, &gdB, &ptA, &ptB, &norm); + dist = ColSphereSphere(cpi, &gdA, &gdB, &ptA, &ptB, &norm); } case Box: { } @@ -220,7 +220,7 @@ fn DynamicQuat(idx: i32,cni: i32) -> vec4 { } //////// import: "enumgen.go" -const BodyVarsN: BodyVars = 40; +const BodyVarsN: BodyVars = 41; const ContactVarsN: ContactVars = 17; const JointControlVarsN: JointControlVars = 3; const DynamicVarsN: DynamicVars = 32; @@ -228,7 +228,7 @@ const GPUVarsN: GPUVars = 10; const JointTypesN: JointTypes = 7; const JointVarsN: JointVars = 50; const JointDoFVarsN: JointDoFVars = 7; -const ShapesN: Shapes = 5; +const ShapesN: Shapes = 6; //////// import: "joint.go" const JointLimitUnlimited = 1e10; @@ -334,40 +334,45 @@ struct GeomData { BodyIdx: i32, Shape: Shapes, MinSize: f32, - Thickness: f32, + Thick: f32, Radius: f32, Size: vec3, - WtoBR: vec3, - WtoBQ: vec4, - BtoWR: vec3, - BtoWQ: vec4, + WbR: vec3, + WbQ: vec4, + BwR: vec3, + BwQ: vec4, } fn NewGeomData(bi: i32,cni: i32, shp: Shapes) -> GeomData { var gd: GeomData; gd.BodyIdx = bi; gd.Shape = shp; gd.Size = BodySize(bi); + gd.Thick = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyThick))]; gd.MinSize = min(gd.Size.x, gd.Size.y); gd.MinSize = min(gd.MinSize, gd.Size.z); - gd.WtoBR = BodyDynamicPos(bi, cni); - gd.WtoBQ = BodyDynamicQuat(bi, cni); + gd.WbR = BodyDynamicPos(bi, cni); + gd.WbQ = BodyDynamicQuat(bi, cni); var bwR: vec3; var bwQ: vec4; - SpatialTransformInverse(gd.WtoBR, gd.WtoBQ, &bwR, &bwQ); - gd.BtoWR = bwR; - gd.BtoWQ = bwQ; + SpatialTransformInverse(gd.WbR, gd.WbQ, &bwR, &bwQ); + gd.BwR = bwR; + gd.BwQ = bwQ; gd.Radius = f32(0); if (shp == Sphere || shp == Capsule) { // todo: cone is separate gd.Radius = gd.Size.x; }return gd; } -fn ColSphereSphere(cni: i32, gdA: ptr, gdB: ptr, ptA: ptr>,ptB: ptr>,norm: ptr>) -> f32 { - *ptA = (*gdA).WtoBR; - *ptB = (*gdB).WtoBR; - var diff = (*ptA)-(*ptB); +fn ColSphereSphere(cpi: i32, gdA: ptr, gdB: ptr, ptA: ptr>,ptB: ptr>,norm: ptr>) -> f32 { + var ptAw = (*gdA).WbR; + var ptBw = (*gdB).WbR; + var diff = ptAw-(ptBw); + *ptA = ptAw; + *ptB = ptBw; *norm = Normal3(diff);return Dot3(diff, *norm); } +//////// import: "shapegeom.go" + //////// import: "shapes.go" alias Shapes = i32; //enums:enum const Plane: Shapes = 0; @@ -375,6 +380,7 @@ const Sphere: Shapes = 1; const Capsule: Shapes = 2; const Cylinder: Shapes = 3; const Box: Shapes = 4; +const Cone: Shapes = 5; //////// import: "slmath-matrix3.go" @@ -400,7 +406,7 @@ fn QuatNormalize(q: vec4) -> vec4 { } fn MulQuatVector(q: vec4, v: vec3) -> vec3 { var xyz = vec3(q.x, q.y, q.z); - var t = MulScalar3(Cross3(xyz, v), f32(f32(2)));return v+(MulScalar3(t, q.w))+(Cross3(xyz, t)); + var t = Cross3(xyz, v)*(2);return v+(t*(q.w))+(Cross3(xyz, t)); } fn SpatialTransformInverse(p: vec3, q: vec4, oP: ptr>, oQ: ptr>) { var qi = QuatInverse(q); @@ -414,13 +420,9 @@ fn QuatInverse(q: vec4) -> vec4 { nq.z *= f32(-1);return QuatNormalize(nq); } +//////// import: "slmath-vector2.go" + //////// import: "slmath-vector3.go" -fn MulScalar3(v: vec3, s: f32) -> vec3 { - return vec3(v.x*s, v.y*s, v.z*s); -} -fn DivScalar3(v: vec3, s: f32) -> vec3 { - return vec3(v.x/s, v.y/s, v.z/s); -} fn Negate3(v: vec3) -> vec3 { return vec3(-v.x, -v.y, -v.z); } @@ -431,7 +433,7 @@ fn Dot3(v: vec3,o: vec3) -> f32 { return v.x*o.x + v.y*o.y + v.z*o.z; } fn Normal3(v: vec3) -> vec3 { - return DivScalar3(v, Length3(v)); + return v/(Length3(v)); } fn Cross3(v: vec3,o: vec3) -> vec3 { return vec3(v.y*o.z-v.z*o.y, v.z*o.x-v.x*o.z, v.x*o.y-v.y*o.x); diff --git a/physics/shaders/DeltasFromJoints.wgsl b/physics/shaders/DeltasFromJoints.wgsl index a364f9dd..24ad6e71 100644 --- a/physics/shaders/DeltasFromJoints.wgsl +++ b/physics/shaders/DeltasFromJoints.wgsl @@ -44,39 +44,40 @@ const BodyGroup: BodyVars = 3; const BodySizeX: BodyVars = 4; const BodySizeY: BodyVars = 5; const BodySizeZ: BodyVars = 6; -const BodyMass: BodyVars = 7; -const BodyInvMass: BodyVars = 8; -const BodyBounce: BodyVars = 9; -const BodyFriction: BodyVars = 10; -const BodyPosX: BodyVars = 11; -const BodyPosY: BodyVars = 12; -const BodyPosZ: BodyVars = 13; -const BodyQuatX: BodyVars = 14; -const BodyQuatY: BodyVars = 15; -const BodyQuatZ: BodyVars = 16; -const BodyQuatW: BodyVars = 17; -const BodyComX: BodyVars = 18; -const BodyComY: BodyVars = 19; -const BodyComZ: BodyVars = 20; -const BodyInertiaXX: BodyVars = 21; -const BodyInertiaYX: BodyVars = 22; -const BodyInertiaZX: BodyVars = 23; -const BodyInertiaXY: BodyVars = 24; -const BodyInertiaYY: BodyVars = 25; -const BodyInertiaZY: BodyVars = 26; -const BodyInertiaXZ: BodyVars = 27; -const BodyInertiaYZ: BodyVars = 28; -const BodyInertiaZZ: BodyVars = 29; -const BodyInvInertiaXX: BodyVars = 30; -const BodyInvInertiaYX: BodyVars = 31; -const BodyInvInertiaZX: BodyVars = 32; -const BodyInvInertiaXY: BodyVars = 33; -const BodyInvInertiaYY: BodyVars = 34; -const BodyInvInertiaZY: BodyVars = 35; -const BodyInvInertiaXZ: BodyVars = 36; -const BodyInvInertiaYZ: BodyVars = 37; -const BodyInvInertiaZZ: BodyVars = 38; -const BodyRadius: BodyVars = 39; +const BodyThick: BodyVars = 7; +const BodyMass: BodyVars = 8; +const BodyInvMass: BodyVars = 9; +const BodyBounce: BodyVars = 10; +const BodyFriction: BodyVars = 11; +const BodyPosX: BodyVars = 12; +const BodyPosY: BodyVars = 13; +const BodyPosZ: BodyVars = 14; +const BodyQuatX: BodyVars = 15; +const BodyQuatY: BodyVars = 16; +const BodyQuatZ: BodyVars = 17; +const BodyQuatW: BodyVars = 18; +const BodyComX: BodyVars = 19; +const BodyComY: BodyVars = 20; +const BodyComZ: BodyVars = 21; +const BodyInertiaXX: BodyVars = 22; +const BodyInertiaYX: BodyVars = 23; +const BodyInertiaZX: BodyVars = 24; +const BodyInertiaXY: BodyVars = 25; +const BodyInertiaYY: BodyVars = 26; +const BodyInertiaZY: BodyVars = 27; +const BodyInertiaXZ: BodyVars = 28; +const BodyInertiaYZ: BodyVars = 29; +const BodyInertiaZZ: BodyVars = 30; +const BodyInvInertiaXX: BodyVars = 31; +const BodyInvInertiaYX: BodyVars = 32; +const BodyInvInertiaZX: BodyVars = 33; +const BodyInvInertiaXY: BodyVars = 34; +const BodyInvInertiaYY: BodyVars = 35; +const BodyInvInertiaZY: BodyVars = 36; +const BodyInvInertiaXZ: BodyVars = 37; +const BodyInvInertiaYZ: BodyVars = 38; +const BodyInvInertiaZZ: BodyVars = 39; +const BodyRadius: BodyVars = 40; //////// import: "contact.go" alias ContactVars = i32; //enums:enum @@ -150,7 +151,7 @@ fn SetDynamicAngDelta(idx: i32,cni: i32, angDelta: vec3) { } //////// import: "enumgen.go" -const BodyVarsN: BodyVars = 40; +const BodyVarsN: BodyVars = 41; const ContactVarsN: ContactVars = 17; const JointControlVarsN: JointControlVars = 3; const DynamicVarsN: DynamicVars = 32; @@ -158,7 +159,7 @@ const GPUVarsN: GPUVars = 10; const JointTypesN: JointTypes = 7; const JointVarsN: JointVars = 50; const JointDoFVarsN: JointDoFVars = 7; -const ShapesN: Shapes = 5; +const ShapesN: Shapes = 6; //////// import: "joint.go" const JointLimitUnlimited = 1e10; @@ -276,15 +277,17 @@ struct GeomData { BodyIdx: i32, Shape: Shapes, MinSize: f32, - Thickness: f32, + Thick: f32, Radius: f32, Size: vec3, - WtoBR: vec3, - WtoBQ: vec4, - BtoWR: vec3, - BtoWQ: vec4, + WbR: vec3, + WbQ: vec4, + BwR: vec3, + BwQ: vec4, } +//////// import: "shapegeom.go" + //////// import: "shapes.go" alias Shapes = i32; //enums:enum const Plane: Shapes = 0; @@ -292,11 +295,14 @@ const Sphere: Shapes = 1; const Capsule: Shapes = 2; const Cylinder: Shapes = 3; const Box: Shapes = 4; +const Cone: Shapes = 5; //////// import: "slmath-matrix3.go" //////// import: "slmath-quaternion.go" +//////// import: "slmath-vector2.go" + //////// import: "slmath-vector3.go" //////// import: "step.go" diff --git a/physics/shaders/DynamicsCurToNext.wgsl b/physics/shaders/DynamicsCurToNext.wgsl index b7a5b89a..34dce5f5 100644 --- a/physics/shaders/DynamicsCurToNext.wgsl +++ b/physics/shaders/DynamicsCurToNext.wgsl @@ -36,39 +36,40 @@ const BodyGroup: BodyVars = 3; const BodySizeX: BodyVars = 4; const BodySizeY: BodyVars = 5; const BodySizeZ: BodyVars = 6; -const BodyMass: BodyVars = 7; -const BodyInvMass: BodyVars = 8; -const BodyBounce: BodyVars = 9; -const BodyFriction: BodyVars = 10; -const BodyPosX: BodyVars = 11; -const BodyPosY: BodyVars = 12; -const BodyPosZ: BodyVars = 13; -const BodyQuatX: BodyVars = 14; -const BodyQuatY: BodyVars = 15; -const BodyQuatZ: BodyVars = 16; -const BodyQuatW: BodyVars = 17; -const BodyComX: BodyVars = 18; -const BodyComY: BodyVars = 19; -const BodyComZ: BodyVars = 20; -const BodyInertiaXX: BodyVars = 21; -const BodyInertiaYX: BodyVars = 22; -const BodyInertiaZX: BodyVars = 23; -const BodyInertiaXY: BodyVars = 24; -const BodyInertiaYY: BodyVars = 25; -const BodyInertiaZY: BodyVars = 26; -const BodyInertiaXZ: BodyVars = 27; -const BodyInertiaYZ: BodyVars = 28; -const BodyInertiaZZ: BodyVars = 29; -const BodyInvInertiaXX: BodyVars = 30; -const BodyInvInertiaYX: BodyVars = 31; -const BodyInvInertiaZX: BodyVars = 32; -const BodyInvInertiaXY: BodyVars = 33; -const BodyInvInertiaYY: BodyVars = 34; -const BodyInvInertiaZY: BodyVars = 35; -const BodyInvInertiaXZ: BodyVars = 36; -const BodyInvInertiaYZ: BodyVars = 37; -const BodyInvInertiaZZ: BodyVars = 38; -const BodyRadius: BodyVars = 39; +const BodyThick: BodyVars = 7; +const BodyMass: BodyVars = 8; +const BodyInvMass: BodyVars = 9; +const BodyBounce: BodyVars = 10; +const BodyFriction: BodyVars = 11; +const BodyPosX: BodyVars = 12; +const BodyPosY: BodyVars = 13; +const BodyPosZ: BodyVars = 14; +const BodyQuatX: BodyVars = 15; +const BodyQuatY: BodyVars = 16; +const BodyQuatZ: BodyVars = 17; +const BodyQuatW: BodyVars = 18; +const BodyComX: BodyVars = 19; +const BodyComY: BodyVars = 20; +const BodyComZ: BodyVars = 21; +const BodyInertiaXX: BodyVars = 22; +const BodyInertiaYX: BodyVars = 23; +const BodyInertiaZX: BodyVars = 24; +const BodyInertiaXY: BodyVars = 25; +const BodyInertiaYY: BodyVars = 26; +const BodyInertiaZY: BodyVars = 27; +const BodyInertiaXZ: BodyVars = 28; +const BodyInertiaYZ: BodyVars = 29; +const BodyInertiaZZ: BodyVars = 30; +const BodyInvInertiaXX: BodyVars = 31; +const BodyInvInertiaYX: BodyVars = 32; +const BodyInvInertiaZX: BodyVars = 33; +const BodyInvInertiaXY: BodyVars = 34; +const BodyInvInertiaYY: BodyVars = 35; +const BodyInvInertiaZY: BodyVars = 36; +const BodyInvInertiaXZ: BodyVars = 37; +const BodyInvInertiaYZ: BodyVars = 38; +const BodyInvInertiaZZ: BodyVars = 39; +const BodyRadius: BodyVars = 40; //////// import: "contact.go" alias ContactVars = i32; //enums:enum @@ -132,7 +133,7 @@ const DynAngDeltaY: DynamicVars = 30; const DynAngDeltaZ: DynamicVars = 31; //////// import: "enumgen.go" -const BodyVarsN: BodyVars = 40; +const BodyVarsN: BodyVars = 41; const ContactVarsN: ContactVars = 17; const JointControlVarsN: JointControlVars = 3; const DynamicVarsN: DynamicVars = 32; @@ -140,7 +141,7 @@ const GPUVarsN: GPUVars = 10; const JointTypesN: JointTypes = 7; const JointVarsN: JointVars = 50; const JointDoFVarsN: JointDoFVars = 7; -const ShapesN: Shapes = 5; +const ShapesN: Shapes = 6; //////// import: "joint.go" const JointLimitUnlimited = 1e10; @@ -246,15 +247,17 @@ struct GeomData { BodyIdx: i32, Shape: Shapes, MinSize: f32, - Thickness: f32, + Thick: f32, Radius: f32, Size: vec3, - WtoBR: vec3, - WtoBQ: vec4, - BtoWR: vec3, - BtoWQ: vec4, + WbR: vec3, + WbQ: vec4, + BwR: vec3, + BwQ: vec4, } +//////// import: "shapegeom.go" + //////// import: "shapes.go" alias Shapes = i32; //enums:enum const Plane: Shapes = 0; @@ -262,11 +265,14 @@ const Sphere: Shapes = 1; const Capsule: Shapes = 2; const Cylinder: Shapes = 3; const Box: Shapes = 4; +const Cone: Shapes = 5; //////// import: "slmath-matrix3.go" //////// import: "slmath-quaternion.go" +//////// import: "slmath-vector2.go" + //////// import: "slmath-vector3.go" //////// import: "step.go" diff --git a/physics/shaders/ForcesFromJoints.wgsl b/physics/shaders/ForcesFromJoints.wgsl index af72b0d8..26ac5cef 100644 --- a/physics/shaders/ForcesFromJoints.wgsl +++ b/physics/shaders/ForcesFromJoints.wgsl @@ -44,39 +44,40 @@ const BodyGroup: BodyVars = 3; const BodySizeX: BodyVars = 4; const BodySizeY: BodyVars = 5; const BodySizeZ: BodyVars = 6; -const BodyMass: BodyVars = 7; -const BodyInvMass: BodyVars = 8; -const BodyBounce: BodyVars = 9; -const BodyFriction: BodyVars = 10; -const BodyPosX: BodyVars = 11; -const BodyPosY: BodyVars = 12; -const BodyPosZ: BodyVars = 13; -const BodyQuatX: BodyVars = 14; -const BodyQuatY: BodyVars = 15; -const BodyQuatZ: BodyVars = 16; -const BodyQuatW: BodyVars = 17; -const BodyComX: BodyVars = 18; -const BodyComY: BodyVars = 19; -const BodyComZ: BodyVars = 20; -const BodyInertiaXX: BodyVars = 21; -const BodyInertiaYX: BodyVars = 22; -const BodyInertiaZX: BodyVars = 23; -const BodyInertiaXY: BodyVars = 24; -const BodyInertiaYY: BodyVars = 25; -const BodyInertiaZY: BodyVars = 26; -const BodyInertiaXZ: BodyVars = 27; -const BodyInertiaYZ: BodyVars = 28; -const BodyInertiaZZ: BodyVars = 29; -const BodyInvInertiaXX: BodyVars = 30; -const BodyInvInertiaYX: BodyVars = 31; -const BodyInvInertiaZX: BodyVars = 32; -const BodyInvInertiaXY: BodyVars = 33; -const BodyInvInertiaYY: BodyVars = 34; -const BodyInvInertiaZY: BodyVars = 35; -const BodyInvInertiaXZ: BodyVars = 36; -const BodyInvInertiaYZ: BodyVars = 37; -const BodyInvInertiaZZ: BodyVars = 38; -const BodyRadius: BodyVars = 39; +const BodyThick: BodyVars = 7; +const BodyMass: BodyVars = 8; +const BodyInvMass: BodyVars = 9; +const BodyBounce: BodyVars = 10; +const BodyFriction: BodyVars = 11; +const BodyPosX: BodyVars = 12; +const BodyPosY: BodyVars = 13; +const BodyPosZ: BodyVars = 14; +const BodyQuatX: BodyVars = 15; +const BodyQuatY: BodyVars = 16; +const BodyQuatZ: BodyVars = 17; +const BodyQuatW: BodyVars = 18; +const BodyComX: BodyVars = 19; +const BodyComY: BodyVars = 20; +const BodyComZ: BodyVars = 21; +const BodyInertiaXX: BodyVars = 22; +const BodyInertiaYX: BodyVars = 23; +const BodyInertiaZX: BodyVars = 24; +const BodyInertiaXY: BodyVars = 25; +const BodyInertiaYY: BodyVars = 26; +const BodyInertiaZY: BodyVars = 27; +const BodyInertiaXZ: BodyVars = 28; +const BodyInertiaYZ: BodyVars = 29; +const BodyInertiaZZ: BodyVars = 30; +const BodyInvInertiaXX: BodyVars = 31; +const BodyInvInertiaYX: BodyVars = 32; +const BodyInvInertiaZX: BodyVars = 33; +const BodyInvInertiaXY: BodyVars = 34; +const BodyInvInertiaYY: BodyVars = 35; +const BodyInvInertiaZY: BodyVars = 36; +const BodyInvInertiaXZ: BodyVars = 37; +const BodyInvInertiaYZ: BodyVars = 38; +const BodyInvInertiaZZ: BodyVars = 39; +const BodyRadius: BodyVars = 40; //////// import: "contact.go" alias ContactVars = i32; //enums:enum @@ -150,7 +151,7 @@ fn SetDynamicTorque(idx: i32,cni: i32, torque: vec3) { } //////// import: "enumgen.go" -const BodyVarsN: BodyVars = 40; +const BodyVarsN: BodyVars = 41; const ContactVarsN: ContactVars = 17; const JointControlVarsN: JointControlVars = 3; const DynamicVarsN: DynamicVars = 32; @@ -158,7 +159,7 @@ const GPUVarsN: GPUVars = 10; const JointTypesN: JointTypes = 7; const JointVarsN: JointVars = 50; const JointDoFVarsN: JointDoFVars = 7; -const ShapesN: Shapes = 5; +const ShapesN: Shapes = 6; //////// import: "joint.go" const JointLimitUnlimited = 1e10; @@ -276,15 +277,17 @@ struct GeomData { BodyIdx: i32, Shape: Shapes, MinSize: f32, - Thickness: f32, + Thick: f32, Radius: f32, Size: vec3, - WtoBR: vec3, - WtoBQ: vec4, - BtoWR: vec3, - BtoWQ: vec4, + WbR: vec3, + WbQ: vec4, + BwR: vec3, + BwQ: vec4, } +//////// import: "shapegeom.go" + //////// import: "shapes.go" alias Shapes = i32; //enums:enum const Plane: Shapes = 0; @@ -292,11 +295,14 @@ const Sphere: Shapes = 1; const Capsule: Shapes = 2; const Cylinder: Shapes = 3; const Box: Shapes = 4; +const Cone: Shapes = 5; //////// import: "slmath-matrix3.go" //////// import: "slmath-quaternion.go" +//////// import: "slmath-vector2.go" + //////// import: "slmath-vector3.go" //////// import: "step.go" diff --git a/physics/shaders/InitDynamics.wgsl b/physics/shaders/InitDynamics.wgsl index 57327838..0ba45597 100644 --- a/physics/shaders/InitDynamics.wgsl +++ b/physics/shaders/InitDynamics.wgsl @@ -42,39 +42,40 @@ const BodyGroup: BodyVars = 3; const BodySizeX: BodyVars = 4; const BodySizeY: BodyVars = 5; const BodySizeZ: BodyVars = 6; -const BodyMass: BodyVars = 7; -const BodyInvMass: BodyVars = 8; -const BodyBounce: BodyVars = 9; -const BodyFriction: BodyVars = 10; -const BodyPosX: BodyVars = 11; -const BodyPosY: BodyVars = 12; -const BodyPosZ: BodyVars = 13; -const BodyQuatX: BodyVars = 14; -const BodyQuatY: BodyVars = 15; -const BodyQuatZ: BodyVars = 16; -const BodyQuatW: BodyVars = 17; -const BodyComX: BodyVars = 18; -const BodyComY: BodyVars = 19; -const BodyComZ: BodyVars = 20; -const BodyInertiaXX: BodyVars = 21; -const BodyInertiaYX: BodyVars = 22; -const BodyInertiaZX: BodyVars = 23; -const BodyInertiaXY: BodyVars = 24; -const BodyInertiaYY: BodyVars = 25; -const BodyInertiaZY: BodyVars = 26; -const BodyInertiaXZ: BodyVars = 27; -const BodyInertiaYZ: BodyVars = 28; -const BodyInertiaZZ: BodyVars = 29; -const BodyInvInertiaXX: BodyVars = 30; -const BodyInvInertiaYX: BodyVars = 31; -const BodyInvInertiaZX: BodyVars = 32; -const BodyInvInertiaXY: BodyVars = 33; -const BodyInvInertiaYY: BodyVars = 34; -const BodyInvInertiaZY: BodyVars = 35; -const BodyInvInertiaXZ: BodyVars = 36; -const BodyInvInertiaYZ: BodyVars = 37; -const BodyInvInertiaZZ: BodyVars = 38; -const BodyRadius: BodyVars = 39; +const BodyThick: BodyVars = 7; +const BodyMass: BodyVars = 8; +const BodyInvMass: BodyVars = 9; +const BodyBounce: BodyVars = 10; +const BodyFriction: BodyVars = 11; +const BodyPosX: BodyVars = 12; +const BodyPosY: BodyVars = 13; +const BodyPosZ: BodyVars = 14; +const BodyQuatX: BodyVars = 15; +const BodyQuatY: BodyVars = 16; +const BodyQuatZ: BodyVars = 17; +const BodyQuatW: BodyVars = 18; +const BodyComX: BodyVars = 19; +const BodyComY: BodyVars = 20; +const BodyComZ: BodyVars = 21; +const BodyInertiaXX: BodyVars = 22; +const BodyInertiaYX: BodyVars = 23; +const BodyInertiaZX: BodyVars = 24; +const BodyInertiaXY: BodyVars = 25; +const BodyInertiaYY: BodyVars = 26; +const BodyInertiaZY: BodyVars = 27; +const BodyInertiaXZ: BodyVars = 28; +const BodyInertiaYZ: BodyVars = 29; +const BodyInertiaZZ: BodyVars = 30; +const BodyInvInertiaXX: BodyVars = 31; +const BodyInvInertiaYX: BodyVars = 32; +const BodyInvInertiaZX: BodyVars = 33; +const BodyInvInertiaXY: BodyVars = 34; +const BodyInvInertiaYY: BodyVars = 35; +const BodyInvInertiaZY: BodyVars = 36; +const BodyInvInertiaXZ: BodyVars = 37; +const BodyInvInertiaYZ: BodyVars = 38; +const BodyInvInertiaZZ: BodyVars = 39; +const BodyRadius: BodyVars = 40; //////// import: "contact.go" alias ContactVars = i32; //enums:enum @@ -141,7 +142,7 @@ fn DynamicBody(idx: i32) -> i32 { } //////// import: "enumgen.go" -const BodyVarsN: BodyVars = 40; +const BodyVarsN: BodyVars = 41; const ContactVarsN: ContactVars = 17; const JointControlVarsN: JointControlVars = 3; const DynamicVarsN: DynamicVars = 32; @@ -149,7 +150,7 @@ const GPUVarsN: GPUVars = 10; const JointTypesN: JointTypes = 7; const JointVarsN: JointVars = 50; const JointDoFVarsN: JointDoFVars = 7; -const ShapesN: Shapes = 5; +const ShapesN: Shapes = 6; //////// import: "joint.go" const JointLimitUnlimited = 1e10; @@ -255,15 +256,17 @@ struct GeomData { BodyIdx: i32, Shape: Shapes, MinSize: f32, - Thickness: f32, + Thick: f32, Radius: f32, Size: vec3, - WtoBR: vec3, - WtoBQ: vec4, - BtoWR: vec3, - BtoWQ: vec4, + WbR: vec3, + WbQ: vec4, + BwR: vec3, + BwQ: vec4, } +//////// import: "shapegeom.go" + //////// import: "shapes.go" alias Shapes = i32; //enums:enum const Plane: Shapes = 0; @@ -271,11 +274,14 @@ const Sphere: Shapes = 1; const Capsule: Shapes = 2; const Cylinder: Shapes = 3; const Box: Shapes = 4; +const Cone: Shapes = 5; //////// import: "slmath-matrix3.go" //////// import: "slmath-quaternion.go" +//////// import: "slmath-vector2.go" + //////// import: "slmath-vector3.go" //////// import: "step.go" diff --git a/physics/shaders/StepBodyDeltas.wgsl b/physics/shaders/StepBodyDeltas.wgsl index 86cea64b..e0216b77 100644 --- a/physics/shaders/StepBodyDeltas.wgsl +++ b/physics/shaders/StepBodyDeltas.wgsl @@ -42,39 +42,40 @@ const BodyGroup: BodyVars = 3; const BodySizeX: BodyVars = 4; const BodySizeY: BodyVars = 5; const BodySizeZ: BodyVars = 6; -const BodyMass: BodyVars = 7; -const BodyInvMass: BodyVars = 8; -const BodyBounce: BodyVars = 9; -const BodyFriction: BodyVars = 10; -const BodyPosX: BodyVars = 11; -const BodyPosY: BodyVars = 12; -const BodyPosZ: BodyVars = 13; -const BodyQuatX: BodyVars = 14; -const BodyQuatY: BodyVars = 15; -const BodyQuatZ: BodyVars = 16; -const BodyQuatW: BodyVars = 17; -const BodyComX: BodyVars = 18; -const BodyComY: BodyVars = 19; -const BodyComZ: BodyVars = 20; -const BodyInertiaXX: BodyVars = 21; -const BodyInertiaYX: BodyVars = 22; -const BodyInertiaZX: BodyVars = 23; -const BodyInertiaXY: BodyVars = 24; -const BodyInertiaYY: BodyVars = 25; -const BodyInertiaZY: BodyVars = 26; -const BodyInertiaXZ: BodyVars = 27; -const BodyInertiaYZ: BodyVars = 28; -const BodyInertiaZZ: BodyVars = 29; -const BodyInvInertiaXX: BodyVars = 30; -const BodyInvInertiaYX: BodyVars = 31; -const BodyInvInertiaZX: BodyVars = 32; -const BodyInvInertiaXY: BodyVars = 33; -const BodyInvInertiaYY: BodyVars = 34; -const BodyInvInertiaZY: BodyVars = 35; -const BodyInvInertiaXZ: BodyVars = 36; -const BodyInvInertiaYZ: BodyVars = 37; -const BodyInvInertiaZZ: BodyVars = 38; -const BodyRadius: BodyVars = 39; +const BodyThick: BodyVars = 7; +const BodyMass: BodyVars = 8; +const BodyInvMass: BodyVars = 9; +const BodyBounce: BodyVars = 10; +const BodyFriction: BodyVars = 11; +const BodyPosX: BodyVars = 12; +const BodyPosY: BodyVars = 13; +const BodyPosZ: BodyVars = 14; +const BodyQuatX: BodyVars = 15; +const BodyQuatY: BodyVars = 16; +const BodyQuatZ: BodyVars = 17; +const BodyQuatW: BodyVars = 18; +const BodyComX: BodyVars = 19; +const BodyComY: BodyVars = 20; +const BodyComZ: BodyVars = 21; +const BodyInertiaXX: BodyVars = 22; +const BodyInertiaYX: BodyVars = 23; +const BodyInertiaZX: BodyVars = 24; +const BodyInertiaXY: BodyVars = 25; +const BodyInertiaYY: BodyVars = 26; +const BodyInertiaZY: BodyVars = 27; +const BodyInertiaXZ: BodyVars = 28; +const BodyInertiaYZ: BodyVars = 29; +const BodyInertiaZZ: BodyVars = 30; +const BodyInvInertiaXX: BodyVars = 31; +const BodyInvInertiaYX: BodyVars = 32; +const BodyInvInertiaZX: BodyVars = 33; +const BodyInvInertiaXY: BodyVars = 34; +const BodyInvInertiaYY: BodyVars = 35; +const BodyInvInertiaZY: BodyVars = 36; +const BodyInvInertiaXZ: BodyVars = 37; +const BodyInvInertiaYZ: BodyVars = 38; +const BodyInvInertiaZZ: BodyVars = 39; +const BodyRadius: BodyVars = 40; fn BodyCom(idx: i32) -> vec3 { return vec3(Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComX))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComY))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComZ))]); } @@ -187,7 +188,7 @@ fn SetDynamicAngDelta(idx: i32,cni: i32, angDelta: vec3) { } //////// import: "enumgen.go" -const BodyVarsN: BodyVars = 40; +const BodyVarsN: BodyVars = 41; const ContactVarsN: ContactVars = 17; const JointControlVarsN: JointControlVars = 3; const DynamicVarsN: DynamicVars = 32; @@ -195,7 +196,7 @@ const GPUVarsN: GPUVars = 10; const JointTypesN: JointTypes = 7; const JointVarsN: JointVars = 50; const JointDoFVarsN: JointDoFVars = 7; -const ShapesN: Shapes = 5; +const ShapesN: Shapes = 6; //////// import: "joint.go" const JointLimitUnlimited = 1e10; @@ -301,15 +302,17 @@ struct GeomData { BodyIdx: i32, Shape: Shapes, MinSize: f32, - Thickness: f32, + Thick: f32, Radius: f32, Size: vec3, - WtoBR: vec3, - WtoBQ: vec4, - BtoWR: vec3, - BtoWQ: vec4, + WbR: vec3, + WbQ: vec4, + BwR: vec3, + BwQ: vec4, } +//////// import: "shapegeom.go" + //////// import: "shapes.go" alias Shapes = i32; //enums:enum const Plane: Shapes = 0; @@ -317,6 +320,7 @@ const Sphere: Shapes = 1; const Capsule: Shapes = 2; const Cylinder: Shapes = 3; const Box: Shapes = 4; +const Cone: Shapes = 5; //////// import: "slmath-matrix3.go" @@ -342,11 +346,11 @@ fn QuatNormalize(q: vec4) -> vec4 { } fn MulQuatVector(q: vec4, v: vec3) -> vec3 { var xyz = vec3(q.x, q.y, q.z); - var t = MulScalar3(Cross3(xyz, v), f32(f32(2)));return v+(MulScalar3(t, q.w))+(Cross3(xyz, t)); + var t = Cross3(xyz, v)*(2);return v+(t*(q.w))+(Cross3(xyz, t)); } fn MulQuatVectorInverse(q: vec4, v: vec3) -> vec3 { var xyz = vec3(q.x, q.y, q.z); - var t = MulScalar3(Cross3(xyz, v), f32(f32(2)));return v-(MulScalar3(t, q.w))+(Cross3(xyz, t)); + var t = Cross3(xyz, v)*(2);return v-(t*(q.w))+(Cross3(xyz, t)); } fn MulQuats(a: vec4,b: vec4) -> vec4 { var q: vec4; @@ -356,10 +360,9 @@ fn MulQuats(a: vec4,b: vec4) -> vec4 { q.w = a.w*b.w - a.x*b.x - a.y*b.y - a.z*b.z;return q; } +//////// import: "slmath-vector2.go" + //////// import: "slmath-vector3.go" -fn MulScalar3(v: vec3, s: f32) -> vec3 { - return vec3(v.x*s, v.y*s, v.z*s); -} fn Length3(v: vec3) -> f32 { return sqrt(v.x*v.x + v.y*v.y + v.z*v.z); } diff --git a/physics/shaders/StepIntegrateBodies.wgsl b/physics/shaders/StepIntegrateBodies.wgsl index 015ec080..b751db10 100644 --- a/physics/shaders/StepIntegrateBodies.wgsl +++ b/physics/shaders/StepIntegrateBodies.wgsl @@ -42,39 +42,40 @@ const BodyGroup: BodyVars = 3; const BodySizeX: BodyVars = 4; const BodySizeY: BodyVars = 5; const BodySizeZ: BodyVars = 6; -const BodyMass: BodyVars = 7; -const BodyInvMass: BodyVars = 8; -const BodyBounce: BodyVars = 9; -const BodyFriction: BodyVars = 10; -const BodyPosX: BodyVars = 11; -const BodyPosY: BodyVars = 12; -const BodyPosZ: BodyVars = 13; -const BodyQuatX: BodyVars = 14; -const BodyQuatY: BodyVars = 15; -const BodyQuatZ: BodyVars = 16; -const BodyQuatW: BodyVars = 17; -const BodyComX: BodyVars = 18; -const BodyComY: BodyVars = 19; -const BodyComZ: BodyVars = 20; -const BodyInertiaXX: BodyVars = 21; -const BodyInertiaYX: BodyVars = 22; -const BodyInertiaZX: BodyVars = 23; -const BodyInertiaXY: BodyVars = 24; -const BodyInertiaYY: BodyVars = 25; -const BodyInertiaZY: BodyVars = 26; -const BodyInertiaXZ: BodyVars = 27; -const BodyInertiaYZ: BodyVars = 28; -const BodyInertiaZZ: BodyVars = 29; -const BodyInvInertiaXX: BodyVars = 30; -const BodyInvInertiaYX: BodyVars = 31; -const BodyInvInertiaZX: BodyVars = 32; -const BodyInvInertiaXY: BodyVars = 33; -const BodyInvInertiaYY: BodyVars = 34; -const BodyInvInertiaZY: BodyVars = 35; -const BodyInvInertiaXZ: BodyVars = 36; -const BodyInvInertiaYZ: BodyVars = 37; -const BodyInvInertiaZZ: BodyVars = 38; -const BodyRadius: BodyVars = 39; +const BodyThick: BodyVars = 7; +const BodyMass: BodyVars = 8; +const BodyInvMass: BodyVars = 9; +const BodyBounce: BodyVars = 10; +const BodyFriction: BodyVars = 11; +const BodyPosX: BodyVars = 12; +const BodyPosY: BodyVars = 13; +const BodyPosZ: BodyVars = 14; +const BodyQuatX: BodyVars = 15; +const BodyQuatY: BodyVars = 16; +const BodyQuatZ: BodyVars = 17; +const BodyQuatW: BodyVars = 18; +const BodyComX: BodyVars = 19; +const BodyComY: BodyVars = 20; +const BodyComZ: BodyVars = 21; +const BodyInertiaXX: BodyVars = 22; +const BodyInertiaYX: BodyVars = 23; +const BodyInertiaZX: BodyVars = 24; +const BodyInertiaXY: BodyVars = 25; +const BodyInertiaYY: BodyVars = 26; +const BodyInertiaZY: BodyVars = 27; +const BodyInertiaXZ: BodyVars = 28; +const BodyInertiaYZ: BodyVars = 29; +const BodyInertiaZZ: BodyVars = 30; +const BodyInvInertiaXX: BodyVars = 31; +const BodyInvInertiaYX: BodyVars = 32; +const BodyInvInertiaZX: BodyVars = 33; +const BodyInvInertiaXY: BodyVars = 34; +const BodyInvInertiaYY: BodyVars = 35; +const BodyInvInertiaZY: BodyVars = 36; +const BodyInvInertiaXZ: BodyVars = 37; +const BodyInvInertiaYZ: BodyVars = 38; +const BodyInvInertiaZZ: BodyVars = 39; +const BodyRadius: BodyVars = 40; fn BodyCom(idx: i32) -> vec3 { return vec3(Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComX))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComY))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComZ))]); } @@ -193,7 +194,7 @@ fn SetDynamicAngDelta(idx: i32,cni: i32, angDelta: vec3) { } //////// import: "enumgen.go" -const BodyVarsN: BodyVars = 40; +const BodyVarsN: BodyVars = 41; const ContactVarsN: ContactVars = 17; const JointControlVarsN: JointControlVars = 3; const DynamicVarsN: DynamicVars = 32; @@ -201,7 +202,7 @@ const GPUVarsN: GPUVars = 10; const JointTypesN: JointTypes = 7; const JointVarsN: JointVars = 50; const JointDoFVarsN: JointDoFVars = 7; -const ShapesN: Shapes = 5; +const ShapesN: Shapes = 6; //////// import: "joint.go" const JointLimitUnlimited = 1e10; @@ -307,15 +308,17 @@ struct GeomData { BodyIdx: i32, Shape: Shapes, MinSize: f32, - Thickness: f32, + Thick: f32, Radius: f32, Size: vec3, - WtoBR: vec3, - WtoBQ: vec4, - BtoWR: vec3, - BtoWQ: vec4, + WbR: vec3, + WbQ: vec4, + BwR: vec3, + BwQ: vec4, } +//////// import: "shapegeom.go" + //////// import: "shapes.go" alias Shapes = i32; //enums:enum const Plane: Shapes = 0; @@ -323,6 +326,7 @@ const Sphere: Shapes = 1; const Capsule: Shapes = 2; const Cylinder: Shapes = 3; const Box: Shapes = 4; +const Cone: Shapes = 5; //////// import: "slmath-matrix3.go" @@ -348,11 +352,11 @@ fn QuatNormalize(q: vec4) -> vec4 { } fn MulQuatVector(q: vec4, v: vec3) -> vec3 { var xyz = vec3(q.x, q.y, q.z); - var t = MulScalar3(Cross3(xyz, v), f32(f32(2)));return v+(MulScalar3(t, q.w))+(Cross3(xyz, t)); + var t = Cross3(xyz, v)*(2);return v+(t*(q.w))+(Cross3(xyz, t)); } fn MulQuatVectorInverse(q: vec4, v: vec3) -> vec3 { var xyz = vec3(q.x, q.y, q.z); - var t = MulScalar3(Cross3(xyz, v), f32(f32(2)));return v-(MulScalar3(t, q.w))+(Cross3(xyz, t)); + var t = Cross3(xyz, v)*(2);return v-(t*(q.w))+(Cross3(xyz, t)); } fn MulQuats(a: vec4,b: vec4) -> vec4 { var q: vec4; @@ -369,10 +373,9 @@ fn QuatAdd(q: vec4, o: vec4) -> vec4 { nq.w += o.w;return nq; } +//////// import: "slmath-vector2.go" + //////// import: "slmath-vector3.go" -fn MulScalar3(v: vec3, s: f32) -> vec3 { - return vec3(v.x*s, v.y*s, v.z*s); -} fn Cross3(v: vec3,o: vec3) -> vec3 { return vec3(v.y*o.z-v.z*o.y, v.z*o.x-v.x*o.z, v.x*o.y-v.y*o.x); } diff --git a/physics/shaders/StepJointForces.wgsl b/physics/shaders/StepJointForces.wgsl index a2fb72d1..f533135f 100644 --- a/physics/shaders/StepJointForces.wgsl +++ b/physics/shaders/StepJointForces.wgsl @@ -48,39 +48,40 @@ const BodyGroup: BodyVars = 3; const BodySizeX: BodyVars = 4; const BodySizeY: BodyVars = 5; const BodySizeZ: BodyVars = 6; -const BodyMass: BodyVars = 7; -const BodyInvMass: BodyVars = 8; -const BodyBounce: BodyVars = 9; -const BodyFriction: BodyVars = 10; -const BodyPosX: BodyVars = 11; -const BodyPosY: BodyVars = 12; -const BodyPosZ: BodyVars = 13; -const BodyQuatX: BodyVars = 14; -const BodyQuatY: BodyVars = 15; -const BodyQuatZ: BodyVars = 16; -const BodyQuatW: BodyVars = 17; -const BodyComX: BodyVars = 18; -const BodyComY: BodyVars = 19; -const BodyComZ: BodyVars = 20; -const BodyInertiaXX: BodyVars = 21; -const BodyInertiaYX: BodyVars = 22; -const BodyInertiaZX: BodyVars = 23; -const BodyInertiaXY: BodyVars = 24; -const BodyInertiaYY: BodyVars = 25; -const BodyInertiaZY: BodyVars = 26; -const BodyInertiaXZ: BodyVars = 27; -const BodyInertiaYZ: BodyVars = 28; -const BodyInertiaZZ: BodyVars = 29; -const BodyInvInertiaXX: BodyVars = 30; -const BodyInvInertiaYX: BodyVars = 31; -const BodyInvInertiaZX: BodyVars = 32; -const BodyInvInertiaXY: BodyVars = 33; -const BodyInvInertiaYY: BodyVars = 34; -const BodyInvInertiaZY: BodyVars = 35; -const BodyInvInertiaXZ: BodyVars = 36; -const BodyInvInertiaYZ: BodyVars = 37; -const BodyInvInertiaZZ: BodyVars = 38; -const BodyRadius: BodyVars = 39; +const BodyThick: BodyVars = 7; +const BodyMass: BodyVars = 8; +const BodyInvMass: BodyVars = 9; +const BodyBounce: BodyVars = 10; +const BodyFriction: BodyVars = 11; +const BodyPosX: BodyVars = 12; +const BodyPosY: BodyVars = 13; +const BodyPosZ: BodyVars = 14; +const BodyQuatX: BodyVars = 15; +const BodyQuatY: BodyVars = 16; +const BodyQuatZ: BodyVars = 17; +const BodyQuatW: BodyVars = 18; +const BodyComX: BodyVars = 19; +const BodyComY: BodyVars = 20; +const BodyComZ: BodyVars = 21; +const BodyInertiaXX: BodyVars = 22; +const BodyInertiaYX: BodyVars = 23; +const BodyInertiaZX: BodyVars = 24; +const BodyInertiaXY: BodyVars = 25; +const BodyInertiaYY: BodyVars = 26; +const BodyInertiaZY: BodyVars = 27; +const BodyInertiaXZ: BodyVars = 28; +const BodyInertiaYZ: BodyVars = 29; +const BodyInertiaZZ: BodyVars = 30; +const BodyInvInertiaXX: BodyVars = 31; +const BodyInvInertiaYX: BodyVars = 32; +const BodyInvInertiaZX: BodyVars = 33; +const BodyInvInertiaXY: BodyVars = 34; +const BodyInvInertiaYY: BodyVars = 35; +const BodyInvInertiaZY: BodyVars = 36; +const BodyInvInertiaXZ: BodyVars = 37; +const BodyInvInertiaYZ: BodyVars = 38; +const BodyInvInertiaZZ: BodyVars = 39; +const BodyRadius: BodyVars = 40; fn BodyCom(idx: i32) -> vec3 { return vec3(Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComX))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComY))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComZ))]); } @@ -159,7 +160,7 @@ fn DynamicQuat(idx: i32,cni: i32) -> vec4 { } //////// import: "enumgen.go" -const BodyVarsN: BodyVars = 40; +const BodyVarsN: BodyVars = 41; const ContactVarsN: ContactVars = 17; const JointControlVarsN: JointControlVars = 3; const DynamicVarsN: DynamicVars = 32; @@ -167,7 +168,7 @@ const GPUVarsN: GPUVars = 10; const JointTypesN: JointTypes = 7; const JointVarsN: JointVars = 50; const JointDoFVarsN: JointDoFVars = 7; -const ShapesN: Shapes = 5; +const ShapesN: Shapes = 6; //////// import: "joint.go" const JointLimitUnlimited = 1e10; @@ -326,15 +327,17 @@ struct GeomData { BodyIdx: i32, Shape: Shapes, MinSize: f32, - Thickness: f32, + Thick: f32, Radius: f32, Size: vec3, - WtoBR: vec3, - WtoBQ: vec4, - BtoWR: vec3, - BtoWQ: vec4, + WbR: vec3, + WbQ: vec4, + BwR: vec3, + BwQ: vec4, } +//////// import: "shapegeom.go" + //////// import: "shapes.go" alias Shapes = i32; //enums:enum const Plane: Shapes = 0; @@ -342,13 +345,14 @@ const Sphere: Shapes = 1; const Capsule: Shapes = 2; const Cylinder: Shapes = 3; const Box: Shapes = 4; +const Cone: Shapes = 5; //////// import: "slmath-matrix3.go" //////// import: "slmath-quaternion.go" fn MulQuatVector(q: vec4, v: vec3) -> vec3 { var xyz = vec3(q.x, q.y, q.z); - var t = MulScalar3(Cross3(xyz, v), f32(f32(2)));return v+(MulScalar3(t, q.w))+(Cross3(xyz, t)); + var t = Cross3(xyz, v)*(2);return v+(t*(q.w))+(Cross3(xyz, t)); } fn MulQuats(a: vec4,b: vec4) -> vec4 { var q: vec4; @@ -365,10 +369,9 @@ fn MulSpatialPoint(xP: vec3, xQ: vec4, p: vec3) -> vec3 { var dp = MulQuatVector(xQ, p);return dp+(xP); } +//////// import: "slmath-vector2.go" + //////// import: "slmath-vector3.go" -fn MulScalar3(v: vec3, s: f32) -> vec3 { - return vec3(v.x*s, v.y*s, v.z*s); -} fn Cross3(v: vec3,o: vec3) -> vec3 { return vec3(v.y*o.z-v.z*o.y, v.z*o.x-v.x*o.z, v.x*o.y-v.y*o.x); } @@ -427,21 +430,21 @@ fn StepJointForces(i: u32) { //gosl:kernel } case Revolute: { var axis = JointAxis(ji, i32(i32(0))); - t = MulScalar3(MulQuatVector(xwPQ, axis), JointControl(ji, i32(i32(0)), JointControlForce)); + t = MulQuatVector(xwPQ, axis)*(JointControl(ji, i32(i32(0)), JointControlForce)); } case Prismatic: { var axis = JointAxis(ji, i32(i32(0))); - f = MulScalar3(MulQuatVector(xwPQ, axis), JointControl(ji, i32(i32(0)), JointControlForce)); + f = MulQuatVector(xwPQ, axis)*(JointControl(ji, i32(i32(0)), JointControlForce)); } default: { for (var dof=0; dof vec3 { return vec3(Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComX))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComY))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComZ))]); } @@ -170,7 +171,7 @@ fn DynamicAngDelta(idx: i32,cni: i32) -> vec3 { } //////// import: "enumgen.go" -const BodyVarsN: BodyVars = 40; +const BodyVarsN: BodyVars = 41; const ContactVarsN: ContactVars = 17; const JointControlVarsN: JointControlVars = 3; const DynamicVarsN: DynamicVars = 32; @@ -178,7 +179,7 @@ const GPUVarsN: GPUVars = 10; const JointTypesN: JointTypes = 7; const JointVarsN: JointVars = 50; const JointDoFVarsN: JointDoFVars = 7; -const ShapesN: Shapes = 5; +const ShapesN: Shapes = 6; //////// import: "joint.go" const JointLimitUnlimited = 1e10; @@ -346,15 +347,17 @@ struct GeomData { BodyIdx: i32, Shape: Shapes, MinSize: f32, - Thickness: f32, + Thick: f32, Radius: f32, Size: vec3, - WtoBR: vec3, - WtoBQ: vec4, - BtoWR: vec3, - BtoWQ: vec4, + WbR: vec3, + WbQ: vec4, + BwR: vec3, + BwQ: vec4, } +//////// import: "shapegeom.go" + //////// import: "shapes.go" alias Shapes = i32; //enums:enum const Plane: Shapes = 0; @@ -362,6 +365,7 @@ const Sphere: Shapes = 1; const Capsule: Shapes = 2; const Cylinder: Shapes = 3; const Box: Shapes = 4; +const Cone: Shapes = 5; //////// import: "slmath-matrix3.go" @@ -387,11 +391,11 @@ fn QuatNormalize(q: vec4) -> vec4 { } fn MulQuatVector(q: vec4, v: vec3) -> vec3 { var xyz = vec3(q.x, q.y, q.z); - var t = MulScalar3(Cross3(xyz, v), f32(f32(2)));return v+(MulScalar3(t, q.w))+(Cross3(xyz, t)); + var t = Cross3(xyz, v)*(2);return v+(t*(q.w))+(Cross3(xyz, t)); } fn MulQuatVectorInverse(q: vec4, v: vec3) -> vec3 { var xyz = vec3(q.x, q.y, q.z); - var t = MulScalar3(Cross3(xyz, v), f32(f32(2)));return v-(MulScalar3(t, q.w))+(Cross3(xyz, t)); + var t = Cross3(xyz, v)*(2);return v-(t*(q.w))+(Cross3(xyz, t)); } fn MulQuats(a: vec4,b: vec4) -> vec4 { var q: vec4; @@ -429,13 +433,9 @@ fn QuatMulScalar(q: vec4, s: f32) -> vec4 { nq.w *= s;return nq; } +//////// import: "slmath-vector2.go" + //////// import: "slmath-vector3.go" -fn MulScalar3(v: vec3, s: f32) -> vec3 { - return vec3(v.x*s, v.y*s, v.z*s); -} -fn DivScalar3(v: vec3, s: f32) -> vec3 { - return vec3(v.x/s, v.y/s, v.z/s); -} fn DivSafe3(v: vec3, o: vec3) -> vec3 { var nv = v; if (o.x != 0) { @@ -470,7 +470,7 @@ fn Abs3(v: vec3) -> vec3 { return vec3(abs(v.x), abs(v.y), abs(v.z)); } fn Normal3(v: vec3) -> vec3 { - return DivScalar3(v, Length3(v)); + return v/(Length3(v)); } fn Cross3(v: vec3,o: vec3) -> vec3 { return vec3(v.y*o.z-v.z*o.y, v.z*o.x-v.x*o.z, v.x*o.y-v.y*o.x); diff --git a/physics/shapecollide.go b/physics/shapecollide.go index 0dfa5636..34a7e3c0 100644 --- a/physics/shapecollide.go +++ b/physics/shapecollide.go @@ -1,3 +1,5 @@ +// Code generated by "goal build"; DO NOT EDIT. +//line shapecollide.goal:1 // Copyright (c) 2025, Cogent Core. All rights reserved. // Use of this source code is governed by a BSD-style // license that can be found in the LICENSE file. @@ -22,7 +24,8 @@ type GeomData struct { // MinSize is the min of the Size dimensions. MinSize float32 - Thickness float32 + // Thickness of shape. + Thick float32 // Radius is the effective radius for sphere-like elements (Sphere, Capsule, Cone) Radius float32 @@ -31,15 +34,15 @@ type GeomData struct { // World-to-Body transform // Position (R) (i.e., BodyPos) - WtoBR math32.Vector3 + WbR math32.Vector3 // Quaternion (Q) (i.e., BodyQuat) - WtoBQ math32.Quat + WbQ math32.Quat // Body-to-World transform (inverse) // Position (R) - BtoWR math32.Vector3 + BwR math32.Vector3 // Quaternion (Q) - BtoWQ math32.Quat + BwQ math32.Quat } func NewGeomData(bi, cni int32, shp Shapes) GeomData { @@ -47,15 +50,16 @@ func NewGeomData(bi, cni int32, shp Shapes) GeomData { gd.BodyIdx = bi gd.Shape = shp gd.Size = BodySize(bi) + gd.Thick = Bodies.Value(int(bi), int(BodyThick)) gd.MinSize = min(gd.Size.X, gd.Size.Y) gd.MinSize = min(gd.MinSize, gd.Size.Z) - gd.WtoBR = BodyDynamicPos(bi, cni) - gd.WtoBQ = BodyDynamicQuat(bi, cni) + gd.WbR = BodyDynamicPos(bi, cni) + gd.WbQ = BodyDynamicQuat(bi, cni) var bwR math32.Vector3 var bwQ math32.Quat - slmath.SpatialTransformInverse(gd.WtoBR, gd.WtoBQ, &bwR, &bwQ) - gd.BtoWR = bwR - gd.BtoWQ = bwQ + slmath.SpatialTransformInverse(gd.WbR, gd.WbQ, &bwR, &bwQ) + gd.BwR = bwR + gd.BwQ = bwQ gd.Radius = 0 if shp == Sphere || shp == Capsule { // todo: cone is separate gd.Radius = gd.Size.X @@ -63,15 +67,60 @@ func NewGeomData(bi, cni int32, shp Shapes) GeomData { return gd } -/////// Collision methods: +/////// Collision methods: in geometry/kernels.py // note: have to pass a non-pointer arg as first arg, due to gosl issue. +// cpi = contact point index. -func ColSphereSphere(cni int32, gdA *GeomData, gdB *GeomData, ptA, ptB, norm *math32.Vector3) float32 { - *ptA = gdA.WtoBR - *ptB = gdB.WtoBR - diff := (*ptA).Sub(*ptB) +// X_wb, X_ws -> WtoB +// X_bw, X_sw -> BtoW + +// ptAw = point in A, world coords; b = body coords +// spatial_vector = w,v = top, bottom = delta, angDelta + +func ColSphereSphere(cpi int32, gdA *GeomData, gdB *GeomData, ptA, ptB, norm *math32.Vector3) float32 { + ptAw := gdA.WbR + ptBw := gdB.WbR + diff := ptAw.Sub(ptBw) + *ptA = ptAw + *ptB = ptBw *norm = slmath.Normal3(diff) return slmath.Dot3(diff, *norm) } +func ColCapsulePlane(cpi int32, gdA *GeomData, gdB *GeomData, ptA, ptB, norm *math32.Vector3) float32 { + var ptAw, ptBw, diff math32.Vector3 + hh := gdA.Size.Y + if cpi < 2 { // vertex + side := float32(cpi)*2 - 1 + ptAw = slmath.MulSpatialPoint(gdA.WbR, gdA.WbQ, math32.Vec3(0, side*hh, 0)) + queryB := slmath.MulSpatialPoint(gdB.BwR, gdB.BwQ, ptAw) + ptBb := ClosestPointPlane(gdB.Size.X, gdB.Size.Z, queryB) + ptBw = slmath.MulSpatialPoint(gdA.WbR, gdA.WbQ, ptBb) + diff = ptAw.Sub(ptBw) + if gdB.Size.X > 0 { + *norm = slmath.Normal3(diff) + } else { + *norm = slmath.MulSpatialPoint(gdB.WbR, gdB.WbQ, math32.Vec3(0, 1, 0)) + } + } else { // edges of finite plane -- only here if plane is finite + var edge0, edge1 math32.Vector3 + PlaneEdge(cpi-2, gdB.Size.X, gdB.Size.Z, &edge0, &edge1) + edge0w := slmath.MulSpatialPoint(gdB.WbR, gdB.WbQ, edge0) + edge1w := slmath.MulSpatialPoint(gdB.WbR, gdB.WbQ, edge1) + edge0a := slmath.MulSpatialPoint(gdA.BwR, gdA.BwQ, edge0w) + edge1a := slmath.MulSpatialPoint(gdA.BwR, gdA.BwQ, edge1w) + u := ClosestEdgeCapsule(gdA.Size.X, gdA.Size.Y, edge0a, edge1a, 10) // todo: edge_sdf_iter + ptBw = edge0w.MulScalar(1 - u).Add(edge1w.MulScalar(u)) + // find closest point + contact normal on capsule A + pt0Aw := slmath.MulSpatialPoint(gdA.WbR, gdA.WbQ, math32.Vec3(0, hh, 0)) + pt1Aw := slmath.MulSpatialPoint(gdA.WbR, gdA.WbQ, math32.Vec3(0, -hh, 0)) + ptAw = ClosestPointLineSegment(pt0Aw, pt1Aw, ptBw) + diff = ptAw.Sub(ptBw) + *norm = slmath.MulSpatialPoint(gdB.WbR, gdB.WbQ, math32.Vec3(0, 1, 0)) + } + *ptA = ptAw + *ptB = ptBw + return slmath.Dot3(diff, *norm) +} + //gosl:end diff --git a/physics/shapecollide.goal b/physics/shapecollide.goal new file mode 100644 index 00000000..7a329f20 --- /dev/null +++ b/physics/shapecollide.goal @@ -0,0 +1,124 @@ +// Copyright (c) 2025, Cogent Core. All rights reserved. +// Use of this source code is governed by a BSD-style +// license that can be found in the LICENSE file. + +package physics + +import ( + "cogentcore.org/core/math32" + "cogentcore.org/lab/gosl/slmath" +) + +//gosl:start + +// newton: geometry/kernels.py class GeoData + +// GeomData contains all geometric data for narrow-phase collision. +type GeomData struct { + BodyIdx int32 + + Shape Shapes + + // MinSize is the min of the Size dimensions. + MinSize float32 + + // Thickness of shape. + Thick float32 + + // Radius is the effective radius for sphere-like elements (Sphere, Capsule, Cone) + Radius float32 + + Size math32.Vector3 + + // World-to-Body transform + // Position (R) (i.e., BodyPos) + WbR math32.Vector3 + // Quaternion (Q) (i.e., BodyQuat) + WbQ math32.Quat + + // Body-to-World transform (inverse) + // Position (R) + BwR math32.Vector3 + // Quaternion (Q) + BwQ math32.Quat +} + +func NewGeomData(bi, cni int32, shp Shapes) GeomData { + var gd GeomData + gd.BodyIdx = bi + gd.Shape = shp + gd.Size = BodySize(bi) + gd.Thick = Bodies[bi, BodyThick] + gd.MinSize = min(gd.Size.X, gd.Size.Y) + gd.MinSize = min(gd.MinSize, gd.Size.Z) + gd.WbR = BodyDynamicPos(bi, cni) + gd.WbQ = BodyDynamicQuat(bi, cni) + var bwR math32.Vector3 + var bwQ math32.Quat + slmath.SpatialTransformInverse(gd.WbR, gd.WbQ, &bwR, &bwQ) + gd.BwR = bwR + gd.BwQ = bwQ + gd.Radius = 0 + if shp == Sphere || shp == Capsule { // todo: cone is separate + gd.Radius = gd.Size.X + } + return gd +} + +/////// Collision methods: in geometry/kernels.py +// note: have to pass a non-pointer arg as first arg, due to gosl issue. +// cpi = contact point index. + +// X_wb, X_ws -> WtoB +// X_bw, X_sw -> BtoW + +// ptAw = point in A, world coords; b = body coords +// spatial_vector = w,v = top, bottom = delta, angDelta + +func ColSphereSphere(cpi int32, gdA *GeomData, gdB *GeomData, ptA, ptB, norm *math32.Vector3) float32 { + ptAw := gdA.WbR + ptBw := gdB.WbR + diff := ptAw.Sub(ptBw) + *ptA = ptAw + *ptB = ptBw + *norm = slmath.Normal3(diff) + return slmath.Dot3(diff, *norm) +} + +func ColCapsulePlane(cpi int32, gdA *GeomData, gdB *GeomData, ptA, ptB, norm *math32.Vector3) float32 { + var ptAw, ptBw, diff math32.Vector3 + hh := gdA.Size.Y + if cpi < 2 { // vertex + side := float32(cpi)*2 - 1 + ptAw = slmath.MulSpatialPoint(gdA.WbR, gdA.WbQ, math32.Vec3(0, side*hh, 0)) + queryB := slmath.MulSpatialPoint(gdB.BwR, gdB.BwQ, ptAw) + ptBb := ClosestPointPlane(gdB.Size.X, gdB.Size.Z, queryB) + ptBw = slmath.MulSpatialPoint(gdA.WbR, gdA.WbQ, ptBb) + diff = ptAw.Sub(ptBw) + if gdB.Size.X > 0 { + *norm = slmath.Normal3(diff) + } else { + *norm = slmath.MulSpatialPoint(gdB.WbR, gdB.WbQ, math32.Vec3(0, 1, 0)) + } + } else { // edges of finite plane -- only here if plane is finite + var edge0, edge1 math32.Vector3 + PlaneEdge(cpi-2, gdB.Size.X, gdB.Size.Z, &edge0, &edge1) + edge0w := slmath.MulSpatialPoint(gdB.WbR, gdB.WbQ, edge0) + edge1w := slmath.MulSpatialPoint(gdB.WbR, gdB.WbQ, edge1) + edge0a := slmath.MulSpatialPoint(gdA.BwR, gdA.BwQ, edge0w) + edge1a := slmath.MulSpatialPoint(gdA.BwR, gdA.BwQ, edge1w) + u := ClosestEdgeCapsule(gdA.Size.X, gdA.Size.Y, edge0a, edge1a, 10) // todo: edge_sdf_iter + ptBw = edge0w.MulScalar(1 - u).Add(edge1w.MulScalar(u)) + // find closest point + contact normal on capsule A + pt0Aw := slmath.MulSpatialPoint(gdA.WbR, gdA.WbQ, math32.Vec3(0, hh, 0)) + pt1Aw := slmath.MulSpatialPoint(gdA.WbR, gdA.WbQ, math32.Vec3(0, -hh, 0)) + ptAw = ClosestPointLineSegment(pt0Aw, pt1Aw, ptBw) + diff = ptAw.Sub(ptBw) + *norm = slmath.MulSpatialPoint(gdB.WbR, gdB.WbQ, math32.Vec3(0, 1, 0)) + } + *ptA = ptAw + *ptB = ptBw + return slmath.Dot3(diff, *norm) +} + +//gosl:end diff --git a/physics/shapegeom.go b/physics/shapegeom.go new file mode 100644 index 00000000..f07e542f --- /dev/null +++ b/physics/shapegeom.go @@ -0,0 +1,282 @@ +// Copyright (c) 2025, Cogent Core. All rights reserved. +// Use of this source code is governed by a BSD-style +// license that can be found in the LICENSE file. + +package physics + +import ( + "cogentcore.org/core/math32" + "cogentcore.org/lab/gosl/slmath" +) + +//gosl:start + +func SphereSDF(center math32.Vector3, radius float32, p math32.Vector3) float32 { + return slmath.Length3(p.Sub(center)) - radius +} + +func BoxSDF(upper, p math32.Vector3) float32 { + // adapted from https://www.iquilezles.org/www/articles/distfunctions/distfunctions.htm + qx := math32.Abs(p.X) - upper.X + qy := math32.Abs(p.Y) - upper.Y + qz := math32.Abs(p.Z) - upper.Z + e := math32.Vec3(max(qx, 0.0), max(qy, 0.0), max(qz, 0.0)) + return slmath.Length3(e) + min(max(qx, max(qy, qz)), 0.0) +} + +func CapsuleSDF(radius, hh float32, p math32.Vector3) float32 { + if p.Y > hh { + return slmath.Length3(math32.Vec3(p.X, p.Y-hh, p.Z)) - radius + } + if p.Y < -hh { + return slmath.Length3(math32.Vec3(p.X, p.Y+hh, p.Z)) - radius + } + return slmath.Length3(math32.Vec3(p.X, 0.0, p.Z)) - radius +} + +func CylinderSDF(radius, hh float32, p math32.Vector3) float32 { + dx := slmath.Length3(math32.Vec3(p.X, 0.0, p.Z)) - radius + dy := math32.Abs(p.Y) - hh + return min(max(dx, dy), 0.0) + slmath.Length2(math32.Vec2(max(dx, 0.0), max(dy, 0.0))) +} + +// Cone with apex at +hh and base at -hh +func ConeSDF(radius, hh float32, p math32.Vector3) float32 { + dx := slmath.Length3(math32.Vec3(p.X, 0.0, p.Z)) - radius*(hh-p.Y)/(2.0*hh) + dy := math32.Abs(p.Y) - hh + return min(max(dx, dy), 0.0) + slmath.Length2(math32.Vec2(max(dx, 0.0), max(dy, 0.0))) +} + +// SDF for a quad in the xz plane +func PlaneSDF(width, length float32, p math32.Vector3) float32 { + if width > 0.0 && length > 0.0 { + d := max(math32.Abs(p.X)-width, math32.Abs(p.Z)-length) + return max(d, math32.Abs(p.Y)) + } + return p.Y +} + +// ClosestPointPlane projects the point onto the quad in +// the xz plane (if size > 0.0, otherwise infinite. +func ClosestPointPlane(width, length float32, pt math32.Vector3) math32.Vector3 { + cp := pt + if width == 0.0 { + cp.Y = 0 + return cp + } + cp.X = math32.Clamp(pt.X, -width, width) + cp.Z = math32.Clamp(pt.Z, -length, length) + return cp +} + +func ClosestPointLineSegment(a, b, pt math32.Vector3) math32.Vector3 { + ab := b.Sub(a) + ap := pt.Sub(a) + t := slmath.Dot3(ap, ab) / slmath.Dot3(ab, ab) + t = math32.Clamp(t, 0.0, 1.0) + return a.Add(ab.MulScalar(t)) +} + +// closest point to box surface +func ClosestPointBox(upper, pt math32.Vector3) math32.Vector3 { + x := math32.Clamp(pt.X, -upper.X, upper.X) + y := math32.Clamp(pt.Y, -upper.Y, upper.Y) + z := math32.Clamp(pt.Z, -upper.Z, upper.Z) + if math32.Abs(pt.X) <= upper.X && math32.Abs(pt.Y) <= upper.Y && math32.Abs(pt.Z) <= upper.Z { + // the point is inside, find closest face + sx := math32.Abs(math32.Abs(pt.X) - upper.X) + sy := math32.Abs(math32.Abs(pt.Y) - upper.Y) + sz := math32.Abs(math32.Abs(pt.Z) - upper.Z) + // return closest point on closest side, handle corner cases + if (sx < sy && sx < sz) || (sy == 0.0 && sz == 0.0) { + x = math32.Sign(pt.X) * upper.X + } else if (sy < sx && sy < sz) || (sx == 0.0 && sz == 0.0) { + y = math32.Sign(pt.Y) * upper.Y + } else { + z = math32.Sign(pt.Z) * upper.Z + } + } + return math32.Vec3(x, y, z) +} + +// box vertex numbering: +// +// 6---7 +// |\ |\ y +// | 2-+-3 | +// 4-+-5 | z \| +// \| \| o---x +// 0---1 +// +// get the vertex of the box given its ID (0-7) +func BoxVertex(ptId int32, upper math32.Vector3) math32.Vector3 { + sign_x := float32(ptId%2)*2.0 - 1.0 + sign_y := float32((ptId/2)%2)*2.0 - 1.0 + sign_z := float32((ptId/4)%2)*2.0 - 1.0 + return math32.Vec3(sign_x*upper.X, sign_y*upper.Y, sign_z*upper.Z) +} + +// get the edge of the box given its ID (0-11) +func BoxEdge(edgeId int32, upper math32.Vector3, edge0, edge1 *math32.Vector3) { + if edgeId < 4 { + // edges along x: 0-1, 2-3, 4-5, 6-7 + i := edgeId * 2 + j := i + 1 + *edge0 = BoxVertex(i, upper) + *edge1 = BoxVertex(j, upper) + } else if edgeId < 8 { + // edges along y: 0-2, 1-3, 4-6, 5-7 + edgeId -= 4 + i := edgeId%2 + edgeId // 2 * 4 + j := i + 2 + *edge0 = BoxVertex(i, upper) + *edge1 = BoxVertex(j, upper) + } + // edges along z: 0-4, 1-5, 2-6, 3-7 + edgeId -= 8 + i := edgeId + j := i + 4 + *edge0 = BoxVertex(i, upper) + *edge1 = BoxVertex(j, upper) +} + +// get the edge of the plane given its ID (0-3) +func PlaneEdge(edgeId int32, width, length float32, edge0, edge1 *math32.Vector3) { + p0x := (2*float32(edgeId%2) - 1) * width + p0z := (2*float32(edgeId/2) - 1) * length + var p1x, p1z float32 + if edgeId == 0 || edgeId == 3 { + p1x = p0x + p1z = -p0z + } else { + p1x = -p0x + p1z = p0z + } + (*edge0).Set(p0x, 0, p0z) + (*edge1).Set(p1x, 0, p1z) +} + +// find point on edge closest to box, return its barycentric edge coordinate +func ClosestEdgeBox(upper, edgeA, edgeB math32.Vector3, maxIter int32) float32 { + // Golden-section search + a := float32(0.0) + b := float32(1.0) + h := b - a + invphi := float32(0.61803398875) // 1 / phi + invphi2 := float32(0.38196601125) // 1 / phi^2 + c := a + invphi2*h + d := a + invphi*h + query := edgeA.MulScalar(1.0 - c).Add(edgeB.MulScalar(c)) + yc := BoxSDF(upper, query) + query = edgeA.MulScalar(1.0 - d).Add(edgeB.MulScalar(d)) + yd := BoxSDF(upper, query) + + for range maxIter { + if yc < yd { // yc > yd to find the maximum + b = d + d = c + yd = yc + h = invphi * h + c = a + invphi2*h + query = edgeA.MulScalar(1.0 - c).Add(edgeB.MulScalar(c)) + yc = BoxSDF(upper, query) + } else { + a = c + c = d + yc = yd + h = invphi * h + d = a + invphi*h + query = edgeA.MulScalar(1.0 - d).Add(edgeB.MulScalar(d)) + yd = BoxSDF(upper, query) + } + } + + if yc < yd { + return 0.5 * (a + d) + } + return 0.5 * (c + b) +} + +// find point on edge closest to plane, return its barycentric edge coordinate +func ClosestEdgePlane(width, length float32, edgeA, edgeB math32.Vector3, maxIter int32) float32 { + // Golden-section search + a := float32(0.0) + b := float32(1.0) + h := b - a + invphi := float32(0.61803398875) // 1 / phi + invphi2 := float32(0.38196601125) // 1 / phi^2 + c := a + invphi2*h + d := a + invphi*h + query := edgeA.MulScalar(1.0 - c).Add(edgeB.MulScalar(c)) + yc := PlaneSDF(width, length, query) + query = edgeA.MulScalar(1.0 - d).Add(edgeB.MulScalar(d)) + yd := PlaneSDF(width, length, query) + + for range maxIter { + if yc < yd { // yc > yd to find the maximum + b = d + d = c + yd = yc + h = invphi * h + c = a + invphi2*h + query = edgeA.MulScalar(1.0 - c).Add(edgeB.MulScalar(c)) + yc = PlaneSDF(width, length, query) + } else { + a = c + c = d + yc = yd + h = invphi * h + d = a + invphi*h + query = edgeA.MulScalar(1.0 - d).Add(edgeB.MulScalar(d)) + yd = PlaneSDF(width, length, query) + } + } + + if yc < yd { + return 0.5 * (a + d) + } + return 0.5 * (c + b) +} + +// find point on edge closest to capsule, return its barycentric edge coordinate +func ClosestEdgeCapsule(radius, hh float32, edgeA, edgeB math32.Vector3, maxIter int32) float32 { + // Golden-section search + a := float32(0.0) + b := float32(1.0) + h := b - a + invphi := float32(0.61803398875) // 1 / phi + invphi2 := float32(0.38196601125) // 1 / phi^2 + c := a + invphi2*h + d := a + invphi*h + query := edgeA.MulScalar(1.0 - c).Add(edgeB.MulScalar(c)) + yc := CylinderSDF(radius, hh, query) + query = edgeA.MulScalar(1.0 - d).Add(edgeB.MulScalar(d)) + yd := CylinderSDF(radius, hh, query) + + for range maxIter { + if yc < yd { // yc > yd to find the maximum + b = d + d = c + yd = yc + h = invphi * h + c = a + invphi2*h + query = edgeA.MulScalar(1.0 - c).Add(edgeB.MulScalar(c)) + yc = CylinderSDF(radius, hh, query) + } else { + a = c + c = d + yc = yd + h = invphi * h + d = a + invphi*h + query = edgeA.MulScalar(1.0 - d).Add(edgeB.MulScalar(d)) + yd = CylinderSDF(radius, hh, query) + } + } + + if yc < yd { + return 0.5 * (a + d) + } + return 0.5 * (c + b) +} + +//gosl:end diff --git a/physics/shapes.go b/physics/shapes.go index 63c71a3e..5269dad5 100644 --- a/physics/shapes.go +++ b/physics/shapes.go @@ -23,30 +23,38 @@ type Shapes int32 //enums:enum const ( // Plane cannot be a dynamic shape, but is most efficient for // collision computations. Use size = 0 for an infinite plane. + // Natively extends in the X-Z plane: SizeX x SizeZ. Plane Shapes = iota + // todo: HeightField here (terrain) + // Sphere. SizeX is the radius. Sphere - // Capsule, which is a cylinder with half-spheres on the ends. + // Capsule is a cylinder with half-spheres on the ends. // Natively oriented vertically along the Y axis. - // SizeX = bottom radius, SizeY = half-height, SizeZ = top radius. + // SizeX = radius, SizeY = half-height. Capsule // todo: Ellipsoid goes here // Cylinder, natively oriented vertically along the Y axis. - // If one radius is 0, then it is a cone. - // SizeX = bottom radius, SizeY = half-height in Y axis, SizeZ = top radius. + // SizeX = radius, SizeY = half-height in Y axis. // Cylinder can not collide with a Box. Cylinder - // todo: add separate Cone - // Box is a 3D rectalinear shape. // The sizes are _half_ sizes along each dimension, // relative to the center. Box + + // todo: Mesh, SDF here + + // Cone is like a cylinder with the top radius = 0, + // oriented up. SizeX = bottom radius, SizeY = half-height in Y. + // Cone does not support any collisions and is not recommended for + // interacting bodies. + Cone ) // newton: geometry/kernels.py: count_contact_points_for_pair @@ -97,7 +105,7 @@ func ShapePairContacts(a, b Shapes, infPlane bool, ba *int32) int32 { case Box: *ba = 12 return 12 - default: + default: // note: Cone has no collision points! return 0 } } diff --git a/physics/step_joint.go b/physics/step_joint.go index 15c9e640..2f6ecbfd 100644 --- a/physics/step_joint.go +++ b/physics/step_joint.go @@ -88,19 +88,19 @@ func StepJointForces(i uint32) { //gosl:kernel t = math32.Vec3(JointControl(ji, 0, JointControlForce), JointControl(ji, 1, JointControlForce), JointControl(ji, 2, JointControlForce)) case Revolute: axis := JointAxis(ji, 0) - t = slmath.MulScalar3(slmath.MulQuatVector(xwPQ, axis), JointControl(ji, 0, JointControlForce)) + t = slmath.MulQuatVector(xwPQ, axis).MulScalar(JointControl(ji, 0, JointControlForce)) case Prismatic: axis := JointAxis(ji, 0) - f = slmath.MulScalar3(slmath.MulQuatVector(xwPQ, axis), JointControl(ji, 0, JointControlForce)) + f = slmath.MulQuatVector(xwPQ, axis).MulScalar(JointControl(ji, 0, JointControlForce)) default: for dof := range jLinearN { axis := JointAxis(ji, int32(dof)) - f = f.Add(slmath.MulScalar3(slmath.MulQuatVector(xwPQ, axis), JointControl(ji, int32(dof), JointControlForce))) + f = f.Add(slmath.MulQuatVector(xwPQ, axis).MulScalar(JointControl(ji, int32(dof), JointControlForce))) } for dof := range jAngularN { di := int32(jLinearN) + int32(dof) axis := JointAxis(ji, di) - t = t.Add(slmath.MulScalar3(slmath.MulQuatVector(xwPQ, axis), JointControl(ji, di, JointControlForce))) + t = t.Add(slmath.MulQuatVector(xwPQ, axis).MulScalar(JointControl(ji, di, JointControlForce))) } } // These are unique to joint: aggregate into dynamics Next in [ForcesFromJoints] diff --git a/physics/step_joint.goal b/physics/step_joint.goal index 7d556bff..bc7b2996 100644 --- a/physics/step_joint.goal +++ b/physics/step_joint.goal @@ -86,19 +86,19 @@ func StepJointForces(i uint32) { //gosl:kernel t = math32.Vec3(JointControl(ji, 0, JointControlForce), JointControl(ji, 1, JointControlForce), JointControl(ji, 2, JointControlForce)) case Revolute: axis := JointAxis(ji, 0) - t = slmath.MulScalar3(slmath.MulQuatVector(xwPQ, axis), JointControl(ji, 0, JointControlForce)) + t = slmath.MulQuatVector(xwPQ, axis).MulScalar(JointControl(ji, 0, JointControlForce)) case Prismatic: axis := JointAxis(ji, 0) - f = slmath.MulScalar3(slmath.MulQuatVector(xwPQ, axis), JointControl(ji, 0, JointControlForce)) + f = slmath.MulQuatVector(xwPQ, axis).MulScalar(JointControl(ji, 0, JointControlForce)) default: for dof := range jLinearN { axis := JointAxis(ji, int32(dof)) - f = f.Add(slmath.MulScalar3(slmath.MulQuatVector(xwPQ, axis), JointControl(ji, int32(dof), JointControlForce))) + f = f.Add(slmath.MulQuatVector(xwPQ, axis).MulScalar(JointControl(ji, int32(dof), JointControlForce))) } for dof := range jAngularN { di := int32(jLinearN) + int32(dof) axis := JointAxis(ji, di) - t = t.Add(slmath.MulScalar3(slmath.MulQuatVector(xwPQ, axis), JointControl(ji, di, JointControlForce))) + t = t.Add(slmath.MulQuatVector(xwPQ, axis).MulScalar(JointControl(ji, di, JointControlForce))) } } // These are unique to joint: aggregate into dynamics Next in [ForcesFromJoints] diff --git a/physics/typegen.go b/physics/typegen.go index 149ebb39..72b3a769 100644 --- a/physics/typegen.go +++ b/physics/typegen.go @@ -26,7 +26,7 @@ var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.JointDoFVars var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.PhysParams", IDName: "phys-params", Doc: "PhysParams are the physics parameters", Directives: []types.Directive{{Tool: "gosl", Directive: "start"}}, Fields: []types.Field{{Name: "Iterations", Doc: "Iterations is the number of iterations to perform."}, {Name: "Dt", Doc: "Dt is the integration stepsize."}, {Name: "SoftRelax", Doc: "SoftRelax is soft-body relaxation constant."}, {Name: "JointLinearRelax", Doc: "JointLinearRelax is joint linear relaxation constant."}, {Name: "JointAngularRelax", Doc: "JointAngularRelax is joint angular relaxation constant."}, {Name: "JointLinearComply", Doc: "JointLinearComply is joint linear compliance constant."}, {Name: "JointAngularComply", Doc: "JointAngularComply is joint angular compliance constant."}, {Name: "ContactRelax", Doc: "ContactRelax is rigid contact relaxation constant."}, {Name: "AngularDamping", Doc: "AngularDamping is damping of angular motion."}, {Name: "ContactWeighting", Doc: "Contact weighting"}, {Name: "Restitution", Doc: "Restitution"}, {Name: "ContactMargin", Doc: "Contact margin is the extra distance for broadphase collision\naround rigid bodies."}, {Name: "ContactsMax", Doc: "Maximum number of contacts to process at any given point."}, {Name: "Cur", Doc: "Index for the current state (0 or 1, alternates with Next)."}, {Name: "Next", Doc: "Index for the next state (1 or 0, alternates with Cur)."}, {Name: "BodiesN", Doc: "BodiesN is number of rigid bodies."}, {Name: "DynamicsN", Doc: "DynamicsN is number of dynamics bodies."}, {Name: "JointsN", Doc: "JointsN is number of joints."}, {Name: "JointDoFsN", Doc: "JointDoFsN is number of joint DoFs."}, {Name: "BodyJointsMax", Doc: "BodyJointsMax is max number of joints per body + 1 for actual n."}, {Name: "BodyCollidePairsN", Doc: "BodyCollidePairsN is the total number of pre-compiled collision pairs\nto examine."}, {Name: "pad"}, {Name: "pad1"}, {Name: "pad2"}, {Name: "Gravity", Doc: "Gravity is the gravity acceleration function"}}}) -var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.GeomData", IDName: "geom-data", Doc: "GeomData contains all geometric data for narrow-phase collision.", Directives: []types.Directive{{Tool: "gosl", Directive: "start"}}, Fields: []types.Field{{Name: "BodyIdx"}, {Name: "Shape"}, {Name: "MinSize", Doc: "MinSize is the min of the Size dimensions."}, {Name: "Thickness"}, {Name: "Radius", Doc: "Radius is the effective radius for sphere-like elements (Sphere, Capsule, Cone)"}, {Name: "Size"}, {Name: "WtoBR", Doc: "World-to-Body transform\nPosition (R) (i.e., BodyPos)"}, {Name: "WtoBQ", Doc: "Quaternion (Q) (i.e., BodyQuat)"}, {Name: "BtoWR", Doc: "Body-to-World transform (inverse)\nPosition (R)"}, {Name: "BtoWQ", Doc: "Quaternion (Q)"}}}) +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.GeomData", IDName: "geom-data", Doc: "GeomData contains all geometric data for narrow-phase collision.", Directives: []types.Directive{{Tool: "gosl", Directive: "start"}}, Fields: []types.Field{{Name: "BodyIdx"}, {Name: "Shape"}, {Name: "MinSize", Doc: "MinSize is the min of the Size dimensions."}, {Name: "Thick", Doc: "Thickness of shape."}, {Name: "Radius", Doc: "Radius is the effective radius for sphere-like elements (Sphere, Capsule, Cone)"}, {Name: "Size"}, {Name: "WbR", Doc: "World-to-Body transform\nPosition (R) (i.e., BodyPos)"}, {Name: "WbQ", Doc: "Quaternion (Q) (i.e., BodyQuat)"}, {Name: "BwR", Doc: "Body-to-World transform (inverse)\nPosition (R)"}, {Name: "BwQ", Doc: "Quaternion (Q)"}}}) var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.Shapes", IDName: "shapes", Doc: "Shapes are elemental shapes for rigid bodies.\nIn general, size dimensions are half values\n(e.g., radius, half-height, etc), which is natural for\ncenter-based body coordinates."}) From 7ecbfc2bcba8a22319378e532f1956ac269aec2a Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Sun, 21 Dec 2025 06:43:04 +0100 Subject: [PATCH 33/97] physics: minor --- physics/shapecollide.goal | 1 - 1 file changed, 1 deletion(-) diff --git a/physics/shapecollide.goal b/physics/shapecollide.goal index 7a329f20..e374a6dc 100644 --- a/physics/shapecollide.goal +++ b/physics/shapecollide.goal @@ -73,7 +73,6 @@ func NewGeomData(bi, cni int32, shp Shapes) GeomData { // X_bw, X_sw -> BtoW // ptAw = point in A, world coords; b = body coords -// spatial_vector = w,v = top, bottom = delta, angDelta func ColSphereSphere(cpi int32, gdA *GeomData, gdB *GeomData, ptA, ptB, norm *math32.Vector3) float32 { ptAw := gdA.WbR From 1e704baf8e45d280ff6dd296ef5e9ce9d86b75e7 Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Sun, 21 Dec 2025 14:49:07 +0100 Subject: [PATCH 34/97] physics: all of the narrow collision logic in place and building.. tests to follow! --- gosl/gotosl/nodes.go | 10 + gosl/gotosl/testdata/basic.goal | 8 +- physics/body.go | 20 +- physics/body.goal | 20 +- physics/contact.go | 178 ++++++- physics/contact.goal | 177 ++++++- physics/enumgen.go | 26 +- physics/gosl.go | 116 ++++- physics/params.go | 7 +- physics/shaders/CollisionBroad.wgsl | 75 +-- physics/shaders/CollisionNarrow.wgsl | 626 +++++++++++++++++++++-- physics/shaders/DeltasFromJoints.wgsl | 35 +- physics/shaders/DynamicsCurToNext.wgsl | 35 +- physics/shaders/ForcesFromJoints.wgsl | 35 +- physics/shaders/InitDynamics.wgsl | 35 +- physics/shaders/StepBodyDeltas.wgsl | 35 +- physics/shaders/StepIntegrateBodies.wgsl | 35 +- physics/shaders/StepJointForces.wgsl | 37 +- physics/shaders/StepSolveJoints.wgsl | 37 +- physics/shapecollide.go | 341 +++++++++++- physics/shapecollide.goal | 339 +++++++++++- physics/shapegeom.go | 51 +- physics/shapegeom_test.go | 176 +++++++ physics/typegen.go | 4 +- physics/vars.go | 22 +- physics/world.go | 38 +- physics/world.goal | 38 +- 27 files changed, 2161 insertions(+), 395 deletions(-) create mode 100644 physics/shapegeom_test.go diff --git a/gosl/gotosl/nodes.go b/gosl/gotosl/nodes.go index 60457347..fd4dfa6f 100644 --- a/gosl/gotosl/nodes.go +++ b/gosl/gotosl/nodes.go @@ -2541,6 +2541,16 @@ func (p *printer) stmt(stmt ast.Stmt, nextIsRBrace bool, nosemi bool) { // p.print(blank) // p.setPos(s.TokPos) // p.print(s.Tok, blank) + } else { + p.print(token.LPAREN, "var", blank) + p.print("i") + p.print(token.ASSIGN, "0", token.SEMICOLON, blank) + p.print("i") + p.print(token.LSS) + p.expr(stripParens(s.X)) + p.print(token.SEMICOLON, blank) + p.print("i") + p.print(token.INC, token.RPAREN) } // p.print(token.RANGE, blank) // p.expr(stripParens(s.X)) diff --git a/gosl/gotosl/testdata/basic.goal b/gosl/gotosl/testdata/basic.goal index 77a5df1e..fc55e624 100644 --- a/gosl/gotosl/testdata/basic.goal +++ b/gosl/gotosl/testdata/basic.goal @@ -206,8 +206,12 @@ func (ps *ParamStruct) AnotherMeth(ctx *Context, idx int, ptrarg *float32) { Data[idx, Exp] = ps.Subs.SumPlus(b) Data[idx, Integ] = a - for i := range 10 { - _ = i + // for i := range 10 { + // _ = i + // Data[idx, Exp] *= 0.99 + // } + + for range 10 { Data[idx, Exp] *= 0.99 } diff --git a/physics/body.go b/physics/body.go index a18f6984..b4e7487d 100644 --- a/physics/body.go +++ b/physics/body.go @@ -45,10 +45,10 @@ const ( // does not need to be handled here. BodyGroup - // BodySize is the size of the object (values depend on shape type). - BodySizeX - BodySizeY - BodySizeZ + // BodyHSize is the size of the object (values depend on shape type). + BodyHSizeX + BodyHSizeY + BodyHSizeZ // BodyThick is the thickness of the body, as a hollow shape. // If 0, then it is a solid shape (default). @@ -145,14 +145,14 @@ func GetBodyGroup(idx int32) int32 { return int32(math.Float32bits(Bodies.Value(int(idx), int(BodyGroup)))) } -func BodySize(idx int32) math32.Vector3 { - return math32.Vec3(Bodies.Value(int(idx), int(BodySizeX)), Bodies.Value(int(idx), int(BodySizeY)), Bodies.Value(int(idx), int(BodySizeZ))) +func BodyHSize(idx int32) math32.Vector3 { + return math32.Vec3(Bodies.Value(int(idx), int(BodyHSizeX)), Bodies.Value(int(idx), int(BodyHSizeY)), Bodies.Value(int(idx), int(BodyHSizeZ))) } -func SetBodySize(idx int32, size math32.Vector3) { - Bodies.Set(size.X, int(idx), int(BodySizeX)) - Bodies.Set(size.Y, int(idx), int(BodySizeY)) - Bodies.Set(size.Z, int(idx), int(BodySizeZ)) +func SetBodyHSize(idx int32, size math32.Vector3) { + Bodies.Set(size.X, int(idx), int(BodyHSizeX)) + Bodies.Set(size.Y, int(idx), int(BodyHSizeY)) + Bodies.Set(size.Z, int(idx), int(BodyHSizeZ)) } func BodyPos(idx int32) math32.Vector3 { diff --git a/physics/body.goal b/physics/body.goal index fb0ecaf2..527b5c1d 100644 --- a/physics/body.goal +++ b/physics/body.goal @@ -43,10 +43,10 @@ const ( // does not need to be handled here. BodyGroup - // BodySize is the size of the object (values depend on shape type). - BodySizeX - BodySizeY - BodySizeZ + // BodyHSize is the size of the object (values depend on shape type). + BodyHSizeX + BodyHSizeY + BodyHSizeZ // BodyThick is the thickness of the body, as a hollow shape. // If 0, then it is a solid shape (default). @@ -143,14 +143,14 @@ func GetBodyGroup(idx int32) int32 { return int32(math.Float32bits(Bodies[idx, BodyGroup])) } -func BodySize(idx int32) math32.Vector3 { - return math32.Vec3(Bodies[idx, BodySizeX], Bodies[idx, BodySizeY], Bodies[idx, BodySizeZ]) +func BodyHSize(idx int32) math32.Vector3 { + return math32.Vec3(Bodies[idx, BodyHSizeX], Bodies[idx, BodyHSizeY], Bodies[idx, BodyHSizeZ]) } -func SetBodySize(idx int32, size math32.Vector3) { - Bodies[idx, BodySizeX] = size.X - Bodies[idx, BodySizeY] = size.Y - Bodies[idx, BodySizeZ] = size.Z +func SetBodyHSize(idx int32, size math32.Vector3) { + Bodies[idx, BodyHSizeX] = size.X + Bodies[idx, BodyHSizeY] = size.Y + Bodies[idx, BodyHSizeZ] = size.Z } func BodyPos(idx int32) math32.Vector3 { diff --git a/physics/contact.go b/physics/contact.go index bb65c72d..80907345 100644 --- a/physics/contact.go +++ b/physics/contact.go @@ -43,9 +43,19 @@ const ( ContactBPointY ContactBPointZ - // Contact depths (thickness in newton) - ContactADepth - ContactBDepth + // contact offset on body A + ContactAOffX + ContactAOffY + ContactAOffZ + + // contact offset on body B + ContactBOffX + ContactBOffY + ContactBOffZ + + // Contact thickness + ContactAThick + ContactBThick // normal pointing from center of B to center of A ContactNormX @@ -58,6 +68,35 @@ const ( ContactForceZ ) +// number of broad-phase contact values: just the indexes +const BroadContactVarsN = ContactAPointX + +func SetBroadContactA(idx, bodIdx int32) { + BroadContacts.Set(math.Float32frombits(uint32(bodIdx)), int(idx), int(ContactA)) +} + +func GetBroadContactA(idx int32) int32 { + return int32(math.Float32bits(BroadContacts.Value(int(idx), int(ContactA)))) +} + +func SetBroadContactB(idx, bodIdx int32) { + BroadContacts.Set(math.Float32frombits(uint32(bodIdx)), int(idx), int(ContactB)) +} + +func GetBroadContactB(idx int32) int32 { + return int32(math.Float32bits(BroadContacts.Value(int(idx), int(ContactB)))) +} + +func SetBroadContactPointIdx(idx, ptIdx int32) { + BroadContacts.Set(math.Float32frombits(uint32(ptIdx)), int(idx), int(ContactPointIdx)) +} + +func GetBroadContactPointIdx(idx int32) int32 { + return int32(math.Float32bits(BroadContacts.Value(int(idx), int(ContactPointIdx)))) +} + +//////// Narrow + func SetContactA(idx, bodIdx int32) { Contacts.Set(math.Float32frombits(uint32(bodIdx)), int(idx), int(ContactA)) } @@ -102,6 +141,26 @@ func SetContactBPoint(idx int32, pos math32.Vector3) { Contacts.Set(pos.Z, int(idx), int(ContactBPointZ)) } +func ContactAOff(idx int32) math32.Vector3 { + return math32.Vec3(Contacts.Value(int(idx), int(ContactAOffX)), Contacts.Value(int(idx), int(ContactAOffY)), Contacts.Value(int(idx), int(ContactAOffZ))) +} + +func SetContactAOff(idx int32, pos math32.Vector3) { + Contacts.Set(pos.X, int(idx), int(ContactAOffX)) + Contacts.Set(pos.Y, int(idx), int(ContactAOffY)) + Contacts.Set(pos.Z, int(idx), int(ContactAOffZ)) +} + +func ContactBOff(idx int32) math32.Vector3 { + return math32.Vec3(Contacts.Value(int(idx), int(ContactBOffX)), Contacts.Value(int(idx), int(ContactBOffY)), Contacts.Value(int(idx), int(ContactBOffZ))) +} + +func SetContactBOff(idx int32, pos math32.Vector3) { + Contacts.Set(pos.X, int(idx), int(ContactBOffX)) + Contacts.Set(pos.Y, int(idx), int(ContactBOffY)) + Contacts.Set(pos.Z, int(idx), int(ContactBOffZ)) +} + func ContactNorm(idx int32) math32.Vector3 { return math32.Vec3(Contacts.Value(int(idx), int(ContactNormX)), Contacts.Value(int(idx), int(ContactNormY)), Contacts.Value(int(idx), int(ContactNormZ))) } @@ -142,6 +201,15 @@ func GroupsCollide(ga, gb int32) bool { return false } +// CollisionInit performs initialization at start of collision (i=1) +func CollisionInit(i uint32) { //gosl:kernel + if i > 0 { + return + } + BroadContactsN.Values[0] = 0 + ContactsN.Values[0] = 0 +} + // newton: geometry/kernels.py: broadphase_collision_pairs // CollisionBroad performs broad-phase collision detection, generating Contacts. @@ -174,7 +242,7 @@ func CollisionBroad(i uint32) { //gosl:kernel // bounding sphere check infPlane := false if sA == Plane { - szA := BodySize(biA) + szA := BodyHSize(biA) if szA.X == 0 { infPlane = true } @@ -199,7 +267,7 @@ func CollisionBroad(i uint32) { //gosl:kernel // note: ignoring contact_point_limit code for now - enci := atomic.AddInt32(&ContactCount.Values[int(params.Cur)], ncA+ncB) + enci := atomic.AddInt32(&BroadContactsN.Values[0], ncA+ncB) // Go returns post-added value, while WGSL returns pre-added value //gosl:wgsl @@ -210,22 +278,22 @@ func CollisionBroad(i uint32) { //gosl:kernel if nci >= params.ContactsMax { // shouldn't happen! return } - AddContacts(biA, biB, ci, ncA, ncB) + AddBroadContacts(biA, biB, nci, ncA, ncB) } // newton: geometry/kernels.py: allocate_contact_points -// AddContacts adds contact records in prep for narrow phase. -func AddContacts(biA, biB, ci, ncA, ncB int32) { +// AddBroadContacts adds broad-phase contact records in prep for narrow phase. +func AddBroadContacts(biA, biB, nci, ncA, ncB int32) { for i := range ncA { - SetContactA(ci+i, biA) - SetContactB(ci+i, biB) - SetContactPointIdx(ci+i, i) + SetBroadContactA(nci+i, biA) + SetBroadContactB(nci+i, biB) + SetBroadContactPointIdx(nci+i, i) } for i := range ncB { - SetContactA(ci+ncA+i, biB) // flipped - SetContactB(ci+ncA+i, biA) - SetContactPointIdx(ci+i, i) + SetBroadContactA(nci+ncA+i, biB) // flipped + SetBroadContactB(nci+ncA+i, biA) + SetBroadContactPointIdx(nci+i, i) } } @@ -235,12 +303,13 @@ func AddContacts(biA, biB, ci, ncA, ncB int32) { func CollisionNarrow(i uint32) { //gosl:kernel params := GetParams(0) ci := int32(i) - if ci >= params.ContactsMax { + cmax := BroadContactsN.Values[0] + if ci >= cmax { return } - biA := GetContactA(ci) - biB := GetContactB(ci) - cpi := GetContactPointIdx(ci) + biA := GetBroadContactA(ci) + biB := GetBroadContactB(ci) + cpi := GetBroadContactPointIdx(ci) sA := GetBodyShape(biA) sB := GetBodyShape(biB) @@ -251,21 +320,83 @@ func CollisionNarrow(i uint32) { //gosl:kernel // could be per-shape // margin = wp.max(shape_contact_margin[shape_a], shape_contact_margin[shape_b]) margin := params.ContactMargin - thick := gdA.Thick + gdB.Thick - dist := float32(1.0e6) - var ptA, ptB, norm math32.Vector3 + maxIter := params.MaxGeomIter + + // note: no Cone on anything + var ptA, ptB, norm, nnorm math32.Vector3 switch gdA.Shape { + case Plane: + switch gdB.Shape { + case Sphere: + dist = ColSpherePlane(cpi, maxIter, &gdB, &gdA, &ptB, &ptA, &nnorm) // reverse + norm = slmath.Negate3(nnorm) + case Capsule: + dist = ColCapsulePlane(cpi, maxIter, &gdB, &gdA, &ptB, &ptA, &nnorm) // reverse + norm = slmath.Negate3(nnorm) + case Cylinder: + dist = ColCylinderPlane(cpi, maxIter, &gdB, &gdA, &ptB, &ptA, &nnorm) // reverse + norm = slmath.Negate3(nnorm) + case Box: + dist = ColBoxPlane(cpi, maxIter, &gdB, &gdA, &ptB, &ptA, &nnorm) // reverse + norm = slmath.Negate3(nnorm) + default: + } case Sphere: switch gdB.Shape { case Sphere: - dist = ColSphereSphere(cpi, &gdA, &gdB, &ptA, &ptB, &norm) + dist = ColSphereSphere(cpi, maxIter, &gdA, &gdB, &ptA, &ptB, &norm) + case Capsule: + dist = ColSphereCapsule(cpi, maxIter, &gdA, &gdB, &ptA, &ptB, &norm) + // no cylinder case Box: + dist = ColSphereBox(cpi, maxIter, &gdA, &gdB, &ptA, &ptB, &norm) + default: + } + case Capsule: + switch gdB.Shape { case Capsule: + dist = ColCapsuleCapsule(cpi, maxIter, &gdA, &gdB, &ptA, &ptB, &norm) + // no cylinder + case Box: + dist = ColBoxCapsule(cpi, maxIter, &gdB, &gdA, &ptB, &ptA, &nnorm) // reverse + norm = slmath.Negate3(nnorm) + default: + } + case Box: + switch gdB.Shape { + case Box: + dist = ColBoxBox(cpi, maxIter, &gdA, &gdB, &ptA, &ptB, &norm) default: } default: } + var ctA, ctB, offA, offB math32.Vector3 + var distActual, offMagA, offMagB float32 + actual := ContactPoints(dist, margin, &gdA, &gdB, ptA, ptB, norm, &ctA, &ctB, &offA, &offB, &distActual, &offMagA, &offMagB) + if !actual { + return + } + + enci := atomic.AddInt32(&ContactsN.Values[0], 1) + // Go returns post-added value, while WGSL returns pre-added value + + //gosl:wgsl + // enci += int32(1) // wgsl now matches Go + //gosl:end + + nci := enci - 1 + + SetContactA(nci, biA) + SetContactB(nci, biB) + SetContactPointIdx(nci, cpi) + SetContactAPoint(nci, ctA) + SetContactBPoint(nci, ctB) + SetContactAOff(nci, offA) + SetContactBOff(nci, offB) + SetContactNorm(nci, norm) + Contacts.Set(offMagA, int(nci), int(ContactAThick)) + Contacts.Set(offMagB, int(nci), int(ContactBThick)) } //gosl:end @@ -355,7 +486,7 @@ func (wl *World) SetMaxContacts() { sB := GetBodyShape(biB) infPlane := false - szA := BodySize(biA) + szA := BodyHSize(biA) if szA.X == 0 { infPlane = true } @@ -370,5 +501,6 @@ func (wl *World) SetMaxContacts() { n = n / 2 // todo: could do more of this as N gets larger } params.ContactsMax = n + wl.BroadContacts.SetShapeSizes(int(n), int(BroadContactVarsN)) wl.Contacts.SetShapeSizes(int(n), int(ContactVarsN)) } diff --git a/physics/contact.goal b/physics/contact.goal index 75f40b54..7d990694 100644 --- a/physics/contact.goal +++ b/physics/contact.goal @@ -41,9 +41,19 @@ const ( ContactBPointY ContactBPointZ - // Contact depths (thickness in newton) - ContactADepth - ContactBDepth + // contact offset on body A + ContactAOffX + ContactAOffY + ContactAOffZ + + // contact offset on body B + ContactBOffX + ContactBOffY + ContactBOffZ + + // Contact thickness + ContactAThick + ContactBThick // normal pointing from center of B to center of A ContactNormX @@ -56,6 +66,34 @@ const ( ContactForceZ ) +// number of broad-phase contact values: just the indexes +const BroadContactVarsN = ContactAPointX + +func SetBroadContactA(idx, bodIdx int32) { + BroadContacts[idx, ContactA] = math.Float32frombits(uint32(bodIdx)) +} + +func GetBroadContactA(idx int32) int32 { + return int32(math.Float32bits(BroadContacts[idx, ContactA])) +} + +func SetBroadContactB(idx, bodIdx int32) { + BroadContacts[idx, ContactB] = math.Float32frombits(uint32(bodIdx)) +} + +func GetBroadContactB(idx int32) int32 { + return int32(math.Float32bits(BroadContacts[idx, ContactB])) +} + +func SetBroadContactPointIdx(idx, ptIdx int32) { + BroadContacts[idx, ContactPointIdx] = math.Float32frombits(uint32(ptIdx)) +} + +func GetBroadContactPointIdx(idx int32) int32 { + return int32(math.Float32bits(BroadContacts[idx, ContactPointIdx])) +} + +//////// Narrow func SetContactA(idx, bodIdx int32) { Contacts[idx, ContactA] = math.Float32frombits(uint32(bodIdx)) @@ -101,6 +139,26 @@ func SetContactBPoint(idx int32, pos math32.Vector3) { Contacts[idx, ContactBPointZ] = pos.Z } +func ContactAOff(idx int32) math32.Vector3 { + return math32.Vec3(Contacts[idx, ContactAOffX], Contacts[idx, ContactAOffY], Contacts[idx, ContactAOffZ]) +} + +func SetContactAOff(idx int32, pos math32.Vector3) { + Contacts[idx, ContactAOffX] = pos.X + Contacts[idx, ContactAOffY] = pos.Y + Contacts[idx, ContactAOffZ] = pos.Z +} + +func ContactBOff(idx int32) math32.Vector3 { + return math32.Vec3(Contacts[idx, ContactBOffX], Contacts[idx, ContactBOffY], Contacts[idx, ContactBOffZ]) +} + +func SetContactBOff(idx int32, pos math32.Vector3) { + Contacts[idx, ContactBOffX] = pos.X + Contacts[idx, ContactBOffY] = pos.Y + Contacts[idx, ContactBOffZ] = pos.Z +} + func ContactNorm(idx int32) math32.Vector3 { return math32.Vec3(Contacts[idx, ContactNormX], Contacts[idx, ContactNormY], Contacts[idx, ContactNormZ]) } @@ -141,6 +199,15 @@ func GroupsCollide(ga, gb int32) bool { return false } +// CollisionInit performs initialization at start of collision (i=1) +func CollisionInit(i uint32) { //gosl:kernel + if i > 0 { + return + } + BroadContactsN.Values[0] = 0 + ContactsN.Values[0] = 0 +} + // newton: geometry/kernels.py: broadphase_collision_pairs // CollisionBroad performs broad-phase collision detection, generating Contacts. @@ -173,7 +240,7 @@ func CollisionBroad(i uint32) { //gosl:kernel // bounding sphere check infPlane := false if sA == Plane { - szA := BodySize(biA) + szA := BodyHSize(biA) if szA.X == 0 { infPlane = true } @@ -198,7 +265,7 @@ func CollisionBroad(i uint32) { //gosl:kernel // note: ignoring contact_point_limit code for now - enci := atomic.AddInt32(&ContactCount.Values[int(params.Cur)], ncA + ncB) + enci := atomic.AddInt32(&BroadContactsN.Values[0], ncA + ncB) // Go returns post-added value, while WGSL returns pre-added value //gosl:wgsl @@ -209,22 +276,22 @@ func CollisionBroad(i uint32) { //gosl:kernel if nci >= params.ContactsMax { // shouldn't happen! return } - AddContacts(biA, biB, ci, ncA, ncB) + AddBroadContacts(biA, biB, nci, ncA, ncB) } // newton: geometry/kernels.py: allocate_contact_points -// AddContacts adds contact records in prep for narrow phase. -func AddContacts(biA, biB, ci, ncA, ncB int32) { +// AddBroadContacts adds broad-phase contact records in prep for narrow phase. +func AddBroadContacts(biA, biB, nci, ncA, ncB int32) { for i := range ncA { - SetContactA(ci + i, biA) - SetContactB(ci + i, biB) - SetContactPointIdx(ci+i, i) + SetBroadContactA(nci + i, biA) + SetBroadContactB(nci + i, biB) + SetBroadContactPointIdx(nci+i, i) } for i := range ncB { - SetContactA(ci + ncA + i, biB) // flipped - SetContactB(ci + ncA + i, biA) - SetContactPointIdx(ci+i, i) + SetBroadContactA(nci + ncA + i, biB) // flipped + SetBroadContactB(nci + ncA + i, biA) + SetBroadContactPointIdx(nci+i, i) } } @@ -235,12 +302,13 @@ func AddContacts(biA, biB, ci, ncA, ncB int32) { func CollisionNarrow(i uint32) { //gosl:kernel params := GetParams(0) ci := int32(i) - if ci >= params.ContactsMax { + cmax := BroadContactsN.Values[0] + if ci >= cmax { return } - biA := GetContactA(ci) - biB := GetContactB(ci) - cpi := GetContactPointIdx(ci) + biA := GetBroadContactA(ci) + biB := GetBroadContactB(ci) + cpi := GetBroadContactPointIdx(ci) sA := GetBodyShape(biA) sB := GetBodyShape(biB) @@ -251,21 +319,83 @@ func CollisionNarrow(i uint32) { //gosl:kernel // could be per-shape // margin = wp.max(shape_contact_margin[shape_a], shape_contact_margin[shape_b]) margin := params.ContactMargin - thick := gdA.Thick + gdB.Thick - dist := float32(1.0e6) - var ptA, ptB, norm math32.Vector3 + maxIter := params.MaxGeomIter + + // note: no Cone on anything + var ptA, ptB, norm, nnorm math32.Vector3 switch gdA.Shape { + case Plane: + switch gdB.Shape { + case Sphere: + dist = ColSpherePlane(cpi, maxIter, &gdB, &gdA, &ptB, &ptA, &nnorm) // reverse + norm = slmath.Negate3(nnorm) + case Capsule: + dist = ColCapsulePlane(cpi, maxIter, &gdB, &gdA, &ptB, &ptA, &nnorm) // reverse + norm = slmath.Negate3(nnorm) + case Cylinder: + dist = ColCylinderPlane(cpi, maxIter, &gdB, &gdA, &ptB, &ptA, &nnorm) // reverse + norm = slmath.Negate3(nnorm) + case Box: + dist = ColBoxPlane(cpi, maxIter, &gdB, &gdA, &ptB, &ptA, &nnorm) // reverse + norm = slmath.Negate3(nnorm) + default: + } case Sphere: switch gdB.Shape { case Sphere: - dist = ColSphereSphere(cpi, &gdA, &gdB, &ptA, &ptB, &norm) + dist = ColSphereSphere(cpi, maxIter, &gdA, &gdB, &ptA, &ptB, &norm) + case Capsule: + dist = ColSphereCapsule(cpi, maxIter, &gdA, &gdB, &ptA, &ptB, &norm) + // no cylinder case Box: + dist = ColSphereBox(cpi, maxIter, &gdA, &gdB, &ptA, &ptB, &norm) + default: + } + case Capsule: + switch gdB.Shape { case Capsule: + dist = ColCapsuleCapsule(cpi, maxIter, &gdA, &gdB, &ptA, &ptB, &norm) + // no cylinder + case Box: + dist = ColBoxCapsule(cpi, maxIter, &gdB, &gdA, &ptB, &ptA, &nnorm) // reverse + norm = slmath.Negate3(nnorm) + default: + } + case Box: + switch gdB.Shape { + case Box: + dist = ColBoxBox(cpi, maxIter, &gdA, &gdB, &ptA, &ptB, &norm) default: } default: } + var ctA, ctB, offA, offB math32.Vector3 + var distActual, offMagA, offMagB float32 + actual := ContactPoints(dist, margin, &gdA, &gdB, ptA, ptB, norm, &ctA, &ctB, &offA, &offB, &distActual, &offMagA, &offMagB) + if !actual { + return + } + + enci := atomic.AddInt32(&ContactsN.Values[0], 1) + // Go returns post-added value, while WGSL returns pre-added value + + //gosl:wgsl + // enci += int32(1) // wgsl now matches Go + //gosl:end + + nci := enci - 1 + + SetContactA(nci, biA) + SetContactB(nci, biB) + SetContactPointIdx(nci, cpi) + SetContactAPoint(nci, ctA) + SetContactBPoint(nci, ctB) + SetContactAOff(nci, offA) + SetContactBOff(nci, offB) + SetContactNorm(nci, norm) + Contacts[nci, ContactAThick] = offMagA + Contacts[nci, ContactBThick] = offMagB } @@ -356,7 +486,7 @@ func (wl *World) SetMaxContacts() { sB := GetBodyShape(biB) infPlane := false - szA := BodySize(biA) + szA := BodyHSize(biA) if szA.X == 0 { infPlane = true } @@ -371,6 +501,7 @@ func (wl *World) SetMaxContacts() { n = n/2 // todo: could do more of this as N gets larger } params.ContactsMax = n + wl.BroadContacts.SetShapeSizes(int(n), int(BroadContactVarsN)) wl.Contacts.SetShapeSizes(int(n), int(ContactVarsN)) } diff --git a/physics/enumgen.go b/physics/enumgen.go index 68078cbd..6a412f76 100644 --- a/physics/enumgen.go +++ b/physics/enumgen.go @@ -15,11 +15,11 @@ const BodyVarsN BodyVars = 41 //gosl:end -var _BodyVarsValueMap = map[string]BodyVars{`BodyShape`: 0, `BodyDynamic`: 1, `BodyWorld`: 2, `BodyGroup`: 3, `BodySizeX`: 4, `BodySizeY`: 5, `BodySizeZ`: 6, `BodyThick`: 7, `BodyMass`: 8, `BodyInvMass`: 9, `BodyBounce`: 10, `BodyFriction`: 11, `BodyPosX`: 12, `BodyPosY`: 13, `BodyPosZ`: 14, `BodyQuatX`: 15, `BodyQuatY`: 16, `BodyQuatZ`: 17, `BodyQuatW`: 18, `BodyComX`: 19, `BodyComY`: 20, `BodyComZ`: 21, `BodyInertiaXX`: 22, `BodyInertiaYX`: 23, `BodyInertiaZX`: 24, `BodyInertiaXY`: 25, `BodyInertiaYY`: 26, `BodyInertiaZY`: 27, `BodyInertiaXZ`: 28, `BodyInertiaYZ`: 29, `BodyInertiaZZ`: 30, `BodyInvInertiaXX`: 31, `BodyInvInertiaYX`: 32, `BodyInvInertiaZX`: 33, `BodyInvInertiaXY`: 34, `BodyInvInertiaYY`: 35, `BodyInvInertiaZY`: 36, `BodyInvInertiaXZ`: 37, `BodyInvInertiaYZ`: 38, `BodyInvInertiaZZ`: 39, `BodyRadius`: 40} +var _BodyVarsValueMap = map[string]BodyVars{`BodyShape`: 0, `BodyDynamic`: 1, `BodyWorld`: 2, `BodyGroup`: 3, `BodyHSizeX`: 4, `BodyHSizeY`: 5, `BodyHSizeZ`: 6, `BodyThick`: 7, `BodyMass`: 8, `BodyInvMass`: 9, `BodyBounce`: 10, `BodyFriction`: 11, `BodyPosX`: 12, `BodyPosY`: 13, `BodyPosZ`: 14, `BodyQuatX`: 15, `BodyQuatY`: 16, `BodyQuatZ`: 17, `BodyQuatW`: 18, `BodyComX`: 19, `BodyComY`: 20, `BodyComZ`: 21, `BodyInertiaXX`: 22, `BodyInertiaYX`: 23, `BodyInertiaZX`: 24, `BodyInertiaXY`: 25, `BodyInertiaYY`: 26, `BodyInertiaZY`: 27, `BodyInertiaXZ`: 28, `BodyInertiaYZ`: 29, `BodyInertiaZZ`: 30, `BodyInvInertiaXX`: 31, `BodyInvInertiaYX`: 32, `BodyInvInertiaZX`: 33, `BodyInvInertiaXY`: 34, `BodyInvInertiaYY`: 35, `BodyInvInertiaZY`: 36, `BodyInvInertiaXZ`: 37, `BodyInvInertiaYZ`: 38, `BodyInvInertiaZZ`: 39, `BodyRadius`: 40} -var _BodyVarsDescMap = map[BodyVars]string{0: `BodyShape is the shape type of the object, as a Shapes type.`, 1: `BodyDynamic is the index into Dynamics for this body, which is -1 for static bodies. Use this to get current Pos and Quat values for a dynamic body.`, 2: `BodyWorld partitions bodies into different worlds for collision detection: Global bodies = -1 can collide with everything; otherwise only items within the same world collide.`, 3: `BodyGroup partitions bodies within worlds into different groups for collision detection. 0 does not collide with anything. Negative numbers are global within a world, except they don't collide amongst themselves (all non-dynamic bodies should go in -1 because they don't collide amongst each-other, but do potentially collide with dynamics). Positive numbers only collide amongst themselves, and with negative groups, but not other positive groups. This is for more special-purpose dynamics: in general use 1 for all dynamic bodies. There is an automatic constraint that the two objects within a single joint do not collide with each other, so this does not need to be handled here.`, 4: `BodySize is the size of the object (values depend on shape type).`, 5: ``, 6: ``, 7: `BodyThick is the thickness of the body, as a hollow shape. If 0, then it is a solid shape (default).`, 8: `BodyMass is the mass of the object.`, 9: `BodyInvMass is 1/mass of the object or 0 if no mass.`, 10: `BodyBounce specifies the COR or coefficient of restitution (0..1), which determines how elastic the collision is, i.e., final velocity / initial velocity.`, 11: `BodyFriction coefficient: how much friction is generated by transverse motion. Additive across the two surfaces.`, 12: `3D position of body (structural center).`, 13: ``, 14: ``, 15: `Quaternion rotation of body.`, 16: ``, 17: ``, 18: ``, 19: `Relative center-of-mass offset from 3D position of body.`, 20: ``, 21: ``, 22: `Inertia 3x3 matrix (column matrix organization, r,c labels).`, 23: ``, 24: ``, 25: ``, 26: ``, 27: ``, 28: ``, 29: ``, 30: ``, 31: `InvInertia inverse inertia 3x3 matrix (column matrix organization, r,c labels).`, 32: ``, 33: ``, 34: ``, 35: ``, 36: ``, 37: ``, 38: ``, 39: ``, 40: `radius for broadphase collision`} +var _BodyVarsDescMap = map[BodyVars]string{0: `BodyShape is the shape type of the object, as a Shapes type.`, 1: `BodyDynamic is the index into Dynamics for this body, which is -1 for static bodies. Use this to get current Pos and Quat values for a dynamic body.`, 2: `BodyWorld partitions bodies into different worlds for collision detection: Global bodies = -1 can collide with everything; otherwise only items within the same world collide.`, 3: `BodyGroup partitions bodies within worlds into different groups for collision detection. 0 does not collide with anything. Negative numbers are global within a world, except they don't collide amongst themselves (all non-dynamic bodies should go in -1 because they don't collide amongst each-other, but do potentially collide with dynamics). Positive numbers only collide amongst themselves, and with negative groups, but not other positive groups. This is for more special-purpose dynamics: in general use 1 for all dynamic bodies. There is an automatic constraint that the two objects within a single joint do not collide with each other, so this does not need to be handled here.`, 4: `BodyHSize is the size of the object (values depend on shape type).`, 5: ``, 6: ``, 7: `BodyThick is the thickness of the body, as a hollow shape. If 0, then it is a solid shape (default).`, 8: `BodyMass is the mass of the object.`, 9: `BodyInvMass is 1/mass of the object or 0 if no mass.`, 10: `BodyBounce specifies the COR or coefficient of restitution (0..1), which determines how elastic the collision is, i.e., final velocity / initial velocity.`, 11: `BodyFriction coefficient: how much friction is generated by transverse motion. Additive across the two surfaces.`, 12: `3D position of body (structural center).`, 13: ``, 14: ``, 15: `Quaternion rotation of body.`, 16: ``, 17: ``, 18: ``, 19: `Relative center-of-mass offset from 3D position of body.`, 20: ``, 21: ``, 22: `Inertia 3x3 matrix (column matrix organization, r,c labels).`, 23: ``, 24: ``, 25: ``, 26: ``, 27: ``, 28: ``, 29: ``, 30: ``, 31: `InvInertia inverse inertia 3x3 matrix (column matrix organization, r,c labels).`, 32: ``, 33: ``, 34: ``, 35: ``, 36: ``, 37: ``, 38: ``, 39: ``, 40: `radius for broadphase collision`} -var _BodyVarsMap = map[BodyVars]string{0: `BodyShape`, 1: `BodyDynamic`, 2: `BodyWorld`, 3: `BodyGroup`, 4: `BodySizeX`, 5: `BodySizeY`, 6: `BodySizeZ`, 7: `BodyThick`, 8: `BodyMass`, 9: `BodyInvMass`, 10: `BodyBounce`, 11: `BodyFriction`, 12: `BodyPosX`, 13: `BodyPosY`, 14: `BodyPosZ`, 15: `BodyQuatX`, 16: `BodyQuatY`, 17: `BodyQuatZ`, 18: `BodyQuatW`, 19: `BodyComX`, 20: `BodyComY`, 21: `BodyComZ`, 22: `BodyInertiaXX`, 23: `BodyInertiaYX`, 24: `BodyInertiaZX`, 25: `BodyInertiaXY`, 26: `BodyInertiaYY`, 27: `BodyInertiaZY`, 28: `BodyInertiaXZ`, 29: `BodyInertiaYZ`, 30: `BodyInertiaZZ`, 31: `BodyInvInertiaXX`, 32: `BodyInvInertiaYX`, 33: `BodyInvInertiaZX`, 34: `BodyInvInertiaXY`, 35: `BodyInvInertiaYY`, 36: `BodyInvInertiaZY`, 37: `BodyInvInertiaXZ`, 38: `BodyInvInertiaYZ`, 39: `BodyInvInertiaZZ`, 40: `BodyRadius`} +var _BodyVarsMap = map[BodyVars]string{0: `BodyShape`, 1: `BodyDynamic`, 2: `BodyWorld`, 3: `BodyGroup`, 4: `BodyHSizeX`, 5: `BodyHSizeY`, 6: `BodyHSizeZ`, 7: `BodyThick`, 8: `BodyMass`, 9: `BodyInvMass`, 10: `BodyBounce`, 11: `BodyFriction`, 12: `BodyPosX`, 13: `BodyPosY`, 14: `BodyPosZ`, 15: `BodyQuatX`, 16: `BodyQuatY`, 17: `BodyQuatZ`, 18: `BodyQuatW`, 19: `BodyComX`, 20: `BodyComY`, 21: `BodyComZ`, 22: `BodyInertiaXX`, 23: `BodyInertiaYX`, 24: `BodyInertiaZX`, 25: `BodyInertiaXY`, 26: `BodyInertiaYY`, 27: `BodyInertiaZY`, 28: `BodyInertiaXZ`, 29: `BodyInertiaYZ`, 30: `BodyInertiaZZ`, 31: `BodyInvInertiaXX`, 32: `BodyInvInertiaYX`, 33: `BodyInvInertiaZX`, 34: `BodyInvInertiaXY`, 35: `BodyInvInertiaYY`, 36: `BodyInvInertiaZY`, 37: `BodyInvInertiaXZ`, 38: `BodyInvInertiaYZ`, 39: `BodyInvInertiaZZ`, 40: `BodyRadius`} // String returns the string representation of this BodyVars value. func (i BodyVars) String() string { return enums.String(i, _BodyVarsMap) } @@ -51,20 +51,20 @@ func (i BodyVars) MarshalText() ([]byte, error) { return []byte(i.String()), nil // UnmarshalText implements the [encoding.TextUnmarshaler] interface. func (i *BodyVars) UnmarshalText(text []byte) error { return enums.UnmarshalText(i, text, "BodyVars") } -var _ContactVarsValues = []ContactVars{0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16} +var _ContactVarsValues = []ContactVars{0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22} // ContactVarsN is the highest valid value for type ContactVars, plus one. // //gosl:start -const ContactVarsN ContactVars = 17 +const ContactVarsN ContactVars = 23 //gosl:end -var _ContactVarsValueMap = map[string]ContactVars{`ContactA`: 0, `ContactB`: 1, `ContactPointIdx`: 2, `ContactAPointX`: 3, `ContactAPointY`: 4, `ContactAPointZ`: 5, `ContactBPointX`: 6, `ContactBPointY`: 7, `ContactBPointZ`: 8, `ContactADepth`: 9, `ContactBDepth`: 10, `ContactNormX`: 11, `ContactNormY`: 12, `ContactNormZ`: 13, `ContactForceX`: 14, `ContactForceY`: 15, `ContactForceZ`: 16} +var _ContactVarsValueMap = map[string]ContactVars{`ContactA`: 0, `ContactB`: 1, `ContactPointIdx`: 2, `ContactAPointX`: 3, `ContactAPointY`: 4, `ContactAPointZ`: 5, `ContactBPointX`: 6, `ContactBPointY`: 7, `ContactBPointZ`: 8, `ContactAOffX`: 9, `ContactAOffY`: 10, `ContactAOffZ`: 11, `ContactBOffX`: 12, `ContactBOffY`: 13, `ContactBOffZ`: 14, `ContactAThick`: 15, `ContactBThick`: 16, `ContactNormX`: 17, `ContactNormY`: 18, `ContactNormZ`: 19, `ContactForceX`: 20, `ContactForceY`: 21, `ContactForceZ`: 22} -var _ContactVarsDescMap = map[ContactVars]string{0: `first body index`, 1: `the other body index`, 2: `contact point index for A-B pair`, 3: `contact point on body A`, 4: ``, 5: ``, 6: `contact point on body B`, 7: ``, 8: ``, 9: `Contact depths (thickness in newton)`, 10: ``, 11: `normal pointing from center of B to center of A`, 12: ``, 13: ``, 14: `computed contact force vector`, 15: ``, 16: ``} +var _ContactVarsDescMap = map[ContactVars]string{0: `first body index`, 1: `the other body index`, 2: `contact point index for A-B pair`, 3: `contact point on body A`, 4: ``, 5: ``, 6: `contact point on body B`, 7: ``, 8: ``, 9: `contact offset on body A`, 10: ``, 11: ``, 12: `contact offset on body B`, 13: ``, 14: ``, 15: `Contact thickness`, 16: ``, 17: `normal pointing from center of B to center of A`, 18: ``, 19: ``, 20: `computed contact force vector`, 21: ``, 22: ``} -var _ContactVarsMap = map[ContactVars]string{0: `ContactA`, 1: `ContactB`, 2: `ContactPointIdx`, 3: `ContactAPointX`, 4: `ContactAPointY`, 5: `ContactAPointZ`, 6: `ContactBPointX`, 7: `ContactBPointY`, 8: `ContactBPointZ`, 9: `ContactADepth`, 10: `ContactBDepth`, 11: `ContactNormX`, 12: `ContactNormY`, 13: `ContactNormZ`, 14: `ContactForceX`, 15: `ContactForceY`, 16: `ContactForceZ`} +var _ContactVarsMap = map[ContactVars]string{0: `ContactA`, 1: `ContactB`, 2: `ContactPointIdx`, 3: `ContactAPointX`, 4: `ContactAPointY`, 5: `ContactAPointZ`, 6: `ContactBPointX`, 7: `ContactBPointY`, 8: `ContactBPointZ`, 9: `ContactAOffX`, 10: `ContactAOffY`, 11: `ContactAOffZ`, 12: `ContactBOffX`, 13: `ContactBOffY`, 14: `ContactBOffZ`, 15: `ContactAThick`, 16: `ContactBThick`, 17: `ContactNormX`, 18: `ContactNormY`, 19: `ContactNormZ`, 20: `ContactForceX`, 21: `ContactForceY`, 22: `ContactForceZ`} // String returns the string representation of this ContactVars value. func (i ContactVars) String() string { return enums.String(i, _ContactVarsMap) } @@ -192,20 +192,20 @@ func (i *DynamicVars) UnmarshalText(text []byte) error { return enums.UnmarshalText(i, text, "DynamicVars") } -var _GPUVarsValues = []GPUVars{0, 1, 2, 3, 4, 5, 6, 7, 8, 9} +var _GPUVarsValues = []GPUVars{0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11} // GPUVarsN is the highest valid value for type GPUVars, plus one. // //gosl:start -const GPUVarsN GPUVars = 10 +const GPUVarsN GPUVars = 12 //gosl:end -var _GPUVarsValueMap = map[string]GPUVars{`ParamsVar`: 0, `BodiesVar`: 1, `JointsVar`: 2, `JointDoFsVar`: 3, `BodyJointsVar`: 4, `BodyCollidePairsVar`: 5, `DynamicsVar`: 6, `ContactCountVar`: 7, `ContactsVar`: 8, `JointControlsVar`: 9} +var _GPUVarsValueMap = map[string]GPUVars{`ParamsVar`: 0, `BodiesVar`: 1, `JointsVar`: 2, `JointDoFsVar`: 3, `BodyJointsVar`: 4, `BodyCollidePairsVar`: 5, `DynamicsVar`: 6, `BroadContactsNVar`: 7, `BroadContactsVar`: 8, `ContactsNVar`: 9, `ContactsVar`: 10, `JointControlsVar`: 11} -var _GPUVarsDescMap = map[GPUVars]string{0: ``, 1: ``, 2: ``, 3: ``, 4: ``, 5: ``, 6: ``, 7: ``, 8: ``, 9: ``} +var _GPUVarsDescMap = map[GPUVars]string{0: ``, 1: ``, 2: ``, 3: ``, 4: ``, 5: ``, 6: ``, 7: ``, 8: ``, 9: ``, 10: ``, 11: ``} -var _GPUVarsMap = map[GPUVars]string{0: `ParamsVar`, 1: `BodiesVar`, 2: `JointsVar`, 3: `JointDoFsVar`, 4: `BodyJointsVar`, 5: `BodyCollidePairsVar`, 6: `DynamicsVar`, 7: `ContactCountVar`, 8: `ContactsVar`, 9: `JointControlsVar`} +var _GPUVarsMap = map[GPUVars]string{0: `ParamsVar`, 1: `BodiesVar`, 2: `JointsVar`, 3: `JointDoFsVar`, 4: `BodyJointsVar`, 5: `BodyCollidePairsVar`, 6: `DynamicsVar`, 7: `BroadContactsNVar`, 8: `BroadContactsVar`, 9: `ContactsNVar`, 10: `ContactsVar`, 11: `JointControlsVar`} // String returns the string representation of this GPUVars value. func (i GPUVars) String() string { return enums.String(i, _GPUVarsMap) } diff --git a/physics/gosl.go b/physics/gosl.go index e7ca6827..9924b16c 100644 --- a/physics/gosl.go +++ b/physics/gosl.go @@ -46,9 +46,11 @@ const ( BodyJointsVar GPUVars = 4 BodyCollidePairsVar GPUVars = 5 DynamicsVar GPUVars = 6 - ContactCountVar GPUVars = 7 - ContactsVar GPUVars = 8 - JointControlsVar GPUVars = 9 + BroadContactsNVar GPUVars = 7 + BroadContactsVar GPUVars = 8 + ContactsNVar GPUVars = 9 + ContactsVar GPUVars = 10 + JointControlsVar GPUVars = 11 ) // Tensor stride variables @@ -100,7 +102,9 @@ func GPUInit() { var vr *gpu.Var _ = vr vr = sgp.Add("Dynamics", gpu.Float32, 1, gpu.ComputeShader) - vr = sgp.Add("ContactCount", gpu.Int32, 1, gpu.ComputeShader) + vr = sgp.Add("BroadContactsN", gpu.Int32, 1, gpu.ComputeShader) + vr = sgp.Add("BroadContacts", gpu.Float32, 1, gpu.ComputeShader) + vr = sgp.Add("ContactsN", gpu.Int32, 1, gpu.ComputeShader) vr = sgp.Add("Contacts", gpu.Float32, 1, gpu.ComputeShader) sgp.SetNValues(1) } @@ -116,14 +120,21 @@ func GPUInit() { pl.AddVarUsed(0, "TensorStrides") pl.AddVarUsed(1, "Bodies") pl.AddVarUsed(1, "BodyCollidePairs") - pl.AddVarUsed(2, "ContactCount") - pl.AddVarUsed(2, "Contacts") + pl.AddVarUsed(2, "BroadContacts") + pl.AddVarUsed(2, "BroadContactsN") pl.AddVarUsed(2, "Dynamics") pl.AddVarUsed(0, "Params") + pl = gpu.NewComputePipelineShaderFS(shaders, "shaders/CollisionInit.wgsl", sy) + pl.AddVarUsed(0, "TensorStrides") + pl.AddVarUsed(2, "BroadContactsN") + pl.AddVarUsed(2, "ContactsN") pl = gpu.NewComputePipelineShaderFS(shaders, "shaders/CollisionNarrow.wgsl", sy) pl.AddVarUsed(0, "TensorStrides") pl.AddVarUsed(1, "Bodies") + pl.AddVarUsed(2, "BroadContacts") + pl.AddVarUsed(2, "BroadContactsN") pl.AddVarUsed(2, "Contacts") + pl.AddVarUsed(2, "ContactsN") pl.AddVarUsed(2, "Dynamics") pl.AddVarUsed(0, "Params") pl = gpu.NewComputePipelineShaderFS(shaders, "shaders/DeltasFromJoints.wgsl", sy) @@ -234,6 +245,48 @@ func RunOneCollisionBroad(n int, syncVars ...GPUVars) { RunCollisionBroadCPU(n) } } +// RunCollisionInit runs the CollisionInit kernel with given number of elements, +// on either the CPU or GPU depending on the UseGPU variable. +// Can call multiple Run* kernels in a row, which are then all launched +// in the same command submission on the GPU, which is by far the most efficient. +// MUST call RunDone (with optional vars to sync) after all Run calls. +// Alternatively, a single-shot RunOneCollisionInit call does Run and Done for a +// single run-and-sync case. +func RunCollisionInit(n int) { + if UseGPU { + RunCollisionInitGPU(n) + } else { + RunCollisionInitCPU(n) + } +} + +// RunCollisionInitGPU runs the CollisionInit kernel on the GPU. See [RunCollisionInit] for more info. +func RunCollisionInitGPU(n int) { + sy := GPUSystem + pl := sy.ComputePipelines["CollisionInit"] + ce, _ := sy.BeginComputePass() + pl.Dispatch1D(ce, n, 64) +} + +// RunCollisionInitCPU runs the CollisionInit kernel on the CPU. +func RunCollisionInitCPU(n int) { + gpu.VectorizeFunc(0, n, CollisionInit) +} + +// RunOneCollisionInit runs the CollisionInit kernel with given number of elements, +// on either the CPU or GPU depending on the UseGPU variable. +// This version then calls RunDone with the given variables to sync +// after the Run, for a single-shot Run-and-Done call. If multiple kernels +// can be run in sequence, it is much more efficient to do multiple Run* +// calls followed by a RunDone call. +func RunOneCollisionInit(n int, syncVars ...GPUVars) { + if UseGPU { + RunCollisionInitGPU(n) + RunDone(syncVars...) + } else { + RunCollisionInitCPU(n) + } +} // RunCollisionNarrow runs the CollisionNarrow kernel with given number of elements, // on either the CPU or GPU depending on the UseGPU variable. // Can call multiple Run* kernels in a row, which are then all launched @@ -658,9 +711,15 @@ func ToGPU(vars ...GPUVars) { case DynamicsVar: v, _ := syVars.ValueByIndex(2, "Dynamics", 0) gpu.SetValueFrom(v, Dynamics.Values) - case ContactCountVar: - v, _ := syVars.ValueByIndex(2, "ContactCount", 0) - gpu.SetValueFrom(v, ContactCount.Values) + case BroadContactsNVar: + v, _ := syVars.ValueByIndex(2, "BroadContactsN", 0) + gpu.SetValueFrom(v, BroadContactsN.Values) + case BroadContactsVar: + v, _ := syVars.ValueByIndex(2, "BroadContacts", 0) + gpu.SetValueFrom(v, BroadContacts.Values) + case ContactsNVar: + v, _ := syVars.ValueByIndex(2, "ContactsN", 0) + gpu.SetValueFrom(v, ContactsN.Values) case ContactsVar: v, _ := syVars.ValueByIndex(2, "Contacts", 0) gpu.SetValueFrom(v, Contacts.Values) @@ -688,7 +747,7 @@ func ToGPUTensorStrides() { } sy := GPUSystem syVars := sy.Vars() - TensorStrides.SetShapeSizes(90) + TensorStrides.SetShapeSizes(110) TensorStrides.SetInt1D(Bodies.Shape().Strides[0], 0) TensorStrides.SetInt1D(Bodies.Shape().Strides[1], 1) TensorStrides.SetInt1D(Joints.Shape().Strides[0], 10) @@ -703,11 +762,14 @@ func ToGPUTensorStrides() { TensorStrides.SetInt1D(Dynamics.Shape().Strides[0], 50) TensorStrides.SetInt1D(Dynamics.Shape().Strides[1], 51) TensorStrides.SetInt1D(Dynamics.Shape().Strides[2], 52) - TensorStrides.SetInt1D(ContactCount.Shape().Strides[0], 60) - TensorStrides.SetInt1D(Contacts.Shape().Strides[0], 70) - TensorStrides.SetInt1D(Contacts.Shape().Strides[1], 71) - TensorStrides.SetInt1D(JointControls.Shape().Strides[0], 80) - TensorStrides.SetInt1D(JointControls.Shape().Strides[1], 81) + TensorStrides.SetInt1D(BroadContactsN.Shape().Strides[0], 60) + TensorStrides.SetInt1D(BroadContacts.Shape().Strides[0], 70) + TensorStrides.SetInt1D(BroadContacts.Shape().Strides[1], 71) + TensorStrides.SetInt1D(ContactsN.Shape().Strides[0], 80) + TensorStrides.SetInt1D(Contacts.Shape().Strides[0], 90) + TensorStrides.SetInt1D(Contacts.Shape().Strides[1], 91) + TensorStrides.SetInt1D(JointControls.Shape().Strides[0], 100) + TensorStrides.SetInt1D(JointControls.Shape().Strides[1], 101) v, _ := syVars.ValueByIndex(0, "TensorStrides", 0) gpu.SetValueFrom(v, TensorStrides.Values) } @@ -739,8 +801,14 @@ func ReadFromGPU(vars ...GPUVars) { case DynamicsVar: v, _ := syVars.ValueByIndex(2, "Dynamics", 0) v.GPUToRead(sy.CommandEncoder) - case ContactCountVar: - v, _ := syVars.ValueByIndex(2, "ContactCount", 0) + case BroadContactsNVar: + v, _ := syVars.ValueByIndex(2, "BroadContactsN", 0) + v.GPUToRead(sy.CommandEncoder) + case BroadContactsVar: + v, _ := syVars.ValueByIndex(2, "BroadContacts", 0) + v.GPUToRead(sy.CommandEncoder) + case ContactsNVar: + v, _ := syVars.ValueByIndex(2, "ContactsN", 0) v.GPUToRead(sy.CommandEncoder) case ContactsVar: v, _ := syVars.ValueByIndex(2, "Contacts", 0) @@ -786,10 +854,18 @@ func SyncFromGPU(vars ...GPUVars) { v, _ := syVars.ValueByIndex(2, "Dynamics", 0) v.ReadSync() gpu.ReadToBytes(v, Dynamics.Values) - case ContactCountVar: - v, _ := syVars.ValueByIndex(2, "ContactCount", 0) + case BroadContactsNVar: + v, _ := syVars.ValueByIndex(2, "BroadContactsN", 0) + v.ReadSync() + gpu.ReadToBytes(v, BroadContactsN.Values) + case BroadContactsVar: + v, _ := syVars.ValueByIndex(2, "BroadContacts", 0) + v.ReadSync() + gpu.ReadToBytes(v, BroadContacts.Values) + case ContactsNVar: + v, _ := syVars.ValueByIndex(2, "ContactsN", 0) v.ReadSync() - gpu.ReadToBytes(v, ContactCount.Values) + gpu.ReadToBytes(v, ContactsN.Values) case ContactsVar: v, _ := syVars.ValueByIndex(2, "Contacts", 0) v.ReadSync() diff --git a/physics/params.go b/physics/params.go index 2d03aa90..6d801721 100644 --- a/physics/params.go +++ b/physics/params.go @@ -46,6 +46,10 @@ type PhysParams struct { // Restitution Restitution slbool.Bool `default:"false"` + // MaxGeomIter is number of iterations to perform in shape-based + // geometry collision computations + MaxGeomIter int32 `default:"10"` + // Contact margin is the extra distance for broadphase collision // around rigid bodies. ContactMargin float32 @@ -78,7 +82,7 @@ type PhysParams struct { // to examine. BodyCollidePairsN int32 `edit:"-"` - pad, pad1, pad2 int32 + pad, pad1 int32 // Gravity is the gravity acceleration function Gravity slvec.Vector3 @@ -95,6 +99,7 @@ func (pr *PhysParams) Defaults() { pr.JointAngularComply = 0 pr.ContactRelax = 0.8 pr.AngularDamping = 0 + pr.MaxGeomIter = 10 pr.ContactWeighting.SetBool(true) pr.Restitution.SetBool(false) pr.ContactMargin = 0.1 diff --git a/physics/shaders/CollisionBroad.wgsl b/physics/shaders/CollisionBroad.wgsl index 879dc5d9..264e36e4 100644 --- a/physics/shaders/CollisionBroad.wgsl +++ b/physics/shaders/CollisionBroad.wgsl @@ -15,9 +15,9 @@ var BodyCollidePairs: array; @group(2) @binding(0) var Dynamics: array; @group(2) @binding(1) -var ContactCount: array>; +var BroadContactsN: array>; @group(2) @binding(2) -var Contacts: array; +var BroadContacts: array; // // JointControls are dynamic joint control inputs, per joint DoF // (in correspondence with [JointDoFs]). This can be uploaded to the // GPU at every step. // [dof][JointControlVarsN] alias GPUVars = i32; @@ -49,9 +49,9 @@ const BodyShape: BodyVars = 0; const BodyDynamic: BodyVars = 1; const BodyWorld: BodyVars = 2; const BodyGroup: BodyVars = 3; -const BodySizeX: BodyVars = 4; -const BodySizeY: BodyVars = 5; -const BodySizeZ: BodyVars = 6; +const BodyHSizeX: BodyVars = 4; +const BodyHSizeY: BodyVars = 5; +const BodyHSizeZ: BodyVars = 6; const BodyThick: BodyVars = 7; const BodyMass: BodyVars = 8; const BodyInvMass: BodyVars = 9; @@ -92,8 +92,8 @@ fn GetBodyShape(idx: i32) -> Shapes { fn GetBodyDynamic(idx: i32) -> i32 { return i32(bitcast(Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyDynamic))])); } -fn BodySize(idx: i32) -> vec3 { - return vec3(Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodySizeX))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodySizeY))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodySizeZ))]); +fn BodyHSize(idx: i32) -> vec3 { + return vec3(Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyHSizeX))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyHSizeY))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyHSizeZ))]); } fn BodyPos(idx: i32) -> vec3 { return vec3(Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyPosX))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyPosY))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyPosZ))]); @@ -125,22 +125,29 @@ const ContactAPointZ: ContactVars = 5; const ContactBPointX: ContactVars = 6; const ContactBPointY: ContactVars = 7; const ContactBPointZ: ContactVars = 8; -const ContactADepth: ContactVars = 9; -const ContactBDepth: ContactVars = 10; -const ContactNormX: ContactVars = 11; -const ContactNormY: ContactVars = 12; -const ContactNormZ: ContactVars = 13; -const ContactForceX: ContactVars = 14; -const ContactForceY: ContactVars = 15; -const ContactForceZ: ContactVars = 16; -fn SetContactA(idx: i32,bodIdx: i32) { - Contacts[Index2D(TensorStrides[70], TensorStrides[71], u32(idx), u32(ContactA))] = bitcast(u32(bodIdx)); +const ContactAOffX: ContactVars = 9; +const ContactAOffY: ContactVars = 10; +const ContactAOffZ: ContactVars = 11; +const ContactBOffX: ContactVars = 12; +const ContactBOffY: ContactVars = 13; +const ContactBOffZ: ContactVars = 14; +const ContactAThick: ContactVars = 15; +const ContactBThick: ContactVars = 16; +const ContactNormX: ContactVars = 17; +const ContactNormY: ContactVars = 18; +const ContactNormZ: ContactVars = 19; +const ContactForceX: ContactVars = 20; +const ContactForceY: ContactVars = 21; +const ContactForceZ: ContactVars = 22; +const BroadContactVarsN = ContactAPointX; +fn SetBroadContactA(idx: i32,bodIdx: i32) { + BroadContacts[Index2D(TensorStrides[70], TensorStrides[71], u32(idx), u32(ContactA))] = bitcast(u32(bodIdx)); } -fn SetContactB(idx: i32,bodIdx: i32) { - Contacts[Index2D(TensorStrides[70], TensorStrides[71], u32(idx), u32(ContactB))] = bitcast(u32(bodIdx)); +fn SetBroadContactB(idx: i32,bodIdx: i32) { + BroadContacts[Index2D(TensorStrides[70], TensorStrides[71], u32(idx), u32(ContactB))] = bitcast(u32(bodIdx)); } -fn SetContactPointIdx(idx: i32,ptIdx: i32) { - Contacts[Index2D(TensorStrides[70], TensorStrides[71], u32(idx), u32(ContactPointIdx))] = bitcast(u32(ptIdx)); +fn SetBroadContactPointIdx(idx: i32,ptIdx: i32) { + BroadContacts[Index2D(TensorStrides[70], TensorStrides[71], u32(idx), u32(ContactPointIdx))] = bitcast(u32(ptIdx)); } fn CollisionBroad(i: u32) { //gosl:kernel let params = Params[0]; @@ -160,7 +167,7 @@ fn CollisionBroad(i: u32) { //gosl:kernel var margin = params.ContactMargin; var infPlane = false; if (sA == Plane) { - var szA = BodySize(biA); + var szA = BodyHSize(biA); if (szA.x == 0) { infPlane = true; } @@ -179,24 +186,24 @@ fn CollisionBroad(i: u32) { //gosl:kernel } var ncB: i32; var ncA = ShapePairContacts(sA, sB, infPlane, &ncB); - var enci = atomicAdd(&ContactCount[i32(params.Cur)], ncA+ncB); + var enci = atomicAdd(&BroadContactsN[0], ncA+ncB); enci += ncA + ncB; // wgsl now matches Go var nci = enci - (ncA + ncB); // starting index if (nci >= params.ContactsMax) { // shouldn't happen! return; } - AddContacts(biA, biB, ci, ncA, ncB); + AddBroadContacts(biA, biB, nci, ncA, ncB); } -fn AddContacts(biA: i32,biB: i32,ci: i32,ncA: i32,ncB: i32) { +fn AddBroadContacts(biA: i32,biB: i32,nci: i32,ncA: i32,ncB: i32) { for (var i=0; i vec4 { //////// import: "enumgen.go" const BodyVarsN: BodyVars = 41; -const ContactVarsN: ContactVars = 17; +const ContactVarsN: ContactVars = 23; const JointControlVarsN: JointControlVars = 3; const DynamicVarsN: DynamicVars = 32; -const GPUVarsN: GPUVars = 10; +const GPUVarsN: GPUVars = 12; const JointTypesN: JointTypes = 7; const JointVarsN: JointVars = 50; const JointDoFVarsN: JointDoFVars = 7; @@ -341,6 +348,7 @@ struct PhysParams { AngularDamping: f32, ContactWeighting: i32, Restitution: i32, + MaxGeomIter: i32, ContactMargin: f32, ContactsMax: i32, Cur: i32, @@ -353,7 +361,6 @@ struct PhysParams { BodyCollidePairsN: i32, pad: i32, pad1: i32, - pad2: i32, Gravity: vec4, } diff --git a/physics/shaders/CollisionNarrow.wgsl b/physics/shaders/CollisionNarrow.wgsl index 3ad2aeac..bd1572a4 100644 --- a/physics/shaders/CollisionNarrow.wgsl +++ b/physics/shaders/CollisionNarrow.wgsl @@ -12,7 +12,13 @@ var Bodies: array; // // Dynamics are the dynamic rigid body elements: these actually move. // Two alternating states are used: Params.Cur and Params.Next. // [dyn body][cur/next][DynamicVarsN] @group(2) @binding(0) var Dynamics: array; +@group(2) @binding(1) +var BroadContactsN: array; @group(2) @binding(2) +var BroadContacts: array; +@group(2) @binding(3) +var ContactsN: array>; +@group(2) @binding(4) var Contacts: array; // // JointControls are dynamic joint control inputs, per joint DoF // (in correspondence with [JointDoFs]). This can be uploaded to the // GPU at every step. // [dof][JointControlVarsN] @@ -32,6 +38,10 @@ fn Index3D(s0: u32, s1: u32, s2: u32, i0: u32, i1: u32, i2: u32) -> u32 { return s0 * i0 + s1 * i1 + s2 * i2; } +fn Index1D(s0: u32, i0: u32) -> u32 { + return s0 * i0; +} + //////// import: "vars.go" @@ -41,9 +51,9 @@ const BodyShape: BodyVars = 0; const BodyDynamic: BodyVars = 1; const BodyWorld: BodyVars = 2; const BodyGroup: BodyVars = 3; -const BodySizeX: BodyVars = 4; -const BodySizeY: BodyVars = 5; -const BodySizeZ: BodyVars = 6; +const BodyHSizeX: BodyVars = 4; +const BodyHSizeY: BodyVars = 5; +const BodyHSizeZ: BodyVars = 6; const BodyThick: BodyVars = 7; const BodyMass: BodyVars = 8; const BodyInvMass: BodyVars = 9; @@ -84,8 +94,8 @@ fn GetBodyShape(idx: i32) -> Shapes { fn GetBodyDynamic(idx: i32) -> i32 { return i32(bitcast(Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyDynamic))])); } -fn BodySize(idx: i32) -> vec3 { - return vec3(Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodySizeX))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodySizeY))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodySizeZ))]); +fn BodyHSize(idx: i32) -> vec3 { + return vec3(Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyHSizeX))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyHSizeY))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyHSizeZ))]); } fn BodyPos(idx: i32) -> vec3 { return vec3(Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyPosX))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyPosY))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyPosZ))]); @@ -117,51 +127,141 @@ const ContactAPointZ: ContactVars = 5; const ContactBPointX: ContactVars = 6; const ContactBPointY: ContactVars = 7; const ContactBPointZ: ContactVars = 8; -const ContactADepth: ContactVars = 9; -const ContactBDepth: ContactVars = 10; -const ContactNormX: ContactVars = 11; -const ContactNormY: ContactVars = 12; -const ContactNormZ: ContactVars = 13; -const ContactForceX: ContactVars = 14; -const ContactForceY: ContactVars = 15; -const ContactForceZ: ContactVars = 16; -fn GetContactA(idx: i32) -> i32 { - return i32(bitcast(Contacts[Index2D(TensorStrides[70], TensorStrides[71], u32(idx), u32(ContactA))])); -} -fn GetContactB(idx: i32) -> i32 { - return i32(bitcast(Contacts[Index2D(TensorStrides[70], TensorStrides[71], u32(idx), u32(ContactB))])); -} -fn GetContactPointIdx(idx: i32) -> i32 { - return i32(bitcast(Contacts[Index2D(TensorStrides[70], TensorStrides[71], u32(idx), u32(ContactPointIdx))])); +const ContactAOffX: ContactVars = 9; +const ContactAOffY: ContactVars = 10; +const ContactAOffZ: ContactVars = 11; +const ContactBOffX: ContactVars = 12; +const ContactBOffY: ContactVars = 13; +const ContactBOffZ: ContactVars = 14; +const ContactAThick: ContactVars = 15; +const ContactBThick: ContactVars = 16; +const ContactNormX: ContactVars = 17; +const ContactNormY: ContactVars = 18; +const ContactNormZ: ContactVars = 19; +const ContactForceX: ContactVars = 20; +const ContactForceY: ContactVars = 21; +const ContactForceZ: ContactVars = 22; +const BroadContactVarsN = ContactAPointX; +fn GetBroadContactA(idx: i32) -> i32 { + return i32(bitcast(BroadContacts[Index2D(TensorStrides[70], TensorStrides[71], u32(idx), u32(ContactA))])); +} +fn GetBroadContactB(idx: i32) -> i32 { + return i32(bitcast(BroadContacts[Index2D(TensorStrides[70], TensorStrides[71], u32(idx), u32(ContactB))])); +} +fn GetBroadContactPointIdx(idx: i32) -> i32 { + return i32(bitcast(BroadContacts[Index2D(TensorStrides[70], TensorStrides[71], + u32(idx), u32(ContactPointIdx))])); +} +fn SetContactA(idx: i32,bodIdx: i32) { + Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactA))] = bitcast(u32(bodIdx)); +} +fn SetContactB(idx: i32,bodIdx: i32) { + Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactB))] = bitcast(u32(bodIdx)); +} +fn SetContactPointIdx(idx: i32,ptIdx: i32) { + Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactPointIdx))] = bitcast(u32(ptIdx)); +} +fn SetContactAPoint(idx: i32, pos: vec3) { + Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactAPointX))] = pos.x; + Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactAPointY))] = pos.y; + Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactAPointZ))] = pos.z; +} +fn SetContactBPoint(idx: i32, pos: vec3) { + Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactBPointX))] = pos.x; + Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactBPointY))] = pos.y; + Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactBPointZ))] = pos.z; +} +fn SetContactAOff(idx: i32, pos: vec3) { + Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactAOffX))] = pos.x; + Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactAOffY))] = pos.y; + Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactAOffZ))] = pos.z; +} +fn SetContactBOff(idx: i32, pos: vec3) { + Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactBOffX))] = pos.x; + Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactBOffY))] = pos.y; + Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactBOffZ))] = pos.z; +} +fn SetContactNorm(idx: i32, pos: vec3) { + Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactNormX))] = pos.x; + Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactNormY))] = pos.y; + Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactNormZ))] = pos.z; } fn CollisionNarrow(i: u32) { //gosl:kernel let params = Params[0]; var ci = i32(i); - if (ci >= params.ContactsMax) { + var cmax = BroadContactsN[0]; + if (ci >= cmax) { return; } - var biA = GetContactA(ci); - var biB = GetContactB(ci); - var cpi = GetContactPointIdx(ci); + var biA = GetBroadContactA(ci); + var biB = GetBroadContactB(ci); + var cpi = GetBroadContactPointIdx(ci); var sA = GetBodyShape(biA); var sB = GetBodyShape(biB); var gdA = NewGeomData(biA, params.Cur, sA); var gdB = NewGeomData(biB, params.Cur, sB); var margin = params.ContactMargin; - var thick = gdA.Thick + gdB.Thick; var dist = f32(1.0e6); + var maxIter = params.MaxGeomIter; var ptA: vec3; var ptB: vec3; var norm: vec3; + var nnorm: vec3; switch (gdA.Shape) { + case Plane: { + switch (gdB.Shape) { + case Sphere: { + dist = ColSpherePlane(cpi, maxIter, &gdB, &gdA, &ptB, &ptA, &nnorm); // reverse + norm = Negate3(nnorm); + } + case Capsule: { + dist = ColCapsulePlane(cpi, maxIter, &gdB, &gdA, &ptB, &ptA, &nnorm); // reverse + norm = Negate3(nnorm); + } + case Cylinder: { + dist = ColCylinderPlane(cpi, maxIter, &gdB, &gdA, &ptB, &ptA, &nnorm); // reverse + norm = Negate3(nnorm); + } + case Box: { + dist = ColBoxPlane(cpi, maxIter, &gdB, &gdA, &ptB, &ptA, &nnorm); // reverse + norm = Negate3(nnorm); + } + default: { + } + } + } case Sphere: { switch (gdB.Shape) { case Sphere: { - dist = ColSphereSphere(cpi, &gdA, &gdB, &ptA, &ptB, &norm); + dist = ColSphereSphere(cpi, maxIter, &gdA, &gdB, &ptA, &ptB, &norm); + } + case Capsule: { + dist = ColSphereCapsule(cpi, maxIter, &gdA, &gdB, &ptA, &ptB, &norm); } case Box: { + dist = ColSphereBox(cpi, maxIter, &gdA, &gdB, &ptA, &ptB, &norm); + } + default: { + } } + } + case Capsule: { + switch (gdB.Shape) { case Capsule: { + dist = ColCapsuleCapsule(cpi, maxIter, &gdA, &gdB, &ptA, &ptB, &norm); + } + case Box: { + dist = ColBoxCapsule(cpi, maxIter, &gdB, &gdA, &ptB, &ptA, &nnorm); // reverse + norm = Negate3(nnorm); + } + default: { + } + } + } + case Box: { + switch (gdB.Shape) { + case Box: { + dist = ColBoxBox(cpi, maxIter, &gdA, &gdB, &ptA, &ptB, &norm); } default: { } @@ -170,6 +270,30 @@ fn CollisionNarrow(i: u32) { //gosl:kernel default: { } } + var ctA: vec3; + var ctB: vec3; + var offA: vec3; + var offB: vec3; + var distActual: f32; + var offMagA: f32; + var offMagB: f32; + var actual = ContactPoints(dist, margin, &gdA, &gdB, ptA, ptB, norm, &ctA, &ctB, &offA, &offB, &distActual, &offMagA, &offMagB); + if (!actual) { + return; + } + var enci = atomicAdd(&ContactsN[0], 1); + enci += i32(1); // wgsl now matches Go + var nci = enci - 1; + SetContactA(nci, biA); + SetContactB(nci, biB); + SetContactPointIdx(nci, cpi); + SetContactAPoint(nci, ctA); + SetContactBPoint(nci, ctB); + SetContactAOff(nci, offA); + SetContactBOff(nci, offB); + SetContactNorm(nci, norm); + Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(nci), u32(ContactAThick))] = offMagA; + Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(nci), u32(ContactBThick))] = offMagB; } //////// import: "control.go" @@ -221,10 +345,10 @@ fn DynamicQuat(idx: i32,cni: i32) -> vec4 { //////// import: "enumgen.go" const BodyVarsN: BodyVars = 41; -const ContactVarsN: ContactVars = 17; +const ContactVarsN: ContactVars = 23; const JointControlVarsN: JointControlVars = 3; const DynamicVarsN: DynamicVars = 32; -const GPUVarsN: GPUVars = 10; +const GPUVarsN: GPUVars = 12; const JointTypesN: JointTypes = 7; const JointVarsN: JointVars = 50; const JointDoFVarsN: JointDoFVars = 7; @@ -313,6 +437,7 @@ struct PhysParams { AngularDamping: f32, ContactWeighting: i32, Restitution: i32, + MaxGeomIter: i32, ContactMargin: f32, ContactsMax: i32, Cur: i32, @@ -325,7 +450,6 @@ struct PhysParams { BodyCollidePairsN: i32, pad: i32, pad1: i32, - pad2: i32, Gravity: vec4, } @@ -346,32 +470,438 @@ fn NewGeomData(bi: i32,cni: i32, shp: Shapes) -> GeomData { var gd: GeomData; gd.BodyIdx = bi; gd.Shape = shp; - gd.Size = BodySize(bi); + gd.Size = BodyHSize(bi); gd.Thick = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyThick))]; gd.MinSize = min(gd.Size.x, gd.Size.y); gd.MinSize = min(gd.MinSize, gd.Size.z); gd.WbR = BodyDynamicPos(bi, cni); gd.WbQ = BodyDynamicQuat(bi, cni); + InitGeomData(bi, &gd);return gd; +} +fn InitGeomData(bi: i32, gd: ptr) { var bwR: vec3; var bwQ: vec4; - SpatialTransformInverse(gd.WbR, gd.WbQ, &bwR, &bwQ); - gd.BwR = bwR; - gd.BwQ = bwQ; - gd.Radius = f32(0); - if (shp == Sphere || shp == Capsule) { // todo: cone is separate - gd.Radius = gd.Size.x; - }return gd; -} -fn ColSphereSphere(cpi: i32, gdA: ptr, gdB: ptr, ptA: ptr>,ptB: ptr>,norm: ptr>) -> f32 { - var ptAw = (*gdA).WbR; - var ptBw = (*gdB).WbR; - var diff = ptAw-(ptBw); - *ptA = ptAw; - *ptB = ptBw; + SpatialTransformInverse((*gd).WbR, (*gd).WbQ, &bwR, &bwQ); + (*gd).BwR = bwR; + (*gd).BwQ = bwQ; + (*gd).Radius = f32(0); + if ((*gd).Shape == Sphere || (*gd).Shape == Capsule || (*gd).Shape == Cone) { + (*gd).Radius = (*gd).Size.x; + } +} +fn ContactPoints(dist: f32,margin: f32, gdA: ptr, gdB: ptr, ptA: vec3,ptB: vec3,norm: vec3, ctA: ptr>,ctB: ptr>,offA: ptr>,offB: ptr>, distActual: ptr,offMagA: ptr,offMagB: ptr) -> bool { + var thick = (*gdA).Thick + (*gdB).Thick; + var totSepReq = (*gdA).Radius + (*gdB).Radius + thick; + *distActual = dist - totSepReq; + if (*distActual >= margin) { + return false; + } + *ctA = MulSpatialPoint((*gdA).BwR, (*gdA).BwQ, ptA); + *ctB = MulSpatialPoint((*gdB).BwR, (*gdB).BwQ, ptB); + *offMagA = (*gdA).Radius + (*gdA).Thick; + *offMagB = (*gdB).Radius + (*gdB).Thick; + *offA = MulQuatVector((*gdA).BwQ, norm*(-(*offMagA))); + *offB = MulQuatVector((*gdB).BwQ, norm*(*offMagB));return true; +} +fn ColSphereSphere(cpi: i32,maxIter: i32, gdA: ptr, gdB: ptr, pA: ptr>,pB: ptr>,norm: ptr>) -> f32 { + var pAw = (*gdA).WbR; + var pBw = (*gdB).WbR; + var diff = pAw-(pBw); + *pA = pAw; + *pB = pBw; *norm = Normal3(diff);return Dot3(diff, *norm); } +fn ColCapsulePlane(cpi: i32,maxIter: i32, gdA: ptr, gdB: ptr, pA: ptr>,pB: ptr>,norm: ptr>) -> f32 { + var pAw: vec3; + var pBw: vec3; + var diff: vec3; + var hh = (*gdA).Size.y; + if (cpi < 2) { // vertex + var side = f32(cpi)*2 - 1; + pAw = MulSpatialPoint((*gdA).WbR, (*gdA).WbQ, vec3(0, side*hh, 0)); + var queryB = MulSpatialPoint((*gdB).BwR, (*gdB).BwQ, pAw); + var pBb = ClosestPointPlane((*gdB).Size.x, (*gdB).Size.z, queryB); + pBw = MulSpatialPoint((*gdA).WbR, (*gdA).WbQ, pBb); + diff = pAw-(pBw); + if ((*gdB).Size.x > 0) { + *norm = Normal3(diff); + } else { + *norm = MulQuatVector((*gdB).WbQ, vec3(0, 1, 0)); + } + } else { // edges of finite plane -- only here if plane is finite + var edge0: vec3; + var edge1: vec3; + PlaneEdge(cpi-2, (*gdB).Size.x, (*gdB).Size.z, &edge0, &edge1); + var edge0w = MulSpatialPoint((*gdB).WbR, (*gdB).WbQ, edge0); + var edge1w = MulSpatialPoint((*gdB).WbR, (*gdB).WbQ, edge1); + var edge0a = MulSpatialPoint((*gdA).BwR, (*gdA).BwQ, edge0w); + var edge1a = MulSpatialPoint((*gdA).BwR, (*gdA).BwQ, edge1w); + var u = ClosestEdgeCapsule((*gdA).Size.x, (*gdA).Size.y, edge0a, edge1a, maxIter); + pBw = edge0w*(1 - u)+(edge1w*(u)); + var p0Aw = MulSpatialPoint((*gdA).WbR, (*gdA).WbQ, vec3(0, hh, 0)); + var p1Aw = MulSpatialPoint((*gdA).WbR, (*gdA).WbQ, vec3(0, -hh, 0)); + pAw = ClosestPointLineSegment(p0Aw, p1Aw, pBw); + diff = pAw-(pBw); + *norm = MulQuatVector((*gdB).WbQ, vec3(0, 1, 0)); + } + *pA = pAw; + *pB = pBw;return Dot3(diff, *norm); +} +fn ColCapsuleCapsule(cpi: i32,maxIter: i32, gdA: ptr, gdB: ptr, pA: ptr>,pB: ptr>,norm: ptr>) -> f32 { + var hhA = (*gdA).Size.y; + var hhB = (*gdB).Size.y; + var e0 = vec3(0.0, 0.0, hhA*f32(cpi%2)); + var e1 = vec3(0.0, 0.0, -hhA*f32((cpi+1)%2)); + var edge0w = MulSpatialPoint((*gdA).WbR, (*gdA).WbQ, e0); + var edge1w = MulSpatialPoint((*gdA).WbR, (*gdA).WbQ, e1); + var edge0b = MulSpatialPoint((*gdA).BwR, (*gdA).BwQ, edge0w); + var edge1b = MulSpatialPoint((*gdA).BwR, (*gdA).BwQ, edge1w); + var u = ClosestEdgeCapsule((*gdB).Size.x, (*gdB).Size.y, edge0b, edge1b, maxIter); + var pAw = edge0w*(1 - u)+(edge1w*(u)); + var p0Bw = MulSpatialPoint((*gdB).WbR, (*gdB).WbQ, vec3(0.0, 0.0, hhB)); + var p1Bw = MulSpatialPoint((*gdB).WbR, (*gdB).WbQ, vec3(0.0, 0.0, -hhB)); + var pBw = ClosestPointLineSegment(p0Bw, p1Bw, pAw); + var diff = pAw-(pBw); + *norm = Normal3(diff); + *pA = pAw; + *pB = pBw;return Dot3(diff, *norm); +} +fn ColBoxBox(cpi: i32,maxIter: i32, gdA: ptr, gdB: ptr, pA: ptr>,pB: ptr>,norm: ptr>) -> f32 { + var edge0: vec3; + var edge1: vec3; + BoxEdge(cpi, (*gdA).Size, &edge0, &edge1); + var edge0w = MulSpatialPoint((*gdA).WbR, (*gdA).WbQ, edge0); + var edge1w = MulSpatialPoint((*gdA).WbR, (*gdA).WbQ, edge1); + var edge0b = MulSpatialPoint((*gdB).BwR, (*gdB).BwQ, edge0w); + var edge1b = MulSpatialPoint((*gdB).BwR, (*gdB).BwQ, edge1w); + var u = ClosestEdgeBox((*gdB).Size, edge0b, edge1b, maxIter); + var pAw = edge0w*(1 - u)+(edge1w*(u)); + var queryB = MulSpatialPoint((*gdB).BwR, (*gdB).BwQ, pAw); + var pBody = ClosestPointBox((*gdB).Size, queryB); + var pBw = MulSpatialPoint((*gdB).WbR, (*gdB).WbQ, pBody); + var diff = pAw-(pBw); + *norm = MulQuatVector((*gdB).WbQ, BoxSDFGrad((*gdB).Size, queryB)); + *pA = pAw; + *pB = pBw;return Dot3(diff, *norm); +} +fn ColBoxCapsule(cpi: i32,maxIter: i32, gdA: ptr, gdB: ptr, pA: ptr>,pB: ptr>,norm: ptr>) -> f32 { + var hhB = (*gdB).Size.y; + var e0 = vec3(0.0, 0.0, -hhB*f32(cpi%2)); + var e1 = vec3(0.0, 0.0, hhB*f32((cpi+1)%2)); + var edge0w = MulSpatialPoint((*gdB).WbR, (*gdB).WbQ, e0); + var edge1w = MulSpatialPoint((*gdB).WbR, (*gdB).WbQ, e1); + var edge0a = MulSpatialPoint((*gdA).BwR, (*gdA).BwQ, edge0w); + var edge1a = MulSpatialPoint((*gdA).BwR, (*gdA).BwQ, edge1w); + var u = ClosestEdgeBox((*gdA).Size, edge0a, edge1a, maxIter); + var pBw = edge0w*(1 - u)+(edge1w*(u)); + var queryA = MulSpatialPoint((*gdA).BwR, (*gdA).BwQ, pBw); + var pABody = ClosestPointBox((*gdA).Size, queryA); + var pAw = MulSpatialPoint((*gdA).WbR, (*gdA).WbQ, pABody); + var diff = pAw-(pBw); + *norm = Negate3(MulQuatVector((*gdA).WbQ, BoxSDFGrad((*gdA).Size, queryA))); + *pA = pAw; + *pB = pBw;return Dot3(diff, *norm); +} +fn ColBoxPlane(cpi: i32,maxIter: i32, gdA: ptr, gdB: ptr, pA: ptr>,pB: ptr>,norm: ptr>) -> f32 { + var width = (*gdB).Size.x; + var length = (*gdB).Size.z; + var pAw: vec3; + var pBw: vec3; + var diff: vec3; + if (cpi < 8) { + var pABody = BoxVertex(cpi, (*gdA).Size); + pAw = MulSpatialPoint((*gdA).WbR, (*gdA).WbQ, pABody); + var queryB = MulSpatialPoint((*gdB).BwR, (*gdB).BwQ, pAw); + var pBody = ClosestPointPlane(width, length, queryB); + pBw = MulSpatialPoint((*gdB).WbR, (*gdB).WbQ, pBody); + diff = pAw-(pBw); + *norm = MulQuatVector((*gdB).WbQ, vec3(0.0, 0.0, 1.0)); + if (width > 0.0 && length > 0.0) { + if (abs(queryB.x) > width || abs(queryB.z) > length) { + return f32( // invalid + 1.0e6); + } + } + } else { + var edge0: vec3; + var edge1: vec3; + PlaneEdge(cpi-8, width, length, &edge0, &edge1); + var edge0w = MulSpatialPoint((*gdB).WbR, (*gdB).WbQ, edge0); + var edge1w = MulSpatialPoint((*gdB).WbR, (*gdB).WbQ, edge1); + var edge0a = MulSpatialPoint((*gdA).BwR, (*gdA).BwQ, edge0w); + var edge1a = MulSpatialPoint((*gdA).BwR, (*gdA).BwQ, edge1w); + var u = ClosestEdgeBox((*gdA).Size, edge0a, edge1a, maxIter); + pBw = edge0w*(1 - u)+(edge1w*(u)); + var queryA = MulSpatialPoint((*gdA).BwR, (*gdA).BwQ, pBw); + var pABody = ClosestPointBox((*gdA).Size, queryA); + pAw = MulSpatialPoint((*gdA).WbR, (*gdA).WbQ, pABody); + var queryB = MulSpatialPoint((*gdA).BwR, (*gdA).BwQ, pAw); + if (abs(queryB.x) > width || abs(queryB.z) > length) { + return f32( // invalid + 1.0e6); + } + diff = pAw-(pBw); + var comA = (*gdA).WbR; + queryB = MulSpatialPoint((*gdB).BwR, (*gdB).BwQ, comA); + if (abs(queryB.x) > width || abs(queryB.z) > length) { + *norm = Normal3(comA-(pBw)); + } else { + *norm = MulQuatVector((*gdB).WbQ, vec3(0.0, 0.0, 1.0)); + } + } + *pA = pAw; + *pB = pBw;return Dot3(diff, *norm); +} +fn ColSphereBox(cpi: i32,maxIter: i32, gdA: ptr, gdB: ptr, pA: ptr>,pB: ptr>,norm: ptr>) -> f32 { + var pAw = (*gdA).WbR; + var pABody = MulSpatialPoint((*gdB).BwR, (*gdB).BwQ, pAw); + var pBody = ClosestPointBox((*gdB).Size, pABody); + var pBw = MulSpatialPoint((*gdB).WbR, (*gdB).WbQ, pBody); + var diff = pAw-(pBw); + *norm = Normal3(diff); + *pA = pAw; + *pB = pBw;return Dot3(diff, *norm); +} +fn ColSphereCapsule(cpi: i32,maxIter: i32, gdA: ptr, gdB: ptr, pA: ptr>,pB: ptr>,norm: ptr>) -> f32 { + var pAw = (*gdA).WbR; + var hhB = (*gdB).Size.y; + var AB = MulSpatialPoint((*gdB).WbR, (*gdB).WbQ, vec3(0.0, 0.0, hhB)); + var BB = MulSpatialPoint((*gdB).WbR, (*gdB).WbQ, vec3(0.0, 0.0, -hhB)); + var pBw = ClosestPointLineSegment(AB, BB, pAw); + var diff = pAw-(pBw); + *norm = Normal3(diff); + *pA = pAw; + *pB = pBw;return Dot3(diff, *norm); +} +fn ColSpherePlane(cpi: i32,maxIter: i32, gdA: ptr, gdB: ptr, pA: ptr>,pB: ptr>,norm: ptr>) -> f32 { + var pAw = (*gdA).WbR; + var pBody = ClosestPointPlane((*gdB).Size.x, (*gdB).Size.z, MulSpatialPoint((*gdB).BwR, (*gdB).BwQ, pAw)); + var pBw = MulSpatialPoint((*gdB).WbR, (*gdB).WbQ, pBody); + var diff = pAw-(pBw); + *norm = MulQuatVector((*gdB).WbQ, vec3(0.0, 0.0, 1.0)); + *pA = pAw; + *pB = pBw;return Dot3(diff, *norm); +} +fn ColCylinderPlane(cpi: i32,maxIter: i32, gdA: ptr, gdB: ptr, pA: ptr>,pB: ptr>,norm: ptr>) -> f32 { + var plNorm = MulQuatVector((*gdB).WbQ, vec3(0.0, 1.0, 0.0)); + var plPos = MulSpatialPoint((*gdB).WbR, (*gdB).WbQ, vec3(0.0, 0.0, 0.0)); + var cylCtr = MulSpatialPoint((*gdA).WbR, (*gdA).WbQ, vec3(0.0, 0.0, 0.0)); + var cylAx = Normal3(MulQuatVector((*gdA).WbQ, vec3(0.0, 0.0, 1.0))); + var cylRad = (*gdA).Size.x; + var cylHh = (*gdA).Size.y; + var dist: f32; + var pos: vec3; + var n = plNorm; + var axis = cylAx; + var prjaxis = Dot3(n, axis); + if (prjaxis > 0) { + axis = Negate3(axis); + prjaxis = -prjaxis; + } + var dist0 = Dot3(cylCtr-(plPos), n); + var vec = axis*(prjaxis)-(n); + var prjvec = Dot3(vec, n); + axis = axis*(cylHh); + prjaxis *= cylHh; + switch (cpi) { + case 0: { // First contact point (end cap closer to plane) + dist = dist0 + prjaxis + prjvec; + pos = cylCtr+(vec)+(axis)-(n*(dist * 0.5)); + } + case 1: { // Second contact point (end cap farther from plane) + dist = dist0 - prjaxis + prjvec; + pos = cylCtr+(vec)-(axis)-(n*(dist * 0.5)); + } + case 2, 3: { // Try triangle contact points on side closer to plane + var prjvec1 = prjvec * -0.5; + dist = dist0 + prjaxis + prjvec1; + var vec1 = Cross3(vec, axis); + vec1 = Normal3(vec1)*(cylRad * sqrt(3.0) * 0.5); + var pextra = vec1+(axis)-(vec*(0.5))-(n*(dist * 0.5)); + pos = cylCtr+(pextra); + if (cpi == 3) { // Add contact point B - adjust to closest side + pos = cylCtr-(pextra); + } + } + default: { + } + } + *pA = pos+(n*(dist * 0.5)); + *pB = pos-(n*(dist * 0.5));return dist; +} //////// import: "shapegeom.go" +fn BoxSDF(upper: vec3,p: vec3) -> f32 { + var qx = abs(p.x) - upper.x; + var qy = abs(p.y) - upper.y; + var qz = abs(p.z) - upper.z; + var e = vec3(max(qx, 0.0), max(qy, 0.0), max(qz, 0.0));return Length3(e) + min(max(qx, max(qy, qz)), 0.0); +} +fn BoxSDFGrad(upper: vec3,p: vec3) -> vec3 { + var qx = abs(p.x) - upper.x; + var qy = abs(p.y) - upper.y; + var qz = abs(p.z) - upper.z; + if (qx > 0.0 || qy > 0.0 || qz > 0.0) { + var x = clamp(p.x, -upper.x, upper.x); + var y = clamp(p.y, -upper.y, upper.y); + var z = clamp(p.z, -upper.z, upper.z);return Normal3(p-(vec3(x, y, z))); + } + var sx = sign(p.x); + var sy = sign(p.y); + var sz = sign(p.z); + if ((qx > qy && qx > qz) || (qy == 0.0 && qz == 0.0)) { + return vec3(sx, 0.0, 0.0); + } + if ((qy > qx && qy > qz) || (qx == 0.0 && qz == 0.0)) { + return vec3(0.0, sy, 0.0); + }return vec3(// z projection +0.0, 0.0, sz); +} +fn CylinderSDF(radius: f32,hh: f32, p: vec3) -> f32 { + var dx = Length3(vec3(p.x, 0.0, p.z)) - radius; + var dy = abs(p.y) - hh;return min(max(dx, dy), 0.0) + Length2(vec2(max(dx, 0.0), max(dy, 0.0))); +} +fn ClosestPointPlane(width: f32,length: f32, pt: vec3) -> vec3 { + var cp = pt; + if (width == 0.0) { + cp.y = f32(0);return cp; + } + cp.x = clamp(pt.x, -width, width); + cp.z = clamp(pt.z, -length, length);return cp; +} +fn ClosestPointLineSegment(a: vec3,b: vec3,pt: vec3) -> vec3 { + var ab = b-(a); + var ap = pt-(a); + var t = Dot3(ap, ab) / Dot3(ab, ab); + t = clamp(t, 0.0, 1.0);return a+(ab*(t)); +} +fn ClosestPointBox(upper: vec3,pt: vec3) -> vec3 { + var x = clamp(pt.x, -upper.x, upper.x); + var y = clamp(pt.y, -upper.y, upper.y); + var z = clamp(pt.z, -upper.z, upper.z); + if (abs(pt.x) <= upper.x && abs(pt.y) <= upper.y && abs(pt.z) <= upper.z) { + var sx = abs(abs(pt.x) - upper.x); + var sy = abs(abs(pt.y) - upper.y); + var sz = abs(abs(pt.z) - upper.z); + if ((sx < sy && sx < sz) || (sy == 0.0 && sz == 0.0)) { + x = sign(pt.x) * upper.x; + } else if ((sy < sx && sy < sz) || (sx == 0.0 && sz == 0.0)) { + y = sign(pt.y) * upper.y; + } else { + z = sign(pt.z) * upper.z; + } + }return vec3(x, y, z); +} +fn BoxVertex(ptId: i32, upper: vec3) -> vec3 { + var sign_x = f32(ptId%2)*2.0 - 1.0; + var sign_y = f32((ptId/2)%2)*2.0 - 1.0; + var sign_z = f32((ptId/4)%2)*2.0 - 1.0;return vec3(sign_x*upper.x, sign_y*upper.y, sign_z*upper.z); +} +fn BoxEdge(edgeId: i32, upper: vec3, edge0: ptr>,edge1: ptr>) { + var eid = edgeId; + if (eid < 4) { + var i = eid * 2; + var j = i + 1; + *edge0 = BoxVertex(i, upper); + *edge1 = BoxVertex(j, upper); + } else if (eid < 8) { + eid -= i32(4); + var i = eid%2 + eid; // 2 * 4 + var j = i + 2; + *edge0 = BoxVertex(i, upper); + *edge1 = BoxVertex(j, upper); + } + eid -= i32(8); + var i = eid; + var j = i + 4; + *edge0 = BoxVertex(i, upper); + *edge1 = BoxVertex(j, upper); +} +fn PlaneEdge(edgeId: i32, width: f32,length: f32, edge0: ptr>,edge1: ptr>) { + var p0x = (2*f32(edgeId%2) - 1) * width; + var p0z = (2*f32(edgeId/2) - 1) * length; + var p1x: f32; + var p1z: f32; + if (edgeId == 0 || edgeId == 3) { + p1x = p0x; + p1z = -p0z; + } else { + p1x = -p0x; + p1z = p0z; + } + *edge0 = vec3(p0x, 0, p0z); + *edge1 = vec3(p1x, 0, p1z); +} +fn ClosestEdgeBox(upper: vec3,edgeA: vec3,edgeB: vec3, maxIter: i32) -> f32 { + var a = f32(0.0); + var b = f32(1.0); + var h = b - a; + var invphi = f32(0.61803398875); // 1 / phi + var invphi2 = f32(0.38196601125); // 1 / phi^2 + var c = a + invphi2*h; + var d = a + invphi*h; + var query = edgeA*(1.0 - c)+(edgeB*(c)); + var yc = BoxSDF(upper, query); + query = edgeA*(1.0 - d)+(edgeB*(d)); + var yd = BoxSDF(upper, query); + for (var i=0; i yd to find the maximum + b = d; + d = c; + yd = yc; + h = invphi * h; + c = a + invphi2*h; + query = edgeA*(1.0 - c)+(edgeB*(c)); + yc = BoxSDF(upper, query); + } else { + a = c; + c = d; + yc = yd; + h = invphi * h; + d = a + invphi*h; + query = edgeA*(1.0 - d)+(edgeB*(d)); + yd = BoxSDF(upper, query); + } + } + if (yc < yd) { + return 0.5 * (a + d); + }return 0.5 * (c + b); +} +fn ClosestEdgeCapsule(radius: f32,hh: f32, edgeA: vec3,edgeB: vec3, maxIter: i32) -> f32 { + var a = f32(0.0); + var b = f32(1.0); + var h = b - a; + var invphi = f32(0.61803398875); // 1 / phi + var invphi2 = f32(0.38196601125); // 1 / phi^2 + var c = a + invphi2*h; + var d = a + invphi*h; + var query = edgeA*(1.0 - c)+(edgeB*(c)); + var yc = CylinderSDF(radius, hh, query); + query = edgeA*(1.0 - d)+(edgeB*(d)); + var yd = CylinderSDF(radius, hh, query); + for (var i=0; i yd to find the maximum + b = d; + d = c; + yd = yc; + h = invphi * h; + c = a + invphi2*h; + query = edgeA*(1.0 - c)+(edgeB*(c)); + yc = CylinderSDF(radius, hh, query); + } else { + a = c; + c = d; + yc = yd; + h = invphi * h; + d = a + invphi*h; + query = edgeA*(1.0 - d)+(edgeB*(d)); + yd = CylinderSDF(radius, hh, query); + } + } + if (yc < yd) { + return 0.5 * (a + d); + }return 0.5 * (c + b); +} //////// import: "shapes.go" alias Shapes = i32; //enums:enum @@ -408,6 +938,9 @@ fn MulQuatVector(q: vec4, v: vec3) -> vec3 { var xyz = vec3(q.x, q.y, q.z); var t = Cross3(xyz, v)*(2);return v+(t*(q.w))+(Cross3(xyz, t)); } +fn MulSpatialPoint(xP: vec3, xQ: vec4, p: vec3) -> vec3 { + var dp = MulQuatVector(xQ, p);return dp+(xP); +} fn SpatialTransformInverse(p: vec3, q: vec4, oP: ptr>, oQ: ptr>) { var qi = QuatInverse(q); *oP = Negate3(MulQuatVector(qi, p)); @@ -421,6 +954,9 @@ fn QuatInverse(q: vec4) -> vec4 { } //////// import: "slmath-vector2.go" +fn Length2(v: vec2) -> f32 { + return sqrt(v.x*v.x + v.y*v.y); +} //////// import: "slmath-vector3.go" fn Negate3(v: vec3) -> vec3 { diff --git a/physics/shaders/DeltasFromJoints.wgsl b/physics/shaders/DeltasFromJoints.wgsl index 24ad6e71..4d9d47cd 100644 --- a/physics/shaders/DeltasFromJoints.wgsl +++ b/physics/shaders/DeltasFromJoints.wgsl @@ -41,9 +41,9 @@ const BodyShape: BodyVars = 0; const BodyDynamic: BodyVars = 1; const BodyWorld: BodyVars = 2; const BodyGroup: BodyVars = 3; -const BodySizeX: BodyVars = 4; -const BodySizeY: BodyVars = 5; -const BodySizeZ: BodyVars = 6; +const BodyHSizeX: BodyVars = 4; +const BodyHSizeY: BodyVars = 5; +const BodyHSizeZ: BodyVars = 6; const BodyThick: BodyVars = 7; const BodyMass: BodyVars = 8; const BodyInvMass: BodyVars = 9; @@ -90,14 +90,21 @@ const ContactAPointZ: ContactVars = 5; const ContactBPointX: ContactVars = 6; const ContactBPointY: ContactVars = 7; const ContactBPointZ: ContactVars = 8; -const ContactADepth: ContactVars = 9; -const ContactBDepth: ContactVars = 10; -const ContactNormX: ContactVars = 11; -const ContactNormY: ContactVars = 12; -const ContactNormZ: ContactVars = 13; -const ContactForceX: ContactVars = 14; -const ContactForceY: ContactVars = 15; -const ContactForceZ: ContactVars = 16; +const ContactAOffX: ContactVars = 9; +const ContactAOffY: ContactVars = 10; +const ContactAOffZ: ContactVars = 11; +const ContactBOffX: ContactVars = 12; +const ContactBOffY: ContactVars = 13; +const ContactBOffZ: ContactVars = 14; +const ContactAThick: ContactVars = 15; +const ContactBThick: ContactVars = 16; +const ContactNormX: ContactVars = 17; +const ContactNormY: ContactVars = 18; +const ContactNormZ: ContactVars = 19; +const ContactForceX: ContactVars = 20; +const ContactForceY: ContactVars = 21; +const ContactForceZ: ContactVars = 22; +const BroadContactVarsN = ContactAPointX; //////// import: "control.go" alias JointControlVars = i32; //enums:enum @@ -152,10 +159,10 @@ fn SetDynamicAngDelta(idx: i32,cni: i32, angDelta: vec3) { //////// import: "enumgen.go" const BodyVarsN: BodyVars = 41; -const ContactVarsN: ContactVars = 17; +const ContactVarsN: ContactVars = 23; const JointControlVarsN: JointControlVars = 3; const DynamicVarsN: DynamicVars = 32; -const GPUVarsN: GPUVars = 10; +const GPUVarsN: GPUVars = 12; const JointTypesN: JointTypes = 7; const JointVarsN: JointVars = 50; const JointDoFVarsN: JointDoFVars = 7; @@ -256,6 +263,7 @@ struct PhysParams { AngularDamping: f32, ContactWeighting: i32, Restitution: i32, + MaxGeomIter: i32, ContactMargin: f32, ContactsMax: i32, Cur: i32, @@ -268,7 +276,6 @@ struct PhysParams { BodyCollidePairsN: i32, pad: i32, pad1: i32, - pad2: i32, Gravity: vec4, } diff --git a/physics/shaders/DynamicsCurToNext.wgsl b/physics/shaders/DynamicsCurToNext.wgsl index 34dce5f5..96657ffb 100644 --- a/physics/shaders/DynamicsCurToNext.wgsl +++ b/physics/shaders/DynamicsCurToNext.wgsl @@ -33,9 +33,9 @@ const BodyShape: BodyVars = 0; const BodyDynamic: BodyVars = 1; const BodyWorld: BodyVars = 2; const BodyGroup: BodyVars = 3; -const BodySizeX: BodyVars = 4; -const BodySizeY: BodyVars = 5; -const BodySizeZ: BodyVars = 6; +const BodyHSizeX: BodyVars = 4; +const BodyHSizeY: BodyVars = 5; +const BodyHSizeZ: BodyVars = 6; const BodyThick: BodyVars = 7; const BodyMass: BodyVars = 8; const BodyInvMass: BodyVars = 9; @@ -82,14 +82,21 @@ const ContactAPointZ: ContactVars = 5; const ContactBPointX: ContactVars = 6; const ContactBPointY: ContactVars = 7; const ContactBPointZ: ContactVars = 8; -const ContactADepth: ContactVars = 9; -const ContactBDepth: ContactVars = 10; -const ContactNormX: ContactVars = 11; -const ContactNormY: ContactVars = 12; -const ContactNormZ: ContactVars = 13; -const ContactForceX: ContactVars = 14; -const ContactForceY: ContactVars = 15; -const ContactForceZ: ContactVars = 16; +const ContactAOffX: ContactVars = 9; +const ContactAOffY: ContactVars = 10; +const ContactAOffZ: ContactVars = 11; +const ContactBOffX: ContactVars = 12; +const ContactBOffY: ContactVars = 13; +const ContactBOffZ: ContactVars = 14; +const ContactAThick: ContactVars = 15; +const ContactBThick: ContactVars = 16; +const ContactNormX: ContactVars = 17; +const ContactNormY: ContactVars = 18; +const ContactNormZ: ContactVars = 19; +const ContactForceX: ContactVars = 20; +const ContactForceY: ContactVars = 21; +const ContactForceZ: ContactVars = 22; +const BroadContactVarsN = ContactAPointX; //////// import: "control.go" alias JointControlVars = i32; //enums:enum @@ -134,10 +141,10 @@ const DynAngDeltaZ: DynamicVars = 31; //////// import: "enumgen.go" const BodyVarsN: BodyVars = 41; -const ContactVarsN: ContactVars = 17; +const ContactVarsN: ContactVars = 23; const JointControlVarsN: JointControlVars = 3; const DynamicVarsN: DynamicVars = 32; -const GPUVarsN: GPUVars = 10; +const GPUVarsN: GPUVars = 12; const JointTypesN: JointTypes = 7; const JointVarsN: JointVars = 50; const JointDoFVarsN: JointDoFVars = 7; @@ -226,6 +233,7 @@ struct PhysParams { AngularDamping: f32, ContactWeighting: i32, Restitution: i32, + MaxGeomIter: i32, ContactMargin: f32, ContactsMax: i32, Cur: i32, @@ -238,7 +246,6 @@ struct PhysParams { BodyCollidePairsN: i32, pad: i32, pad1: i32, - pad2: i32, Gravity: vec4, } diff --git a/physics/shaders/ForcesFromJoints.wgsl b/physics/shaders/ForcesFromJoints.wgsl index 26ac5cef..7feb8aca 100644 --- a/physics/shaders/ForcesFromJoints.wgsl +++ b/physics/shaders/ForcesFromJoints.wgsl @@ -41,9 +41,9 @@ const BodyShape: BodyVars = 0; const BodyDynamic: BodyVars = 1; const BodyWorld: BodyVars = 2; const BodyGroup: BodyVars = 3; -const BodySizeX: BodyVars = 4; -const BodySizeY: BodyVars = 5; -const BodySizeZ: BodyVars = 6; +const BodyHSizeX: BodyVars = 4; +const BodyHSizeY: BodyVars = 5; +const BodyHSizeZ: BodyVars = 6; const BodyThick: BodyVars = 7; const BodyMass: BodyVars = 8; const BodyInvMass: BodyVars = 9; @@ -90,14 +90,21 @@ const ContactAPointZ: ContactVars = 5; const ContactBPointX: ContactVars = 6; const ContactBPointY: ContactVars = 7; const ContactBPointZ: ContactVars = 8; -const ContactADepth: ContactVars = 9; -const ContactBDepth: ContactVars = 10; -const ContactNormX: ContactVars = 11; -const ContactNormY: ContactVars = 12; -const ContactNormZ: ContactVars = 13; -const ContactForceX: ContactVars = 14; -const ContactForceY: ContactVars = 15; -const ContactForceZ: ContactVars = 16; +const ContactAOffX: ContactVars = 9; +const ContactAOffY: ContactVars = 10; +const ContactAOffZ: ContactVars = 11; +const ContactBOffX: ContactVars = 12; +const ContactBOffY: ContactVars = 13; +const ContactBOffZ: ContactVars = 14; +const ContactAThick: ContactVars = 15; +const ContactBThick: ContactVars = 16; +const ContactNormX: ContactVars = 17; +const ContactNormY: ContactVars = 18; +const ContactNormZ: ContactVars = 19; +const ContactForceX: ContactVars = 20; +const ContactForceY: ContactVars = 21; +const ContactForceZ: ContactVars = 22; +const BroadContactVarsN = ContactAPointX; //////// import: "control.go" alias JointControlVars = i32; //enums:enum @@ -152,10 +159,10 @@ fn SetDynamicTorque(idx: i32,cni: i32, torque: vec3) { //////// import: "enumgen.go" const BodyVarsN: BodyVars = 41; -const ContactVarsN: ContactVars = 17; +const ContactVarsN: ContactVars = 23; const JointControlVarsN: JointControlVars = 3; const DynamicVarsN: DynamicVars = 32; -const GPUVarsN: GPUVars = 10; +const GPUVarsN: GPUVars = 12; const JointTypesN: JointTypes = 7; const JointVarsN: JointVars = 50; const JointDoFVarsN: JointDoFVars = 7; @@ -256,6 +263,7 @@ struct PhysParams { AngularDamping: f32, ContactWeighting: i32, Restitution: i32, + MaxGeomIter: i32, ContactMargin: f32, ContactsMax: i32, Cur: i32, @@ -268,7 +276,6 @@ struct PhysParams { BodyCollidePairsN: i32, pad: i32, pad1: i32, - pad2: i32, Gravity: vec4, } diff --git a/physics/shaders/InitDynamics.wgsl b/physics/shaders/InitDynamics.wgsl index 0ba45597..d8636ea7 100644 --- a/physics/shaders/InitDynamics.wgsl +++ b/physics/shaders/InitDynamics.wgsl @@ -39,9 +39,9 @@ const BodyShape: BodyVars = 0; const BodyDynamic: BodyVars = 1; const BodyWorld: BodyVars = 2; const BodyGroup: BodyVars = 3; -const BodySizeX: BodyVars = 4; -const BodySizeY: BodyVars = 5; -const BodySizeZ: BodyVars = 6; +const BodyHSizeX: BodyVars = 4; +const BodyHSizeY: BodyVars = 5; +const BodyHSizeZ: BodyVars = 6; const BodyThick: BodyVars = 7; const BodyMass: BodyVars = 8; const BodyInvMass: BodyVars = 9; @@ -88,14 +88,21 @@ const ContactAPointZ: ContactVars = 5; const ContactBPointX: ContactVars = 6; const ContactBPointY: ContactVars = 7; const ContactBPointZ: ContactVars = 8; -const ContactADepth: ContactVars = 9; -const ContactBDepth: ContactVars = 10; -const ContactNormX: ContactVars = 11; -const ContactNormY: ContactVars = 12; -const ContactNormZ: ContactVars = 13; -const ContactForceX: ContactVars = 14; -const ContactForceY: ContactVars = 15; -const ContactForceZ: ContactVars = 16; +const ContactAOffX: ContactVars = 9; +const ContactAOffY: ContactVars = 10; +const ContactAOffZ: ContactVars = 11; +const ContactBOffX: ContactVars = 12; +const ContactBOffY: ContactVars = 13; +const ContactBOffZ: ContactVars = 14; +const ContactAThick: ContactVars = 15; +const ContactBThick: ContactVars = 16; +const ContactNormX: ContactVars = 17; +const ContactNormY: ContactVars = 18; +const ContactNormZ: ContactVars = 19; +const ContactForceX: ContactVars = 20; +const ContactForceY: ContactVars = 21; +const ContactForceZ: ContactVars = 22; +const BroadContactVarsN = ContactAPointX; //////// import: "control.go" alias JointControlVars = i32; //enums:enum @@ -143,10 +150,10 @@ fn DynamicBody(idx: i32) -> i32 { //////// import: "enumgen.go" const BodyVarsN: BodyVars = 41; -const ContactVarsN: ContactVars = 17; +const ContactVarsN: ContactVars = 23; const JointControlVarsN: JointControlVars = 3; const DynamicVarsN: DynamicVars = 32; -const GPUVarsN: GPUVars = 10; +const GPUVarsN: GPUVars = 12; const JointTypesN: JointTypes = 7; const JointVarsN: JointVars = 50; const JointDoFVarsN: JointDoFVars = 7; @@ -235,6 +242,7 @@ struct PhysParams { AngularDamping: f32, ContactWeighting: i32, Restitution: i32, + MaxGeomIter: i32, ContactMargin: f32, ContactsMax: i32, Cur: i32, @@ -247,7 +255,6 @@ struct PhysParams { BodyCollidePairsN: i32, pad: i32, pad1: i32, - pad2: i32, Gravity: vec4, } diff --git a/physics/shaders/StepBodyDeltas.wgsl b/physics/shaders/StepBodyDeltas.wgsl index e0216b77..6d4e0620 100644 --- a/physics/shaders/StepBodyDeltas.wgsl +++ b/physics/shaders/StepBodyDeltas.wgsl @@ -39,9 +39,9 @@ const BodyShape: BodyVars = 0; const BodyDynamic: BodyVars = 1; const BodyWorld: BodyVars = 2; const BodyGroup: BodyVars = 3; -const BodySizeX: BodyVars = 4; -const BodySizeY: BodyVars = 5; -const BodySizeZ: BodyVars = 6; +const BodyHSizeX: BodyVars = 4; +const BodyHSizeY: BodyVars = 5; +const BodyHSizeZ: BodyVars = 6; const BodyThick: BodyVars = 7; const BodyMass: BodyVars = 8; const BodyInvMass: BodyVars = 9; @@ -101,14 +101,21 @@ const ContactAPointZ: ContactVars = 5; const ContactBPointX: ContactVars = 6; const ContactBPointY: ContactVars = 7; const ContactBPointZ: ContactVars = 8; -const ContactADepth: ContactVars = 9; -const ContactBDepth: ContactVars = 10; -const ContactNormX: ContactVars = 11; -const ContactNormY: ContactVars = 12; -const ContactNormZ: ContactVars = 13; -const ContactForceX: ContactVars = 14; -const ContactForceY: ContactVars = 15; -const ContactForceZ: ContactVars = 16; +const ContactAOffX: ContactVars = 9; +const ContactAOffY: ContactVars = 10; +const ContactAOffZ: ContactVars = 11; +const ContactBOffX: ContactVars = 12; +const ContactBOffY: ContactVars = 13; +const ContactBOffZ: ContactVars = 14; +const ContactAThick: ContactVars = 15; +const ContactBThick: ContactVars = 16; +const ContactNormX: ContactVars = 17; +const ContactNormY: ContactVars = 18; +const ContactNormZ: ContactVars = 19; +const ContactForceX: ContactVars = 20; +const ContactForceY: ContactVars = 21; +const ContactForceZ: ContactVars = 22; +const BroadContactVarsN = ContactAPointX; //////// import: "control.go" alias JointControlVars = i32; //enums:enum @@ -189,10 +196,10 @@ fn SetDynamicAngDelta(idx: i32,cni: i32, angDelta: vec3) { //////// import: "enumgen.go" const BodyVarsN: BodyVars = 41; -const ContactVarsN: ContactVars = 17; +const ContactVarsN: ContactVars = 23; const JointControlVarsN: JointControlVars = 3; const DynamicVarsN: DynamicVars = 32; -const GPUVarsN: GPUVars = 10; +const GPUVarsN: GPUVars = 12; const JointTypesN: JointTypes = 7; const JointVarsN: JointVars = 50; const JointDoFVarsN: JointDoFVars = 7; @@ -281,6 +288,7 @@ struct PhysParams { AngularDamping: f32, ContactWeighting: i32, Restitution: i32, + MaxGeomIter: i32, ContactMargin: f32, ContactsMax: i32, Cur: i32, @@ -293,7 +301,6 @@ struct PhysParams { BodyCollidePairsN: i32, pad: i32, pad1: i32, - pad2: i32, Gravity: vec4, } diff --git a/physics/shaders/StepIntegrateBodies.wgsl b/physics/shaders/StepIntegrateBodies.wgsl index b751db10..100b4074 100644 --- a/physics/shaders/StepIntegrateBodies.wgsl +++ b/physics/shaders/StepIntegrateBodies.wgsl @@ -39,9 +39,9 @@ const BodyShape: BodyVars = 0; const BodyDynamic: BodyVars = 1; const BodyWorld: BodyVars = 2; const BodyGroup: BodyVars = 3; -const BodySizeX: BodyVars = 4; -const BodySizeY: BodyVars = 5; -const BodySizeZ: BodyVars = 6; +const BodyHSizeX: BodyVars = 4; +const BodyHSizeY: BodyVars = 5; +const BodyHSizeZ: BodyVars = 6; const BodyThick: BodyVars = 7; const BodyMass: BodyVars = 8; const BodyInvMass: BodyVars = 9; @@ -101,14 +101,21 @@ const ContactAPointZ: ContactVars = 5; const ContactBPointX: ContactVars = 6; const ContactBPointY: ContactVars = 7; const ContactBPointZ: ContactVars = 8; -const ContactADepth: ContactVars = 9; -const ContactBDepth: ContactVars = 10; -const ContactNormX: ContactVars = 11; -const ContactNormY: ContactVars = 12; -const ContactNormZ: ContactVars = 13; -const ContactForceX: ContactVars = 14; -const ContactForceY: ContactVars = 15; -const ContactForceZ: ContactVars = 16; +const ContactAOffX: ContactVars = 9; +const ContactAOffY: ContactVars = 10; +const ContactAOffZ: ContactVars = 11; +const ContactBOffX: ContactVars = 12; +const ContactBOffY: ContactVars = 13; +const ContactBOffZ: ContactVars = 14; +const ContactAThick: ContactVars = 15; +const ContactBThick: ContactVars = 16; +const ContactNormX: ContactVars = 17; +const ContactNormY: ContactVars = 18; +const ContactNormZ: ContactVars = 19; +const ContactForceX: ContactVars = 20; +const ContactForceY: ContactVars = 21; +const ContactForceZ: ContactVars = 22; +const BroadContactVarsN = ContactAPointX; //////// import: "control.go" alias JointControlVars = i32; //enums:enum @@ -195,10 +202,10 @@ fn SetDynamicAngDelta(idx: i32,cni: i32, angDelta: vec3) { //////// import: "enumgen.go" const BodyVarsN: BodyVars = 41; -const ContactVarsN: ContactVars = 17; +const ContactVarsN: ContactVars = 23; const JointControlVarsN: JointControlVars = 3; const DynamicVarsN: DynamicVars = 32; -const GPUVarsN: GPUVars = 10; +const GPUVarsN: GPUVars = 12; const JointTypesN: JointTypes = 7; const JointVarsN: JointVars = 50; const JointDoFVarsN: JointDoFVars = 7; @@ -287,6 +294,7 @@ struct PhysParams { AngularDamping: f32, ContactWeighting: i32, Restitution: i32, + MaxGeomIter: i32, ContactMargin: f32, ContactsMax: i32, Cur: i32, @@ -299,7 +307,6 @@ struct PhysParams { BodyCollidePairsN: i32, pad: i32, pad1: i32, - pad2: i32, Gravity: vec4, } diff --git a/physics/shaders/StepJointForces.wgsl b/physics/shaders/StepJointForces.wgsl index f533135f..d78f33b2 100644 --- a/physics/shaders/StepJointForces.wgsl +++ b/physics/shaders/StepJointForces.wgsl @@ -45,9 +45,9 @@ const BodyShape: BodyVars = 0; const BodyDynamic: BodyVars = 1; const BodyWorld: BodyVars = 2; const BodyGroup: BodyVars = 3; -const BodySizeX: BodyVars = 4; -const BodySizeY: BodyVars = 5; -const BodySizeZ: BodyVars = 6; +const BodyHSizeX: BodyVars = 4; +const BodyHSizeY: BodyVars = 5; +const BodyHSizeZ: BodyVars = 6; const BodyThick: BodyVars = 7; const BodyMass: BodyVars = 8; const BodyInvMass: BodyVars = 9; @@ -97,14 +97,21 @@ const ContactAPointZ: ContactVars = 5; const ContactBPointX: ContactVars = 6; const ContactBPointY: ContactVars = 7; const ContactBPointZ: ContactVars = 8; -const ContactADepth: ContactVars = 9; -const ContactBDepth: ContactVars = 10; -const ContactNormX: ContactVars = 11; -const ContactNormY: ContactVars = 12; -const ContactNormZ: ContactVars = 13; -const ContactForceX: ContactVars = 14; -const ContactForceY: ContactVars = 15; -const ContactForceZ: ContactVars = 16; +const ContactAOffX: ContactVars = 9; +const ContactAOffY: ContactVars = 10; +const ContactAOffZ: ContactVars = 11; +const ContactBOffX: ContactVars = 12; +const ContactBOffY: ContactVars = 13; +const ContactBOffZ: ContactVars = 14; +const ContactAThick: ContactVars = 15; +const ContactBThick: ContactVars = 16; +const ContactNormX: ContactVars = 17; +const ContactNormY: ContactVars = 18; +const ContactNormZ: ContactVars = 19; +const ContactForceX: ContactVars = 20; +const ContactForceY: ContactVars = 21; +const ContactForceZ: ContactVars = 22; +const BroadContactVarsN = ContactAPointX; //////// import: "control.go" alias JointControlVars = i32; //enums:enum @@ -112,7 +119,7 @@ const JointControlForce: JointControlVars = 0; const JointTargetPos: JointControlVars = 1; const JointTargetVel: JointControlVars = 2; fn JointControl(idx: i32,dof: i32, vr: JointControlVars) -> f32 { - return JointControls[Index2D(TensorStrides[80], TensorStrides[81], u32(JointDoFIndex(idx, dof)), u32(vr))]; + return JointControls[Index2D(TensorStrides[100], TensorStrides[101], u32(JointDoFIndex(idx, dof)), u32(vr))]; } //////// import: "dynamics.go" @@ -161,10 +168,10 @@ fn DynamicQuat(idx: i32,cni: i32) -> vec4 { //////// import: "enumgen.go" const BodyVarsN: BodyVars = 41; -const ContactVarsN: ContactVars = 17; +const ContactVarsN: ContactVars = 23; const JointControlVarsN: JointControlVars = 3; const DynamicVarsN: DynamicVars = 32; -const GPUVarsN: GPUVars = 10; +const GPUVarsN: GPUVars = 12; const JointTypesN: JointTypes = 7; const JointVarsN: JointVars = 50; const JointDoFVarsN: JointDoFVars = 7; @@ -306,6 +313,7 @@ struct PhysParams { AngularDamping: f32, ContactWeighting: i32, Restitution: i32, + MaxGeomIter: i32, ContactMargin: f32, ContactsMax: i32, Cur: i32, @@ -318,7 +326,6 @@ struct PhysParams { BodyCollidePairsN: i32, pad: i32, pad1: i32, - pad2: i32, Gravity: vec4, } diff --git a/physics/shaders/StepSolveJoints.wgsl b/physics/shaders/StepSolveJoints.wgsl index 7e1cf1bf..783c5dbb 100644 --- a/physics/shaders/StepSolveJoints.wgsl +++ b/physics/shaders/StepSolveJoints.wgsl @@ -45,9 +45,9 @@ const BodyShape: BodyVars = 0; const BodyDynamic: BodyVars = 1; const BodyWorld: BodyVars = 2; const BodyGroup: BodyVars = 3; -const BodySizeX: BodyVars = 4; -const BodySizeY: BodyVars = 5; -const BodySizeZ: BodyVars = 6; +const BodyHSizeX: BodyVars = 4; +const BodyHSizeY: BodyVars = 5; +const BodyHSizeZ: BodyVars = 6; const BodyThick: BodyVars = 7; const BodyMass: BodyVars = 8; const BodyInvMass: BodyVars = 9; @@ -102,14 +102,21 @@ const ContactAPointZ: ContactVars = 5; const ContactBPointX: ContactVars = 6; const ContactBPointY: ContactVars = 7; const ContactBPointZ: ContactVars = 8; -const ContactADepth: ContactVars = 9; -const ContactBDepth: ContactVars = 10; -const ContactNormX: ContactVars = 11; -const ContactNormY: ContactVars = 12; -const ContactNormZ: ContactVars = 13; -const ContactForceX: ContactVars = 14; -const ContactForceY: ContactVars = 15; -const ContactForceZ: ContactVars = 16; +const ContactAOffX: ContactVars = 9; +const ContactAOffY: ContactVars = 10; +const ContactAOffZ: ContactVars = 11; +const ContactBOffX: ContactVars = 12; +const ContactBOffY: ContactVars = 13; +const ContactBOffZ: ContactVars = 14; +const ContactAThick: ContactVars = 15; +const ContactBThick: ContactVars = 16; +const ContactNormX: ContactVars = 17; +const ContactNormY: ContactVars = 18; +const ContactNormZ: ContactVars = 19; +const ContactForceX: ContactVars = 20; +const ContactForceY: ContactVars = 21; +const ContactForceZ: ContactVars = 22; +const BroadContactVarsN = ContactAPointX; //////// import: "control.go" alias JointControlVars = i32; //enums:enum @@ -117,7 +124,7 @@ const JointControlForce: JointControlVars = 0; const JointTargetPos: JointControlVars = 1; const JointTargetVel: JointControlVars = 2; fn JointControl(idx: i32,dof: i32, vr: JointControlVars) -> f32 { - return JointControls[Index2D(TensorStrides[80], TensorStrides[81], u32(JointDoFIndex(idx, dof)), u32(vr))]; + return JointControls[Index2D(TensorStrides[100], TensorStrides[101], u32(JointDoFIndex(idx, dof)), u32(vr))]; } //////// import: "dynamics.go" @@ -172,10 +179,10 @@ fn DynamicAngDelta(idx: i32,cni: i32) -> vec3 { //////// import: "enumgen.go" const BodyVarsN: BodyVars = 41; -const ContactVarsN: ContactVars = 17; +const ContactVarsN: ContactVars = 23; const JointControlVarsN: JointControlVars = 3; const DynamicVarsN: DynamicVars = 32; -const GPUVarsN: GPUVars = 10; +const GPUVarsN: GPUVars = 12; const JointTypesN: JointTypes = 7; const JointVarsN: JointVars = 50; const JointDoFVarsN: JointDoFVars = 7; @@ -326,6 +333,7 @@ struct PhysParams { AngularDamping: f32, ContactWeighting: i32, Restitution: i32, + MaxGeomIter: i32, ContactMargin: f32, ContactsMax: i32, Cur: i32, @@ -338,7 +346,6 @@ struct PhysParams { BodyCollidePairsN: i32, pad: i32, pad1: i32, - pad2: i32, Gravity: vec4, } diff --git a/physics/shapecollide.go b/physics/shapecollide.go index 34a7e3c0..3b871f05 100644 --- a/physics/shapecollide.go +++ b/physics/shapecollide.go @@ -49,22 +49,55 @@ func NewGeomData(bi, cni int32, shp Shapes) GeomData { var gd GeomData gd.BodyIdx = bi gd.Shape = shp - gd.Size = BodySize(bi) + gd.Size = BodyHSize(bi) gd.Thick = Bodies.Value(int(bi), int(BodyThick)) gd.MinSize = min(gd.Size.X, gd.Size.Y) gd.MinSize = min(gd.MinSize, gd.Size.Z) gd.WbR = BodyDynamicPos(bi, cni) gd.WbQ = BodyDynamicQuat(bi, cni) + InitGeomData(bi, &gd) + return gd +} + +func InitGeomData(bi int32, gd *GeomData) { var bwR math32.Vector3 var bwQ math32.Quat slmath.SpatialTransformInverse(gd.WbR, gd.WbQ, &bwR, &bwQ) gd.BwR = bwR gd.BwQ = bwQ gd.Radius = 0 - if shp == Sphere || shp == Capsule { // todo: cone is separate + if gd.Shape == Sphere || gd.Shape == Capsule || gd.Shape == Cone { gd.Radius = gd.Size.X } - return gd +} + +// ContactPoints is the common final pathway for all Col* shape-specific +// collision functions, first determining if the computed distance is +// within the given margin and returning false if not (not a true contact). +// Otherwise, sets the actual points of contact and their offsets +// based on ptA, ptB, norm and dist values returned from collision functions. +// The actual distance is reduced by the radius values for Sphere, +// Capsule, and Cone types, and is returned in distActual. +// This is broken out here to support independent testing of the collision functions. +func ContactPoints(dist, margin float32, gdA *GeomData, gdB *GeomData, ptA, ptB, norm math32.Vector3, ctA, ctB, offA, offB *math32.Vector3, distActual, offMagA, offMagB *float32) bool { + thick := gdA.Thick + gdB.Thick + // Total separation required by radii and additional thicknesses + totSepReq := gdA.Radius + gdB.Radius + thick + *distActual = dist - totSepReq + if *distActual >= margin { + return false + } + // transform from world into body frame (so the contact point includes the shape transform) + *ctA = slmath.MulSpatialPoint(gdA.BwR, gdA.BwQ, ptA) + *ctB = slmath.MulSpatialPoint(gdB.BwR, gdB.BwQ, ptB) + // fmt.Println(ptA, ptB, *ctA, *ctB, gdA.BwR, gdA.BwQ) + + *offMagA = gdA.Radius + gdA.Thick + *offMagB = gdB.Radius + gdB.Thick + + *offA = slmath.MulQuatVector(gdA.BwQ, norm.MulScalar(-(*offMagA))) + *offB = slmath.MulQuatVector(gdB.BwQ, norm.MulScalar(*offMagB)) + return true } /////// Collision methods: in geometry/kernels.py @@ -74,33 +107,32 @@ func NewGeomData(bi, cni int32, shp Shapes) GeomData { // X_wb, X_ws -> WtoB // X_bw, X_sw -> BtoW -// ptAw = point in A, world coords; b = body coords -// spatial_vector = w,v = top, bottom = delta, angDelta +// pAw = point in A, world coords; b = body coords -func ColSphereSphere(cpi int32, gdA *GeomData, gdB *GeomData, ptA, ptB, norm *math32.Vector3) float32 { - ptAw := gdA.WbR - ptBw := gdB.WbR - diff := ptAw.Sub(ptBw) - *ptA = ptAw - *ptB = ptBw +func ColSphereSphere(cpi, maxIter int32, gdA *GeomData, gdB *GeomData, pA, pB, norm *math32.Vector3) float32 { + pAw := gdA.WbR + pBw := gdB.WbR + diff := pAw.Sub(pBw) + *pA = pAw + *pB = pBw *norm = slmath.Normal3(diff) return slmath.Dot3(diff, *norm) } -func ColCapsulePlane(cpi int32, gdA *GeomData, gdB *GeomData, ptA, ptB, norm *math32.Vector3) float32 { - var ptAw, ptBw, diff math32.Vector3 +func ColCapsulePlane(cpi, maxIter int32, gdA *GeomData, gdB *GeomData, pA, pB, norm *math32.Vector3) float32 { + var pAw, pBw, diff math32.Vector3 hh := gdA.Size.Y if cpi < 2 { // vertex side := float32(cpi)*2 - 1 - ptAw = slmath.MulSpatialPoint(gdA.WbR, gdA.WbQ, math32.Vec3(0, side*hh, 0)) - queryB := slmath.MulSpatialPoint(gdB.BwR, gdB.BwQ, ptAw) - ptBb := ClosestPointPlane(gdB.Size.X, gdB.Size.Z, queryB) - ptBw = slmath.MulSpatialPoint(gdA.WbR, gdA.WbQ, ptBb) - diff = ptAw.Sub(ptBw) + pAw = slmath.MulSpatialPoint(gdA.WbR, gdA.WbQ, math32.Vec3(0, side*hh, 0)) + queryB := slmath.MulSpatialPoint(gdB.BwR, gdB.BwQ, pAw) + pBb := ClosestPointPlane(gdB.Size.X, gdB.Size.Z, queryB) + pBw = slmath.MulSpatialPoint(gdA.WbR, gdA.WbQ, pBb) + diff = pAw.Sub(pBw) if gdB.Size.X > 0 { *norm = slmath.Normal3(diff) } else { - *norm = slmath.MulSpatialPoint(gdB.WbR, gdB.WbQ, math32.Vec3(0, 1, 0)) + *norm = slmath.MulQuatVector(gdB.WbQ, math32.Vec3(0, 1, 0)) } } else { // edges of finite plane -- only here if plane is finite var edge0, edge1 math32.Vector3 @@ -109,18 +141,271 @@ func ColCapsulePlane(cpi int32, gdA *GeomData, gdB *GeomData, ptA, ptB, norm *ma edge1w := slmath.MulSpatialPoint(gdB.WbR, gdB.WbQ, edge1) edge0a := slmath.MulSpatialPoint(gdA.BwR, gdA.BwQ, edge0w) edge1a := slmath.MulSpatialPoint(gdA.BwR, gdA.BwQ, edge1w) - u := ClosestEdgeCapsule(gdA.Size.X, gdA.Size.Y, edge0a, edge1a, 10) // todo: edge_sdf_iter - ptBw = edge0w.MulScalar(1 - u).Add(edge1w.MulScalar(u)) + u := ClosestEdgeCapsule(gdA.Size.X, gdA.Size.Y, edge0a, edge1a, maxIter) + pBw = edge0w.MulScalar(1 - u).Add(edge1w.MulScalar(u)) // find closest point + contact normal on capsule A - pt0Aw := slmath.MulSpatialPoint(gdA.WbR, gdA.WbQ, math32.Vec3(0, hh, 0)) - pt1Aw := slmath.MulSpatialPoint(gdA.WbR, gdA.WbQ, math32.Vec3(0, -hh, 0)) - ptAw = ClosestPointLineSegment(pt0Aw, pt1Aw, ptBw) - diff = ptAw.Sub(ptBw) - *norm = slmath.MulSpatialPoint(gdB.WbR, gdB.WbQ, math32.Vec3(0, 1, 0)) + p0Aw := slmath.MulSpatialPoint(gdA.WbR, gdA.WbQ, math32.Vec3(0, hh, 0)) + p1Aw := slmath.MulSpatialPoint(gdA.WbR, gdA.WbQ, math32.Vec3(0, -hh, 0)) + pAw = ClosestPointLineSegment(p0Aw, p1Aw, pBw) + diff = pAw.Sub(pBw) + *norm = slmath.MulQuatVector(gdB.WbQ, math32.Vec3(0, 1, 0)) } - *ptA = ptAw - *ptB = ptBw + *pA = pAw + *pB = pBw + return slmath.Dot3(diff, *norm) +} + +// Handle collision between two capsules (gdA and gdB). +func ColCapsuleCapsule(cpi, maxIter int32, gdA *GeomData, gdB *GeomData, pA, pB, norm *math32.Vector3) float32 { + // find closest edge coordinate to capsule SDF B + hhA := gdA.Size.Y + hhB := gdB.Size.Y + // edge from capsule A + // depending on point id, we query an edge from 0 to 0.5 or 0.5 to 1 + e0 := math32.Vec3(0.0, 0.0, hhA*float32(cpi%2)) + e1 := math32.Vec3(0.0, 0.0, -hhA*float32((cpi+1)%2)) + edge0w := slmath.MulSpatialPoint(gdA.WbR, gdA.WbQ, e0) + edge1w := slmath.MulSpatialPoint(gdA.WbR, gdA.WbQ, e1) + edge0b := slmath.MulSpatialPoint(gdA.BwR, gdA.BwQ, edge0w) + edge1b := slmath.MulSpatialPoint(gdA.BwR, gdA.BwQ, edge1w) + u := ClosestEdgeCapsule(gdB.Size.X, gdB.Size.Y, edge0b, edge1b, maxIter) + pAw := edge0w.MulScalar(1 - u).Add(edge1w.MulScalar(u)) + p0Bw := slmath.MulSpatialPoint(gdB.WbR, gdB.WbQ, math32.Vec3(0.0, 0.0, hhB)) + p1Bw := slmath.MulSpatialPoint(gdB.WbR, gdB.WbQ, math32.Vec3(0.0, 0.0, -hhB)) + pBw := ClosestPointLineSegment(p0Bw, p1Bw, pAw) + diff := pAw.Sub(pBw) + *norm = slmath.Normal3(diff) + *pA = pAw + *pB = pBw + return slmath.Dot3(diff, *norm) +} + +// Handle collision between two boxes (gdA and gdB). +func ColBoxBox(cpi, maxIter int32, gdA *GeomData, gdB *GeomData, pA, pB, norm *math32.Vector3) float32 { + // edge-based box contact + var edge0, edge1 math32.Vector3 + BoxEdge(cpi, gdA.Size, &edge0, &edge1) + edge0w := slmath.MulSpatialPoint(gdA.WbR, gdA.WbQ, edge0) + edge1w := slmath.MulSpatialPoint(gdA.WbR, gdA.WbQ, edge1) + edge0b := slmath.MulSpatialPoint(gdB.BwR, gdB.BwQ, edge0w) + edge1b := slmath.MulSpatialPoint(gdB.BwR, gdB.BwQ, edge1w) + u := ClosestEdgeBox(gdB.Size, edge0b, edge1b, maxIter) + pAw := edge0w.MulScalar(1 - u).Add(edge1w.MulScalar(u)) + + // find closest point + contact normal on box B + queryB := slmath.MulSpatialPoint(gdB.BwR, gdB.BwQ, pAw) + pBody := ClosestPointBox(gdB.Size, queryB) + pBw := slmath.MulSpatialPoint(gdB.WbR, gdB.WbQ, pBody) + diff := pAw.Sub(pBw) + + *norm = slmath.MulQuatVector(gdB.WbQ, BoxSDFGrad(gdB.Size, queryB)) + *pA = pAw + *pB = pBw + return slmath.Dot3(diff, *norm) +} + +// Handle collision between a box (gdA) and a capsule (gdB). +func ColBoxCapsule(cpi, maxIter int32, gdA *GeomData, gdB *GeomData, pA, pB, norm *math32.Vector3) float32 { + hhB := gdB.Size.Y + // capsule B + // depending on point id, we query an edge from 0 to 0.5 or 0.5 to 1 + e0 := math32.Vec3(0.0, 0.0, -hhB*float32(cpi%2)) + e1 := math32.Vec3(0.0, 0.0, hhB*float32((cpi+1)%2)) + edge0w := slmath.MulSpatialPoint(gdB.WbR, gdB.WbQ, e0) + edge1w := slmath.MulSpatialPoint(gdB.WbR, gdB.WbQ, e1) + edge0a := slmath.MulSpatialPoint(gdA.BwR, gdA.BwQ, edge0w) + edge1a := slmath.MulSpatialPoint(gdA.BwR, gdA.BwQ, edge1w) + u := ClosestEdgeBox(gdA.Size, edge0a, edge1a, maxIter) + pBw := edge0w.MulScalar(1 - u).Add(edge1w.MulScalar(u)) + // find closest point + contact normal on box A + queryA := slmath.MulSpatialPoint(gdA.BwR, gdA.BwQ, pBw) + pABody := ClosestPointBox(gdA.Size, queryA) + pAw := slmath.MulSpatialPoint(gdA.WbR, gdA.WbQ, pABody) + diff := pAw.Sub(pBw) + // the contact point inside the capsule should already be outside the box + *norm = slmath.Negate3(slmath.MulQuatVector(gdA.WbQ, BoxSDFGrad(gdA.Size, queryA))) + *pA = pAw + *pB = pBw return slmath.Dot3(diff, *norm) } +// Handle collision between a box (gdA) and a plane (gdB). +func ColBoxPlane(cpi, maxIter int32, gdA *GeomData, gdB *GeomData, pA, pB, norm *math32.Vector3) float32 { + width := gdB.Size.X + length := gdB.Size.Z + + var pAw, pBw, diff math32.Vector3 + if cpi < 8 { + // vertex-based contact + pABody := BoxVertex(cpi, gdA.Size) + pAw = slmath.MulSpatialPoint(gdA.WbR, gdA.WbQ, pABody) + queryB := slmath.MulSpatialPoint(gdB.BwR, gdB.BwQ, pAw) + pBody := ClosestPointPlane(width, length, queryB) + pBw = slmath.MulSpatialPoint(gdB.WbR, gdB.WbQ, pBody) + diff = pAw.Sub(pBw) + *norm = slmath.MulQuatVector(gdB.WbQ, math32.Vec3(0.0, 0.0, 1.0)) + if width > 0.0 && length > 0.0 { + if math32.Abs(queryB.X) > width || math32.Abs(queryB.Z) > length { + // skip, we will evaluate the plane edge contact with the box later + return 1.0e6 // invalid + } + // check whether the COM is above the plane + // sign = wp.sign(slmath.Dot3(wp.transform_get_translation(gdA.X_ws) - pBw, normal)) + // if sign < 0.0: + // + // // the entire box is most likely below the plane + // return + } + // the contact point is within plane boundaries + } else { + // contact between box A and edges of finite plane B + var edge0, edge1 math32.Vector3 + PlaneEdge(cpi-8, width, length, &edge0, &edge1) + edge0w := slmath.MulSpatialPoint(gdB.WbR, gdB.WbQ, edge0) + edge1w := slmath.MulSpatialPoint(gdB.WbR, gdB.WbQ, edge1) + edge0a := slmath.MulSpatialPoint(gdA.BwR, gdA.BwQ, edge0w) + edge1a := slmath.MulSpatialPoint(gdA.BwR, gdA.BwQ, edge1w) + u := ClosestEdgeBox(gdA.Size, edge0a, edge1a, maxIter) + pBw = edge0w.MulScalar(1 - u).Add(edge1w.MulScalar(u)) + + // find closest point + contact normal on box A + queryA := slmath.MulSpatialPoint(gdA.BwR, gdA.BwQ, pBw) + pABody := ClosestPointBox(gdA.Size, queryA) + pAw = slmath.MulSpatialPoint(gdA.WbR, gdA.WbQ, pABody) + queryB := slmath.MulSpatialPoint(gdA.BwR, gdA.BwQ, pAw) + if math32.Abs(queryB.X) > width || math32.Abs(queryB.Z) > length { + // ensure that the closest point is actually inside the plane + return 1.0e6 // invalid + } + diff = pAw.Sub(pBw) + comA := gdA.WbR + queryB = slmath.MulSpatialPoint(gdB.BwR, gdB.BwQ, comA) + if math32.Abs(queryB.X) > width || math32.Abs(queryB.Z) > length { + // the COM is outside the plane + *norm = slmath.Normal3(comA.Sub(pBw)) + } else { + *norm = slmath.MulQuatVector(gdB.WbQ, math32.Vec3(0.0, 0.0, 1.0)) + } + } + *pA = pAw + *pB = pBw + return slmath.Dot3(diff, *norm) +} + +// Handle collision between a sphere (gdA) and a box (gdB). +func ColSphereBox(cpi, maxIter int32, gdA *GeomData, gdB *GeomData, pA, pB, norm *math32.Vector3) float32 { + pAw := gdA.WbR + // contact point in frame of body B + pABody := slmath.MulSpatialPoint(gdB.BwR, gdB.BwQ, pAw) + pBody := ClosestPointBox(gdB.Size, pABody) + pBw := slmath.MulSpatialPoint(gdB.WbR, gdB.WbQ, pBody) + diff := pAw.Sub(pBw) + *norm = slmath.Normal3(diff) + *pA = pAw + *pB = pBw + return slmath.Dot3(diff, *norm) +} + +// Handle collision between a sphere (gdA) and a capsule (gdB). +func ColSphereCapsule(cpi, maxIter int32, gdA *GeomData, gdB *GeomData, pA, pB, norm *math32.Vector3) float32 { + pAw := gdA.WbR + hhB := gdB.Size.Y + // capsule B + AB := slmath.MulSpatialPoint(gdB.WbR, gdB.WbQ, math32.Vec3(0.0, 0.0, hhB)) + BB := slmath.MulSpatialPoint(gdB.WbR, gdB.WbQ, math32.Vec3(0.0, 0.0, -hhB)) + pBw := ClosestPointLineSegment(AB, BB, pAw) + diff := pAw.Sub(pBw) + *norm = slmath.Normal3(diff) + *pA = pAw + *pB = pBw + return slmath.Dot3(diff, *norm) +} + +// Handle collision between a sphere (gdA) and a plane (gdB). +func ColSpherePlane(cpi, maxIter int32, gdA *GeomData, gdB *GeomData, pA, pB, norm *math32.Vector3) float32 { + pAw := gdA.WbR + pBody := ClosestPointPlane(gdB.Size.X, gdB.Size.Z, slmath.MulSpatialPoint(gdB.BwR, gdB.BwQ, pAw)) + pBw := slmath.MulSpatialPoint(gdB.WbR, gdB.WbQ, pBody) + diff := pAw.Sub(pBw) + *norm = slmath.MulQuatVector(gdB.WbQ, math32.Vec3(0.0, 0.0, 1.0)) + *pA = pAw + *pB = pBw + return slmath.Dot3(diff, *norm) +} + +// Handle collision between a cylinder (geo_a) and an infinite plane (geo_b). +func ColCylinderPlane(cpi, maxIter int32, gdA *GeomData, gdB *GeomData, pA, pB, norm *math32.Vector3) float32 { + // World-space plane + plNorm := slmath.MulQuatVector(gdB.WbQ, math32.Vec3(0.0, 1.0, 0.0)) + plPos := slmath.MulSpatialPoint(gdB.WbR, gdB.WbQ, math32.Vec3(0.0, 0.0, 0.0)) + + // World-space cylinder params + cylCtr := slmath.MulSpatialPoint(gdA.WbR, gdA.WbQ, math32.Vec3(0.0, 0.0, 0.0)) + cylAx := slmath.Normal3(slmath.MulQuatVector(gdA.WbQ, math32.Vec3(0.0, 0.0, 1.0))) + cylRad := gdA.Size.X + cylHh := gdA.Size.Y + + var dist float32 + var pos math32.Vector3 + + n := plNorm + axis := cylAx + + // Project, make sure axis points toward plane + prjaxis := slmath.Dot3(n, axis) + if prjaxis > 0 { + axis = slmath.Negate3(axis) + prjaxis = -prjaxis + } + + // Compute normal distance from plane to cylinder center + dist0 := slmath.Dot3(cylCtr.Sub(plPos), n) + + // Remove component of -normal along cylinder axis + vec := axis.MulScalar(prjaxis).Sub(n) + // len_sqr := slmath.Dot3(vec, vec) + + // If vector is nondegenerate, normalize and scale by radius + // Otherwise use cylinder's x-axis scaled by radius + // todo: + // vec = wp.where( + // len_sqr >= 1e-12, + // vec * safe_div(cylinder_radius, wp.sqrt(len_sqr)), + // math32.Vec3(1.0, 0.0, 0.0).MuScalar(cylRad), // Default x-axis when degenerate + // ) + + // Project scaled vector on normal + prjvec := slmath.Dot3(vec, n) + + // Scale cylinder axis by half-length + axis = axis.MulScalar(cylHh) + prjaxis *= cylHh + + switch cpi { + case 0: // First contact point (end cap closer to plane) + dist = dist0 + prjaxis + prjvec + pos = cylCtr.Add(vec).Add(axis).Sub(n.MulScalar(dist * 0.5)) + case 1: // Second contact point (end cap farther from plane) + dist = dist0 - prjaxis + prjvec + pos = cylCtr.Add(vec).Sub(axis).Sub(n.MulScalar(dist * 0.5)) + case 2, 3: // Try triangle contact points on side closer to plane + prjvec1 := prjvec * -0.5 + dist = dist0 + prjaxis + prjvec1 + // Compute sideways vector scaled by radius*sqrt(3)/2 + vec1 := slmath.Cross3(vec, axis) + vec1 = slmath.Normal3(vec1).MulScalar(cylRad * math32.Sqrt(3.0) * 0.5) + pextra := vec1.Add(axis).Sub(vec.MulScalar(0.5)).Sub(n.MulScalar(dist * 0.5)) + pos = cylCtr.Add(pextra) + if cpi == 3 { // Add contact point B - adjust to closest side + pos = cylCtr.Sub(pextra) + } + default: + } + + // Split midpoint into shape-plane endpoints + *pA = pos.Add(n.MulScalar(dist * 0.5)) + *pB = pos.Sub(n.MulScalar(dist * 0.5)) + return dist +} + //gosl:end diff --git a/physics/shapecollide.goal b/physics/shapecollide.goal index e374a6dc..114e49ee 100644 --- a/physics/shapecollide.goal +++ b/physics/shapecollide.goal @@ -47,22 +47,55 @@ func NewGeomData(bi, cni int32, shp Shapes) GeomData { var gd GeomData gd.BodyIdx = bi gd.Shape = shp - gd.Size = BodySize(bi) + gd.Size = BodyHSize(bi) gd.Thick = Bodies[bi, BodyThick] gd.MinSize = min(gd.Size.X, gd.Size.Y) gd.MinSize = min(gd.MinSize, gd.Size.Z) gd.WbR = BodyDynamicPos(bi, cni) gd.WbQ = BodyDynamicQuat(bi, cni) + InitGeomData(bi, &gd) + return gd +} + +func InitGeomData(bi int32, gd *GeomData) { var bwR math32.Vector3 var bwQ math32.Quat slmath.SpatialTransformInverse(gd.WbR, gd.WbQ, &bwR, &bwQ) gd.BwR = bwR gd.BwQ = bwQ gd.Radius = 0 - if shp == Sphere || shp == Capsule { // todo: cone is separate + if gd.Shape == Sphere || gd.Shape == Capsule || gd.Shape == Cone { gd.Radius = gd.Size.X } - return gd +} + +// ContactPoints is the common final pathway for all Col* shape-specific +// collision functions, first determining if the computed distance is +// within the given margin and returning false if not (not a true contact). +// Otherwise, sets the actual points of contact and their offsets +// based on ptA, ptB, norm and dist values returned from collision functions. +// The actual distance is reduced by the radius values for Sphere, +// Capsule, and Cone types, and is returned in distActual. +// This is broken out here to support independent testing of the collision functions. +func ContactPoints(dist, margin float32, gdA *GeomData, gdB *GeomData, ptA, ptB, norm math32.Vector3, ctA, ctB, offA, offB *math32.Vector3, distActual, offMagA, offMagB *float32) bool { + thick := gdA.Thick + gdB.Thick + // Total separation required by radii and additional thicknesses + totSepReq := gdA.Radius + gdB.Radius + thick + *distActual = dist - totSepReq + if *distActual >= margin { + return false + } + // transform from world into body frame (so the contact point includes the shape transform) + *ctA = slmath.MulSpatialPoint(gdA.BwR, gdA.BwQ, ptA) + *ctB = slmath.MulSpatialPoint(gdB.BwR, gdB.BwQ, ptB) + // fmt.Println(ptA, ptB, *ctA, *ctB, gdA.BwR, gdA.BwQ) + + *offMagA = gdA.Radius + gdA.Thick + *offMagB = gdB.Radius + gdB.Thick + + *offA = slmath.MulQuatVector(gdA.BwQ, norm.MulScalar(-(*offMagA))) + *offB = slmath.MulQuatVector(gdB.BwQ, norm.MulScalar(*offMagB)) + return true } /////// Collision methods: in geometry/kernels.py @@ -72,32 +105,32 @@ func NewGeomData(bi, cni int32, shp Shapes) GeomData { // X_wb, X_ws -> WtoB // X_bw, X_sw -> BtoW -// ptAw = point in A, world coords; b = body coords +// pAw = point in A, world coords; b = body coords -func ColSphereSphere(cpi int32, gdA *GeomData, gdB *GeomData, ptA, ptB, norm *math32.Vector3) float32 { - ptAw := gdA.WbR - ptBw := gdB.WbR - diff := ptAw.Sub(ptBw) - *ptA = ptAw - *ptB = ptBw +func ColSphereSphere(cpi, maxIter int32, gdA *GeomData, gdB *GeomData, pA, pB, norm *math32.Vector3) float32 { + pAw := gdA.WbR + pBw := gdB.WbR + diff := pAw.Sub(pBw) + *pA = pAw + *pB = pBw *norm = slmath.Normal3(diff) return slmath.Dot3(diff, *norm) } -func ColCapsulePlane(cpi int32, gdA *GeomData, gdB *GeomData, ptA, ptB, norm *math32.Vector3) float32 { - var ptAw, ptBw, diff math32.Vector3 +func ColCapsulePlane(cpi, maxIter int32, gdA *GeomData, gdB *GeomData, pA, pB, norm *math32.Vector3) float32 { + var pAw, pBw, diff math32.Vector3 hh := gdA.Size.Y if cpi < 2 { // vertex side := float32(cpi)*2 - 1 - ptAw = slmath.MulSpatialPoint(gdA.WbR, gdA.WbQ, math32.Vec3(0, side*hh, 0)) - queryB := slmath.MulSpatialPoint(gdB.BwR, gdB.BwQ, ptAw) - ptBb := ClosestPointPlane(gdB.Size.X, gdB.Size.Z, queryB) - ptBw = slmath.MulSpatialPoint(gdA.WbR, gdA.WbQ, ptBb) - diff = ptAw.Sub(ptBw) + pAw = slmath.MulSpatialPoint(gdA.WbR, gdA.WbQ, math32.Vec3(0, side*hh, 0)) + queryB := slmath.MulSpatialPoint(gdB.BwR, gdB.BwQ, pAw) + pBb := ClosestPointPlane(gdB.Size.X, gdB.Size.Z, queryB) + pBw = slmath.MulSpatialPoint(gdA.WbR, gdA.WbQ, pBb) + diff = pAw.Sub(pBw) if gdB.Size.X > 0 { *norm = slmath.Normal3(diff) } else { - *norm = slmath.MulSpatialPoint(gdB.WbR, gdB.WbQ, math32.Vec3(0, 1, 0)) + *norm = slmath.MulQuatVector(gdB.WbQ, math32.Vec3(0, 1, 0)) } } else { // edges of finite plane -- only here if plane is finite var edge0, edge1 math32.Vector3 @@ -106,18 +139,270 @@ func ColCapsulePlane(cpi int32, gdA *GeomData, gdB *GeomData, ptA, ptB, norm *ma edge1w := slmath.MulSpatialPoint(gdB.WbR, gdB.WbQ, edge1) edge0a := slmath.MulSpatialPoint(gdA.BwR, gdA.BwQ, edge0w) edge1a := slmath.MulSpatialPoint(gdA.BwR, gdA.BwQ, edge1w) - u := ClosestEdgeCapsule(gdA.Size.X, gdA.Size.Y, edge0a, edge1a, 10) // todo: edge_sdf_iter - ptBw = edge0w.MulScalar(1 - u).Add(edge1w.MulScalar(u)) + u := ClosestEdgeCapsule(gdA.Size.X, gdA.Size.Y, edge0a, edge1a, maxIter) + pBw = edge0w.MulScalar(1 - u).Add(edge1w.MulScalar(u)) // find closest point + contact normal on capsule A - pt0Aw := slmath.MulSpatialPoint(gdA.WbR, gdA.WbQ, math32.Vec3(0, hh, 0)) - pt1Aw := slmath.MulSpatialPoint(gdA.WbR, gdA.WbQ, math32.Vec3(0, -hh, 0)) - ptAw = ClosestPointLineSegment(pt0Aw, pt1Aw, ptBw) - diff = ptAw.Sub(ptBw) - *norm = slmath.MulSpatialPoint(gdB.WbR, gdB.WbQ, math32.Vec3(0, 1, 0)) + p0Aw := slmath.MulSpatialPoint(gdA.WbR, gdA.WbQ, math32.Vec3(0, hh, 0)) + p1Aw := slmath.MulSpatialPoint(gdA.WbR, gdA.WbQ, math32.Vec3(0, -hh, 0)) + pAw = ClosestPointLineSegment(p0Aw, p1Aw, pBw) + diff = pAw.Sub(pBw) + *norm = slmath.MulQuatVector(gdB.WbQ, math32.Vec3(0, 1, 0)) } - *ptA = ptAw - *ptB = ptBw + *pA = pAw + *pB = pBw + return slmath.Dot3(diff, *norm) +} + +// Handle collision between two capsules (gdA and gdB). +func ColCapsuleCapsule(cpi, maxIter int32, gdA *GeomData, gdB *GeomData, pA, pB, norm *math32.Vector3) float32 { + // find closest edge coordinate to capsule SDF B + hhA := gdA.Size.Y + hhB := gdB.Size.Y + // edge from capsule A + // depending on point id, we query an edge from 0 to 0.5 or 0.5 to 1 + e0 := math32.Vec3(0.0, 0.0, hhA*float32(cpi%2)) + e1 := math32.Vec3(0.0, 0.0, -hhA*float32((cpi+1)%2)) + edge0w := slmath.MulSpatialPoint(gdA.WbR, gdA.WbQ, e0) + edge1w := slmath.MulSpatialPoint(gdA.WbR, gdA.WbQ, e1) + edge0b := slmath.MulSpatialPoint(gdA.BwR, gdA.BwQ, edge0w) + edge1b := slmath.MulSpatialPoint(gdA.BwR, gdA.BwQ, edge1w) + u := ClosestEdgeCapsule(gdB.Size.X, gdB.Size.Y, edge0b, edge1b, maxIter) + pAw := edge0w.MulScalar(1 - u).Add(edge1w.MulScalar(u)) + p0Bw := slmath.MulSpatialPoint(gdB.WbR, gdB.WbQ, math32.Vec3(0.0, 0.0, hhB)) + p1Bw := slmath.MulSpatialPoint(gdB.WbR, gdB.WbQ, math32.Vec3(0.0, 0.0, -hhB)) + pBw := ClosestPointLineSegment(p0Bw, p1Bw, pAw) + diff := pAw.Sub(pBw) + *norm = slmath.Normal3(diff) + *pA = pAw + *pB = pBw + return slmath.Dot3(diff, *norm) +} + +// Handle collision between two boxes (gdA and gdB). +func ColBoxBox(cpi, maxIter int32, gdA *GeomData, gdB *GeomData, pA, pB, norm *math32.Vector3) float32 { + // edge-based box contact + var edge0, edge1 math32.Vector3 + BoxEdge(cpi, gdA.Size, &edge0, &edge1) + edge0w := slmath.MulSpatialPoint(gdA.WbR, gdA.WbQ, edge0) + edge1w := slmath.MulSpatialPoint(gdA.WbR, gdA.WbQ, edge1) + edge0b := slmath.MulSpatialPoint(gdB.BwR, gdB.BwQ, edge0w) + edge1b := slmath.MulSpatialPoint(gdB.BwR, gdB.BwQ, edge1w) + u := ClosestEdgeBox(gdB.Size, edge0b, edge1b, maxIter) + pAw := edge0w.MulScalar(1 - u).Add(edge1w.MulScalar(u)) + + // find closest point + contact normal on box B + queryB := slmath.MulSpatialPoint(gdB.BwR, gdB.BwQ, pAw) + pBody := ClosestPointBox(gdB.Size, queryB) + pBw := slmath.MulSpatialPoint(gdB.WbR, gdB.WbQ, pBody) + diff := pAw.Sub(pBw) + + *norm = slmath.MulQuatVector(gdB.WbQ, BoxSDFGrad(gdB.Size, queryB)) + *pA = pAw + *pB = pBw + return slmath.Dot3(diff, *norm) +} + +// Handle collision between a box (gdA) and a capsule (gdB). +func ColBoxCapsule(cpi, maxIter int32, gdA *GeomData, gdB *GeomData, pA, pB, norm *math32.Vector3) float32 { + hhB := gdB.Size.Y + // capsule B + // depending on point id, we query an edge from 0 to 0.5 or 0.5 to 1 + e0 := math32.Vec3(0.0, 0.0, -hhB*float32(cpi%2)) + e1 := math32.Vec3(0.0, 0.0, hhB*float32((cpi+1)%2)) + edge0w := slmath.MulSpatialPoint(gdB.WbR, gdB.WbQ, e0) + edge1w := slmath.MulSpatialPoint(gdB.WbR, gdB.WbQ, e1) + edge0a := slmath.MulSpatialPoint(gdA.BwR, gdA.BwQ, edge0w) + edge1a := slmath.MulSpatialPoint(gdA.BwR, gdA.BwQ, edge1w) + u := ClosestEdgeBox(gdA.Size, edge0a, edge1a, maxIter) + pBw := edge0w.MulScalar(1 - u).Add(edge1w.MulScalar(u)) + // find closest point + contact normal on box A + queryA := slmath.MulSpatialPoint(gdA.BwR, gdA.BwQ, pBw) + pABody := ClosestPointBox(gdA.Size, queryA) + pAw := slmath.MulSpatialPoint(gdA.WbR, gdA.WbQ, pABody) + diff := pAw.Sub(pBw) + // the contact point inside the capsule should already be outside the box + *norm = slmath.Negate3(slmath.MulQuatVector(gdA.WbQ, BoxSDFGrad(gdA.Size, queryA))) + *pA = pAw + *pB = pBw return slmath.Dot3(diff, *norm) } +// Handle collision between a box (gdA) and a plane (gdB). +func ColBoxPlane(cpi, maxIter int32, gdA *GeomData, gdB *GeomData, pA, pB, norm *math32.Vector3) float32 { + width := gdB.Size.X + length := gdB.Size.Z + + var pAw, pBw, diff math32.Vector3 + if cpi < 8 { + // vertex-based contact + pABody := BoxVertex(cpi, gdA.Size) + pAw = slmath.MulSpatialPoint(gdA.WbR, gdA.WbQ, pABody) + queryB := slmath.MulSpatialPoint(gdB.BwR, gdB.BwQ, pAw) + pBody := ClosestPointPlane(width, length, queryB) + pBw = slmath.MulSpatialPoint(gdB.WbR, gdB.WbQ, pBody) + diff = pAw.Sub(pBw) + *norm = slmath.MulQuatVector(gdB.WbQ, math32.Vec3(0.0, 0.0, 1.0)) + if width > 0.0 && length > 0.0 { + if math32.Abs(queryB.X) > width || math32.Abs(queryB.Z) > length { + // skip, we will evaluate the plane edge contact with the box later + return 1.0e6 // invalid + } + // check whether the COM is above the plane + // sign = wp.sign(slmath.Dot3(wp.transform_get_translation(gdA.X_ws) - pBw, normal)) + // if sign < 0.0: + // // the entire box is most likely below the plane + // return + } + // the contact point is within plane boundaries + } else { + // contact between box A and edges of finite plane B + var edge0, edge1 math32.Vector3 + PlaneEdge(cpi-8, width, length, &edge0, &edge1) + edge0w := slmath.MulSpatialPoint(gdB.WbR, gdB.WbQ, edge0) + edge1w := slmath.MulSpatialPoint(gdB.WbR, gdB.WbQ, edge1) + edge0a := slmath.MulSpatialPoint(gdA.BwR, gdA.BwQ, edge0w) + edge1a := slmath.MulSpatialPoint(gdA.BwR, gdA.BwQ, edge1w) + u := ClosestEdgeBox(gdA.Size, edge0a, edge1a, maxIter) + pBw = edge0w.MulScalar(1 - u).Add(edge1w.MulScalar(u)) + + // find closest point + contact normal on box A + queryA := slmath.MulSpatialPoint(gdA.BwR, gdA.BwQ, pBw) + pABody := ClosestPointBox(gdA.Size, queryA) + pAw = slmath.MulSpatialPoint(gdA.WbR, gdA.WbQ, pABody) + queryB := slmath.MulSpatialPoint(gdA.BwR, gdA.BwQ, pAw) + if math32.Abs(queryB.X) > width || math32.Abs(queryB.Z) > length { + // ensure that the closest point is actually inside the plane + return 1.0e6 // invalid + } + diff = pAw.Sub(pBw) + comA := gdA.WbR + queryB = slmath.MulSpatialPoint(gdB.BwR, gdB.BwQ, comA) + if math32.Abs(queryB.X) > width || math32.Abs(queryB.Z) > length { + // the COM is outside the plane + *norm = slmath.Normal3(comA.Sub(pBw)) + } else { + *norm = slmath.MulQuatVector(gdB.WbQ, math32.Vec3(0.0, 0.0, 1.0)) + } + } + *pA = pAw + *pB = pBw + return slmath.Dot3(diff, *norm) +} + +// Handle collision between a sphere (gdA) and a box (gdB). +func ColSphereBox(cpi, maxIter int32, gdA *GeomData, gdB *GeomData, pA, pB, norm *math32.Vector3) float32 { + pAw := gdA.WbR + // contact point in frame of body B + pABody := slmath.MulSpatialPoint(gdB.BwR, gdB.BwQ, pAw) + pBody := ClosestPointBox(gdB.Size, pABody) + pBw := slmath.MulSpatialPoint(gdB.WbR, gdB.WbQ, pBody) + diff := pAw.Sub(pBw) + *norm = slmath.Normal3(diff) + *pA = pAw + *pB = pBw + return slmath.Dot3(diff, *norm) +} + +// Handle collision between a sphere (gdA) and a capsule (gdB). +func ColSphereCapsule(cpi, maxIter int32, gdA *GeomData, gdB *GeomData, pA, pB, norm *math32.Vector3) float32 { + pAw := gdA.WbR + hhB := gdB.Size.Y + // capsule B + AB := slmath.MulSpatialPoint(gdB.WbR, gdB.WbQ, math32.Vec3(0.0, 0.0, hhB)) + BB := slmath.MulSpatialPoint(gdB.WbR, gdB.WbQ, math32.Vec3(0.0, 0.0, -hhB)) + pBw := ClosestPointLineSegment(AB, BB, pAw) + diff := pAw.Sub(pBw) + *norm = slmath.Normal3(diff) + *pA = pAw + *pB = pBw + return slmath.Dot3(diff, *norm) +} + +// Handle collision between a sphere (gdA) and a plane (gdB). +func ColSpherePlane(cpi, maxIter int32, gdA *GeomData, gdB *GeomData, pA, pB, norm *math32.Vector3) float32 { + pAw := gdA.WbR + pBody := ClosestPointPlane(gdB.Size.X, gdB.Size.Z, slmath.MulSpatialPoint(gdB.BwR, gdB.BwQ, pAw)) + pBw := slmath.MulSpatialPoint(gdB.WbR, gdB.WbQ, pBody) + diff := pAw.Sub(pBw) + *norm = slmath.MulQuatVector(gdB.WbQ, math32.Vec3(0.0, 0.0, 1.0)) + *pA = pAw + *pB = pBw + return slmath.Dot3(diff, *norm) +} + +// Handle collision between a cylinder (geo_a) and an infinite plane (geo_b). +func ColCylinderPlane(cpi, maxIter int32, gdA *GeomData, gdB *GeomData, pA, pB, norm *math32.Vector3) float32 { + // World-space plane + plNorm := slmath.MulQuatVector(gdB.WbQ, math32.Vec3(0.0, 1.0, 0.0)) + plPos := slmath.MulSpatialPoint(gdB.WbR, gdB.WbQ, math32.Vec3(0.0, 0.0, 0.0)) + + // World-space cylinder params + cylCtr := slmath.MulSpatialPoint(gdA.WbR, gdA.WbQ, math32.Vec3(0.0, 0.0, 0.0)) + cylAx := slmath.Normal3(slmath.MulQuatVector(gdA.WbQ, math32.Vec3(0.0, 0.0, 1.0))) + cylRad := gdA.Size.X + cylHh := gdA.Size.Y + + var dist float32 + var pos math32.Vector3 + + n := plNorm + axis := cylAx + + // Project, make sure axis points toward plane + prjaxis := slmath.Dot3(n, axis) + if prjaxis > 0 { + axis = slmath.Negate3(axis) + prjaxis = -prjaxis + } + + // Compute normal distance from plane to cylinder center + dist0 := slmath.Dot3(cylCtr.Sub(plPos), n) + + // Remove component of -normal along cylinder axis + vec := axis.MulScalar(prjaxis).Sub(n) + // len_sqr := slmath.Dot3(vec, vec) + + // If vector is nondegenerate, normalize and scale by radius + // Otherwise use cylinder's x-axis scaled by radius + // todo: + // vec = wp.where( + // len_sqr >= 1e-12, + // vec * safe_div(cylinder_radius, wp.sqrt(len_sqr)), + // math32.Vec3(1.0, 0.0, 0.0).MuScalar(cylRad), // Default x-axis when degenerate + // ) + + // Project scaled vector on normal + prjvec := slmath.Dot3(vec, n) + + // Scale cylinder axis by half-length + axis = axis.MulScalar(cylHh) + prjaxis *= cylHh + + switch cpi { + case 0: // First contact point (end cap closer to plane) + dist = dist0 + prjaxis + prjvec + pos = cylCtr.Add(vec).Add(axis).Sub(n.MulScalar(dist * 0.5)) + case 1: // Second contact point (end cap farther from plane) + dist = dist0 - prjaxis + prjvec + pos = cylCtr.Add(vec).Sub(axis).Sub(n.MulScalar(dist * 0.5)) + case 2, 3: // Try triangle contact points on side closer to plane + prjvec1 := prjvec * -0.5 + dist = dist0 + prjaxis + prjvec1 + // Compute sideways vector scaled by radius*sqrt(3)/2 + vec1 := slmath.Cross3(vec, axis) + vec1 = slmath.Normal3(vec1).MulScalar(cylRad * math32.Sqrt(3.0) * 0.5) + pextra := vec1.Add(axis).Sub(vec.MulScalar(0.5)).Sub(n.MulScalar(dist * 0.5)) + pos = cylCtr.Add(pextra) + if cpi == 3 { // Add contact point B - adjust to closest side + pos = cylCtr.Sub(pextra) + } + default: + } + + // Split midpoint into shape-plane endpoints + *pA = pos.Add(n.MulScalar(dist * 0.5)) + *pB = pos.Sub(n.MulScalar(dist * 0.5)) + return dist +} + //gosl:end diff --git a/physics/shapegeom.go b/physics/shapegeom.go index f07e542f..c24f4580 100644 --- a/physics/shapegeom.go +++ b/physics/shapegeom.go @@ -24,6 +24,38 @@ func BoxSDF(upper, p math32.Vector3) float32 { return slmath.Length3(e) + min(max(qx, max(qy, qz)), 0.0) } +func BoxSDFGrad(upper, p math32.Vector3) math32.Vector3 { + qx := math32.Abs(p.X) - upper.X + qy := math32.Abs(p.Y) - upper.Y + qz := math32.Abs(p.Z) - upper.Z + + // exterior case + if qx > 0.0 || qy > 0.0 || qz > 0.0 { + x := math32.Clamp(p.X, -upper.X, upper.X) + y := math32.Clamp(p.Y, -upper.Y, upper.Y) + z := math32.Clamp(p.Z, -upper.Z, upper.Z) + + return slmath.Normal3(p.Sub(math32.Vec3(x, y, z))) + } + + sx := math32.Sign(p.X) + sy := math32.Sign(p.Y) + sz := math32.Sign(p.Z) + + // x projection + if (qx > qy && qx > qz) || (qy == 0.0 && qz == 0.0) { + return math32.Vec3(sx, 0.0, 0.0) + } + + // y projection + if (qy > qx && qy > qz) || (qx == 0.0 && qz == 0.0) { + return math32.Vec3(0.0, sy, 0.0) + } + + // z projection + return math32.Vec3(0.0, 0.0, sz) +} + func CapsuleSDF(radius, hh float32, p math32.Vector3) float32 { if p.Y > hh { return slmath.Length3(math32.Vec3(p.X, p.Y-hh, p.Z)) - radius @@ -118,23 +150,24 @@ func BoxVertex(ptId int32, upper math32.Vector3) math32.Vector3 { // get the edge of the box given its ID (0-11) func BoxEdge(edgeId int32, upper math32.Vector3, edge0, edge1 *math32.Vector3) { - if edgeId < 4 { + eid := edgeId + if eid < 4 { // edges along x: 0-1, 2-3, 4-5, 6-7 - i := edgeId * 2 + i := eid * 2 j := i + 1 *edge0 = BoxVertex(i, upper) *edge1 = BoxVertex(j, upper) - } else if edgeId < 8 { + } else if eid < 8 { // edges along y: 0-2, 1-3, 4-6, 5-7 - edgeId -= 4 - i := edgeId%2 + edgeId // 2 * 4 + eid -= 4 + i := eid%2 + eid // 2 * 4 j := i + 2 *edge0 = BoxVertex(i, upper) *edge1 = BoxVertex(j, upper) } // edges along z: 0-4, 1-5, 2-6, 3-7 - edgeId -= 8 - i := edgeId + eid -= 8 + i := eid j := i + 4 *edge0 = BoxVertex(i, upper) *edge1 = BoxVertex(j, upper) @@ -152,8 +185,8 @@ func PlaneEdge(edgeId int32, width, length float32, edge0, edge1 *math32.Vector3 p1x = -p0x p1z = p0z } - (*edge0).Set(p0x, 0, p0z) - (*edge1).Set(p1x, 0, p1z) + *edge0 = math32.Vec3(p0x, 0, p0z) + *edge1 = math32.Vec3(p1x, 0, p1z) } // find point on edge closest to box, return its barycentric edge coordinate diff --git a/physics/shapegeom_test.go b/physics/shapegeom_test.go new file mode 100644 index 00000000..39047d48 --- /dev/null +++ b/physics/shapegeom_test.go @@ -0,0 +1,176 @@ +// Copyright (c) 2025, Cogent Core. All rights reserved. +// Use of this source code is governed by a BSD-style +// license that can be found in the LICENSE file. + +package physics + +import ( + "fmt" + "testing" + + "cogentcore.org/core/math32" + "github.com/stretchr/testify/assert" +) + +func TestSphereSphere(t *testing.T) { + // Test sphere-sphere collision with analytical penetration depth validation. + // Analytical calculation: + // - Distance = ||center2 - center1|| - (radius1 + radius2) + // - Negative distance indicates penetration + + tests := []struct { + posA math32.Vector3 + radiusA float32 + posB math32.Vector3 + radiusB, dist float32 + }{ + {math32.Vector3{0.0, 0.0, 0.0}, 1.0, math32.Vector3{3.5, 0.0, 0.0}, 1.0, 1.5}, // Separated by 1.5 + {math32.Vector3{0.0, 0.0, 0.0}, 1.0, math32.Vector3{3.0, 0.0, 0.0}, 1.0, 1.0}, // Separated by 1.0 + {math32.Vector3{0.0, 0.0, 0.0}, 1.0, math32.Vector3{2.5, 0.0, 0.0}, 1.0, 0.5}, // Separated by 0.5 + {math32.Vector3{0.0, 0.0, 0.0}, 1.0, math32.Vector3{2.0, 0.0, 0.0}, 1.0, 0.0}, // Exactly touching + {math32.Vector3{0.0, 1.0, 0.0}, 1.0, math32.Vector3{1.8, 1.0, 0.0}, 1.0, -0.2}, // Penetration = 0.2 + {math32.Vector3{0.0, 0.0, 0.0}, 1.0, math32.Vector3{1.5, 0.0, 0.0}, 1.0, -0.5}, // Penetration = 0.5 + {math32.Vector3{0.0, 0.0, 1.0}, 1.0, math32.Vector3{1.2, 0.0, 1.0}, 1.0, -0.8}, // Penetration = 0.8 + // Different radii + {math32.Vector3{0.0, 0.0, 0.0}, 0.5, math32.Vector3{2.0, 0.0, 0.0}, 1.0, 0.5}, // Separated + {math32.Vector3{0.0, 1.0, 0.0}, 0.5, math32.Vector3{1.5, 1.0, 0.0}, 1.0, 0.0}, // Touching + {math32.Vector3{0.0, 0.0, 0.0}, 0.5, math32.Vector3{1.2, 0.0, 0.0}, 1.0, -0.3}, // Penetration = 0.3 + } + + tol := 1e-5 + + rot := math32.NewQuat(0, 0, 0, 1) + for _, tc := range tests { + gdA := GeomData{Shape: Sphere, Radius: tc.radiusA, Size: math32.Vector3{tc.radiusA, 0, 0}, WbR: tc.posA, WbQ: rot} + gdB := GeomData{Shape: Sphere, Radius: tc.radiusB, Size: math32.Vector3{tc.radiusB, 0, 0}, WbR: tc.posB, WbQ: rot} + InitGeomData(0, &gdA) + InitGeomData(0, &gdB) + + var ptA, ptB, norm math32.Vector3 + dist := ColSphereSphere(0, &gdA, &gdB, &ptA, &ptB, &norm) + margin := float32(0.01) + + var ctA, ctB, offA, offB math32.Vector3 + var distActual, offMagA, offMagB float32 + actual := ContactPoints(dist, margin, &gdA, &gdB, ptA, ptB, norm, &ctA, &ctB, &offA, &offB, &distActual, &offMagA, &offMagB) + _ = actual + + // fmt.Println(distActual, tc.dist, actual) + // if actual { + // fmt.Println(ptA, ptB, ctA, ctB, offA, offB, offMagA, offMagB) + // } + + assert.InDelta(t, tc.dist, distActual, tol) + assert.InDelta(t, 1.0, norm.Length(), tol) + + if !actual { + continue + } + cpA := ctA.Add(offA) + cpB := ctB.Add(offB) + fmt.Println(cpA, cpB) + } + + // + // // Check that normal points from geom 0 (sphere 1) into geom 1 (sphere 2) + // for i in range(len(test_cases)): + // pos1 = np.array(test_cases[i][0]) + // pos2 = np.array(test_cases[i][2]) + // normal = normals_np[i] + // self.assertTrue( + // check_normal_direction_sphere_sphere(pos1, pos2, normal), + // msg=f"Test case {i}: Normal does not point from sphere 1 toward sphere 2", + // ) + // + // // Check that contact position is at midpoint between surfaces + // for i in range(len(test_cases)): + // pos1 = np.array(test_cases[i][0]) + // radius1 = test_cases[i][1] + // pos2 = np.array(test_cases[i][2]) + // radius2 = test_cases[i][3] + // contact_pos = positions_np[i] + // normal = normals_np[i] + // penetration_depth = distances_np[i] + // + // self.assertTrue( + // check_contact_position_midpoint(contact_pos, normal, penetration_depth, pos1, radius1, pos2, radius2), + // msg=f"Test case {i}: Contact position is not at midpoint between surfaces", + // ) + // +} + +func TestPlaneSphere(t *testing.T) { + // Analytical calculation: + // - Distance = (sphere_center - plane_point) · plane_normal - sphere_radius + // - Negative distance indicates penetration + tests := []struct { + normal, posA, posB math32.Vector3 + radius, dist float32 + }{ + {math32.Vector3{0.0, 0.0, 1.0}, math32.Vector3{0.0, 0.0, 0.0}, math32.Vector3{0.0, 0.0, 2.0}, 1.0, 1.0}, // Above plane, separation = 1.0 + {math32.Vector3{0.0, 0.0, 1.0}, math32.Vector3{0.0, 0.0, 0.0}, math32.Vector3{0.0, 0.0, 1.5}, 1.0, 0.5}, // Above plane, separation = 0.5 + {math32.Vector3{0.0, 0.0, 1.0}, math32.Vector3{0.0, 0.0, 0.0}, math32.Vector3{0.0, 0.0, 1.0}, 1.0, 0.0}, // Just touching + {math32.Vector3{0.0, 0.0, 1.0}, math32.Vector3{0.0, 0.0, 0.0}, math32.Vector3{0.0, 0.0, 0.8}, 1.0, -0.2}, // Penetration = 0.2 + {math32.Vector3{0.0, 0.0, 1.0}, math32.Vector3{0.0, 0.0, 0.0}, math32.Vector3{0.0, 0.0, 0.5}, 1.0, -0.5}, // Penetration = 0.5 + {math32.Vector3{0.0, 0.0, 1.0}, math32.Vector3{0.0, 0.0, 0.0}, math32.Vector3{0.0, 0.0, 0.2}, 1.0, -0.8}, // Penetration = 0.8 + {math32.Vector3{1.0, 0.0, 0.0}, math32.Vector3{1.0, 0.0, 0.0}, math32.Vector3{2.0, 0.0, 0.0}, 0.5, 0.5}, // X-axis, separation = 0.5 + {math32.Vector3{1.0, 0.0, 0.0}, math32.Vector3{1.0, 0.0, 0.0}, math32.Vector3{1.5, 0.0, 0.0}, 0.5, 0.0}, // X-axis, touching + {math32.Vector3{1.0, 0.0, 0.0}, math32.Vector3{1.0, 0.0, 0.0}, math32.Vector3{1.3, 0.0, 0.0}, 0.5, -0.2}, // X-axis, penetration = 0.2 + } + _ = tests + + // for _, tc := range tests { + // ColSphereSphere(0, ) + // } + // + // wp.launch( + // test_plane_sphere_kernel, + // dim=len(test_cases), + // inputs=[plane_normals, plane_positions, sphere_positions, sphere_radii, distances, contact_positions], + // ) + // wp.synchronize() + // + // distances_np = distances.numpy() + // positions_np = contact_positions.numpy() + // + // // Verify expected distances with analytical validation + // for i, expected_dist in enumerate([tc[4] for tc in test_cases]): + // self.assertAlmostEqual( + // distances_np[i], + // expected_dist, + // places=5, + // msg=f"Test case {i}: Expected distance {expected_dist:.4f}, got {distances_np[i]:.4f}", + // ) + // + // // Check that contact position lies between sphere and plane + // for i in range(len(test_cases)): + // if distances_np[i] >= 0: + // // Skip separated cases + // continue + // + // plane_normal = np.array(test_cases[i][0]) + // plane_pos = np.array(test_cases[i][1]) + // sphere_pos = np.array(test_cases[i][2]) + // sphere_radius = test_cases[i][3] + // contact_pos = positions_np[i] + // + // // Contact position should be between sphere surface and plane + // // Distance from contact to sphere center should be less than sphere radius + // dist_to_sphere_center = np.linalg.norm(contact_pos - sphere_pos) + // self.assertLess( + // dist_to_sphere_center, + // sphere_radius + 0.01, + // msg=f"Test case {i}: Contact position too far from sphere (dist: {dist_to_sphere_center:.4f})", + // ) + // + // // Contact position should be on the plane side of the sphere center + // // (or at most slightly past the plane) + // dist_contact_to_plane = np.dot(contact_pos - plane_pos, plane_normal) + // dist_sphere_to_plane = np.dot(sphere_pos - plane_pos, plane_normal) + // self.assertLessEqual( + // dist_contact_to_plane, + // dist_sphere_to_plane + 0.01, + // msg=f"Test case {i}: Contact position on wrong side of sphere center", + // ) + // +} diff --git a/physics/typegen.go b/physics/typegen.go index 72b3a769..094f6490 100644 --- a/physics/typegen.go +++ b/physics/typegen.go @@ -24,7 +24,7 @@ var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.JointVars", var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.JointDoFVars", IDName: "joint-do-f-vars", Doc: "JointDoFVars are joint DoF state variables stored in tensor.Float32,\none for each DoF."}) -var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.PhysParams", IDName: "phys-params", Doc: "PhysParams are the physics parameters", Directives: []types.Directive{{Tool: "gosl", Directive: "start"}}, Fields: []types.Field{{Name: "Iterations", Doc: "Iterations is the number of iterations to perform."}, {Name: "Dt", Doc: "Dt is the integration stepsize."}, {Name: "SoftRelax", Doc: "SoftRelax is soft-body relaxation constant."}, {Name: "JointLinearRelax", Doc: "JointLinearRelax is joint linear relaxation constant."}, {Name: "JointAngularRelax", Doc: "JointAngularRelax is joint angular relaxation constant."}, {Name: "JointLinearComply", Doc: "JointLinearComply is joint linear compliance constant."}, {Name: "JointAngularComply", Doc: "JointAngularComply is joint angular compliance constant."}, {Name: "ContactRelax", Doc: "ContactRelax is rigid contact relaxation constant."}, {Name: "AngularDamping", Doc: "AngularDamping is damping of angular motion."}, {Name: "ContactWeighting", Doc: "Contact weighting"}, {Name: "Restitution", Doc: "Restitution"}, {Name: "ContactMargin", Doc: "Contact margin is the extra distance for broadphase collision\naround rigid bodies."}, {Name: "ContactsMax", Doc: "Maximum number of contacts to process at any given point."}, {Name: "Cur", Doc: "Index for the current state (0 or 1, alternates with Next)."}, {Name: "Next", Doc: "Index for the next state (1 or 0, alternates with Cur)."}, {Name: "BodiesN", Doc: "BodiesN is number of rigid bodies."}, {Name: "DynamicsN", Doc: "DynamicsN is number of dynamics bodies."}, {Name: "JointsN", Doc: "JointsN is number of joints."}, {Name: "JointDoFsN", Doc: "JointDoFsN is number of joint DoFs."}, {Name: "BodyJointsMax", Doc: "BodyJointsMax is max number of joints per body + 1 for actual n."}, {Name: "BodyCollidePairsN", Doc: "BodyCollidePairsN is the total number of pre-compiled collision pairs\nto examine."}, {Name: "pad"}, {Name: "pad1"}, {Name: "pad2"}, {Name: "Gravity", Doc: "Gravity is the gravity acceleration function"}}}) +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.PhysParams", IDName: "phys-params", Doc: "PhysParams are the physics parameters", Directives: []types.Directive{{Tool: "gosl", Directive: "start"}}, Fields: []types.Field{{Name: "Iterations", Doc: "Iterations is the number of iterations to perform."}, {Name: "Dt", Doc: "Dt is the integration stepsize."}, {Name: "SoftRelax", Doc: "SoftRelax is soft-body relaxation constant."}, {Name: "JointLinearRelax", Doc: "JointLinearRelax is joint linear relaxation constant."}, {Name: "JointAngularRelax", Doc: "JointAngularRelax is joint angular relaxation constant."}, {Name: "JointLinearComply", Doc: "JointLinearComply is joint linear compliance constant."}, {Name: "JointAngularComply", Doc: "JointAngularComply is joint angular compliance constant."}, {Name: "ContactRelax", Doc: "ContactRelax is rigid contact relaxation constant."}, {Name: "AngularDamping", Doc: "AngularDamping is damping of angular motion."}, {Name: "ContactWeighting", Doc: "Contact weighting"}, {Name: "Restitution", Doc: "Restitution"}, {Name: "MaxGeomIter", Doc: "MaxGeomIter is number of iterations to perform in shape-based\ngeometry collision computations"}, {Name: "ContactMargin", Doc: "Contact margin is the extra distance for broadphase collision\naround rigid bodies."}, {Name: "ContactsMax", Doc: "Maximum number of contacts to process at any given point."}, {Name: "Cur", Doc: "Index for the current state (0 or 1, alternates with Next)."}, {Name: "Next", Doc: "Index for the next state (1 or 0, alternates with Cur)."}, {Name: "BodiesN", Doc: "BodiesN is number of rigid bodies."}, {Name: "DynamicsN", Doc: "DynamicsN is number of dynamics bodies."}, {Name: "JointsN", Doc: "JointsN is number of joints."}, {Name: "JointDoFsN", Doc: "JointDoFsN is number of joint DoFs."}, {Name: "BodyJointsMax", Doc: "BodyJointsMax is max number of joints per body + 1 for actual n."}, {Name: "BodyCollidePairsN", Doc: "BodyCollidePairsN is the total number of pre-compiled collision pairs\nto examine."}, {Name: "pad"}, {Name: "pad1"}, {Name: "Gravity", Doc: "Gravity is the gravity acceleration function"}}}) var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.GeomData", IDName: "geom-data", Doc: "GeomData contains all geometric data for narrow-phase collision.", Directives: []types.Directive{{Tool: "gosl", Directive: "start"}}, Fields: []types.Field{{Name: "BodyIdx"}, {Name: "Shape"}, {Name: "MinSize", Doc: "MinSize is the min of the Size dimensions."}, {Name: "Thick", Doc: "Thickness of shape."}, {Name: "Radius", Doc: "Radius is the effective radius for sphere-like elements (Sphere, Capsule, Cone)"}, {Name: "Size"}, {Name: "WbR", Doc: "World-to-Body transform\nPosition (R) (i.e., BodyPos)"}, {Name: "WbQ", Doc: "Quaternion (Q) (i.e., BodyQuat)"}, {Name: "BwR", Doc: "Body-to-World transform (inverse)\nPosition (R)"}, {Name: "BwQ", Doc: "Quaternion (Q)"}}}) @@ -32,4 +32,4 @@ var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.Shapes", IDN var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.State", IDName: "state", Doc: "State contains the basic physical state including position, orientation, velocity.\nThese are only the values that can be either relative or absolute -- other physical\nstate values such as Mass should go in Rigid.", Methods: []types.Method{{Name: "MoveOnAxis", Doc: "MoveOnAxis moves (translates) the specified distance on the specified local axis,\nrelative to the current rotation orientation.\nThe axis is normalized prior to aplying the distance factor.\nSets the LinVel to motion vector.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z", "dist"}}, {Name: "MoveOnAxisAbs", Doc: "MoveOnAxisAbs moves (translates) the specified distance on the specified local axis,\nin absolute X,Y,Z coordinates (does not apply the Quat rotation factor.\nThe axis is normalized prior to aplying the distance factor.\nSets the LinVel to motion vector.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z", "dist"}}, {Name: "SetEulerRotation", Doc: "SetEulerRotation sets the rotation in Euler angles (degrees).", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z"}}, {Name: "EulerRotation", Doc: "EulerRotation returns the current rotation in Euler angles (degrees).", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Returns: []string{"Vector3"}}, {Name: "SetAxisRotation", Doc: "SetAxisRotation sets rotation from local axis and angle in degrees.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z", "angle"}}, {Name: "RotateOnAxis", Doc: "RotateOnAxis rotates around the specified local axis the specified angle in degrees.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z", "angle"}}, {Name: "RotateEuler", Doc: "RotateEuler rotates by given Euler angles (in degrees) relative to existing rotation.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z"}}}, Fields: []types.Field{{Name: "Pos", Doc: "Pos is the position of center of mass of object."}, {Name: "Quat", Doc: "Quat is the rotation specified as a quaternion."}, {Name: "LinVel", Doc: "LinVel is the linear velocity."}, {Name: "AngVel", Doc: "AngVel is the angular velocity."}}}) -var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.World", IDName: "world", Doc: "World contains and manages all of the physics elements.", Directives: []types.Directive{{Tool: "go", Directive: "generate", Args: []string{"core", "generate", "-add-types", "-gosl"}}}, Fields: []types.Field{{Name: "GPU", Doc: "GPU determines whether to use GPU (else CPU)."}, {Name: "Params", Doc: "Params are global parameters."}, {Name: "Bodies", Doc: "Bodies are the rigid body elements (dynamic and static),\nspecifying the constant, non-dynamic properties,\nwhich is initial state for dynamics.\n[body][BodyVarsN]"}, {Name: "Joints", Doc: "Joints is a list of permanent joints connecting bodies,\nwhich do not change (no dynamic variables).\n[joint][JointVarsN]"}, {Name: "JointDoFs", Doc: "JointDoFs is a list of joint DoF parameters, allocated per joint.\n[dof][JointDoFVars]"}, {Name: "BodyJoints", Doc: "BodyJoints is a list of joint indexes for each dynamic body, for aggregating.\n[dyn body][parent, child][Params.BodyJointsMax]"}, {Name: "BodyCollidePairs", Doc: "BodyCollidePairs are pairs of Body indexes that could potentially collide\nbased on precomputed collision logic, using World, Group, and Joint indexes.\n[BodyCollidePairsN][2]"}, {Name: "Dynamics", Doc: "Dynamics are the dynamic rigid body elements: these actually move.\nThe first set of variables are for initial values, and the second current.\n[body][cur/next][DynamicVarsN]"}, {Name: "ContactCount", Doc: "ContactCount has number of points of contact between bodies.\nparams.Cur is written to and params.Next is zeroed in\nnarrow-phase contacts processing, so it is ready for next time.\n[2]"}, {Name: "Contacts", Doc: "Contacts are points of contact between bodies. Max possible size\nis BodyCollidePairsN, but actual size on given run is updated\ninto ContactCount.\n[BodyCollidePairsN][ContactVarsN]"}, {Name: "JointControls", Doc: "JointControls are dynamic joint control inputs, per joint DoF\n(in correspondence with [JointDoFs]). This can be uploaded to the\nGPU at every step.\n[dof][JointControlVarsN]"}}}) +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.World", IDName: "world", Doc: "World contains and manages all of the physics elements.", Directives: []types.Directive{{Tool: "go", Directive: "generate", Args: []string{"core", "generate", "-add-types", "-gosl"}}}, Fields: []types.Field{{Name: "GPU", Doc: "GPU determines whether to use GPU (else CPU)."}, {Name: "Params", Doc: "Params are global parameters."}, {Name: "Bodies", Doc: "Bodies are the rigid body elements (dynamic and static),\nspecifying the constant, non-dynamic properties,\nwhich is initial state for dynamics.\n[body][BodyVarsN]"}, {Name: "Joints", Doc: "Joints is a list of permanent joints connecting bodies,\nwhich do not change (no dynamic variables).\n[joint][JointVarsN]"}, {Name: "JointDoFs", Doc: "JointDoFs is a list of joint DoF parameters, allocated per joint.\n[dof][JointDoFVars]"}, {Name: "BodyJoints", Doc: "BodyJoints is a list of joint indexes for each dynamic body, for aggregating.\n[dyn body][parent, child][Params.BodyJointsMax]"}, {Name: "BodyCollidePairs", Doc: "BodyCollidePairs are pairs of Body indexes that could potentially collide\nbased on precomputed collision logic, using World, Group, and Joint indexes.\n[BodyCollidePairsN][2]"}, {Name: "Dynamics", Doc: "Dynamics are the dynamic rigid body elements: these actually move.\nThe first set of variables are for initial values, and the second current.\n[body][cur/next][DynamicVarsN]"}, {Name: "BroadContactsN", Doc: "BroadContactsN has number of points of broad contact\nbetween bodies. [1]"}, {Name: "BroadContacts", Doc: "BroadContacts are the results of broad-phase contact processing,\nestablishing possible points of contact between bodies.\n[ContactsMax][BroadContactVarsN]"}, {Name: "ContactsN", Doc: "ContactsN has number of points of narrow (final) contact\nbetween bodies. [1]"}, {Name: "Contacts", Doc: "Contacts are the results of narrow-phase contact processing,\nwhere only actual contacts with fully-specified values are present.\n[ContactsMax][ContactVarsN]"}, {Name: "JointControls", Doc: "JointControls are dynamic joint control inputs, per joint DoF\n(in correspondence with [JointDoFs]). This can be uploaded to the\nGPU at every step.\n[dof][JointControlVarsN]"}}}) diff --git a/physics/vars.go b/physics/vars.go index fb2f5ccd..19041502 100644 --- a/physics/vars.go +++ b/physics/vars.go @@ -59,14 +59,24 @@ var ( //gosl:dims 3 Dynamics *tensor.Float32 - // ContactCount has number of points of contact between bodies. - // params.Cur is written to and params.Next is zeroed in - // narrow-phase contacts processing, so it is ready for next time. - // [2] + // BroadContactsN has number of points of broad contact + // between bodies. [1] //gosl:dims 1 - ContactCount *tensor.Int32 + BroadContactsN *tensor.Int32 - // Contacts are points of contact between bodies. + // BroadContacts are the results of broad-phase contact processing, + // establishing possible points of contact between bodies. + // [ContactsMax][BroadContactVarsN] + //gosl:dims 2 + BroadContacts *tensor.Float32 + + // ContactsN has number of points of narrow (final) contact + // between bodies. [1] + //gosl:dims 1 + ContactsN *tensor.Int32 + + // Contacts are the results of narrow-phase contact processing, + // where only actual contacts with fully-specified values are present. // [ContactsMax][ContactVarsN] //gosl:dims 2 Contacts *tensor.Float32 diff --git a/physics/world.go b/physics/world.go index 13c027ff..ce21dc1d 100644 --- a/physics/world.go +++ b/physics/world.go @@ -50,16 +50,22 @@ type World struct { // [body][cur/next][DynamicVarsN] Dynamics *tensor.Float32 `display:"no-inline"` - // ContactCount has number of points of contact between bodies. - // params.Cur is written to and params.Next is zeroed in - // narrow-phase contacts processing, so it is ready for next time. - // [2] - ContactCount *tensor.Int32 `display:"no-inline"` - - // Contacts are points of contact between bodies. Max possible size - // is BodyCollidePairsN, but actual size on given run is updated - // into ContactCount. - // [BodyCollidePairsN][ContactVarsN] + // BroadContactsN has number of points of broad contact + // between bodies. [1] + BroadContactsN *tensor.Int32 + + // BroadContacts are the results of broad-phase contact processing, + // establishing possible points of contact between bodies. + // [ContactsMax][BroadContactVarsN] + BroadContacts *tensor.Float32 `display:"no-inline"` + + // ContactsN has number of points of narrow (final) contact + // between bodies. [1] + ContactsN *tensor.Int32 `display:"no-inline"` + + // Contacts are the results of narrow-phase contact processing, + // where only actual contacts with fully-specified values are present. + // [ContactsMax][ContactVarsN] Contacts *tensor.Float32 `display:"no-inline"` // JointControls are dynamic joint control inputs, per joint DoF @@ -84,7 +90,9 @@ func (wl *World) Init() { wl.JointDoFs = tensor.NewFloat32(0, int(JointDoFVarsN)) wl.BodyJoints = tensor.NewInt32(0, 2, 2) wl.Dynamics = tensor.NewFloat32(0, 2, int(DynamicVarsN)) - wl.ContactCount = tensor.NewInt32(2) + wl.BroadContactsN = tensor.NewInt32(1) + wl.BroadContacts = tensor.NewFloat32(0, int(ContactVarsN)) + wl.ContactsN = tensor.NewInt32(1) wl.Contacts = tensor.NewFloat32(0, int(ContactVarsN)) wl.JointControls = tensor.NewFloat32(0, int(JointControlVarsN)) wl.SetAsCurrentVars() @@ -99,7 +107,7 @@ func (wl *World) NewBody(shape Shapes, size, pos math32.Vector3, rot math32.Quat wl.Params[0].BodiesN = idx + 1 SetBodyShape(idx, shape) SetBodyDynamic(idx, -1) - SetBodySize(idx, size) + SetBodyHSize(idx, size) SetBodyPos(idx, pos) SetBodyQuat(idx, rot) SetBodyGroup(idx, -1) // assume static @@ -145,7 +153,9 @@ func (wl *World) SetAsCurrentVars() { BodyJoints = wl.BodyJoints JointDoFs = wl.JointDoFs Dynamics = wl.Dynamics - ContactCount = wl.ContactCount + BroadContactsN = wl.BroadContactsN + BroadContacts = wl.BroadContacts + ContactsN = wl.ContactsN Contacts = wl.Contacts JointControls = wl.JointControls } @@ -162,5 +172,5 @@ func (wl *World) GPUInit() { // the GPU. This is done in GPUInit, and if current switched. func (wl *World) ToGPUInfra() { ToGPUTensorStrides() - ToGPU(ParamsVar, BodiesVar, JointsVar, BodyJointsVar, JointDoFsVar, DynamicsVar, ContactCountVar, ContactsVar) + ToGPU(ParamsVar, BodiesVar, JointsVar, BodyJointsVar, JointDoFsVar, DynamicsVar, BroadContactsNVar, BroadContactsVar, ContactsNVar, ContactsVar) } diff --git a/physics/world.goal b/physics/world.goal index ed306070..88af9733 100644 --- a/physics/world.goal +++ b/physics/world.goal @@ -48,16 +48,22 @@ type World struct { // [body][cur/next][DynamicVarsN] Dynamics *tensor.Float32 `display:"no-inline"` - // ContactCount has number of points of contact between bodies. - // params.Cur is written to and params.Next is zeroed in - // narrow-phase contacts processing, so it is ready for next time. - // [2] - ContactCount *tensor.Int32 `display:"no-inline"` - - // Contacts are points of contact between bodies. Max possible size - // is BodyCollidePairsN, but actual size on given run is updated - // into ContactCount. - // [BodyCollidePairsN][ContactVarsN] + // BroadContactsN has number of points of broad contact + // between bodies. [1] + BroadContactsN *tensor.Int32 + + // BroadContacts are the results of broad-phase contact processing, + // establishing possible points of contact between bodies. + // [ContactsMax][BroadContactVarsN] + BroadContacts *tensor.Float32 `display:"no-inline"` + + // ContactsN has number of points of narrow (final) contact + // between bodies. [1] + ContactsN *tensor.Int32 `display:"no-inline"` + + // Contacts are the results of narrow-phase contact processing, + // where only actual contacts with fully-specified values are present. + // [ContactsMax][ContactVarsN] Contacts *tensor.Float32 `display:"no-inline"` // JointControls are dynamic joint control inputs, per joint DoF @@ -82,7 +88,9 @@ func (wl *World) Init() { wl.JointDoFs = tensor.NewFloat32(0, int(JointDoFVarsN)) wl.BodyJoints = tensor.NewInt32(0, 2, 2) wl.Dynamics = tensor.NewFloat32(0, 2, int(DynamicVarsN)) - wl.ContactCount = tensor.NewInt32(2) + wl.BroadContactsN = tensor.NewInt32(1) + wl.BroadContacts = tensor.NewFloat32(0, int(ContactVarsN)) + wl.ContactsN = tensor.NewInt32(1) wl.Contacts = tensor.NewFloat32(0, int(ContactVarsN)) wl.JointControls = tensor.NewFloat32(0, int(JointControlVarsN)) wl.SetAsCurrentVars() @@ -97,7 +105,7 @@ func (wl *World) NewBody(shape Shapes, size, pos math32.Vector3, rot math32.Quat wl.Params[0].BodiesN = idx + 1 SetBodyShape(idx, shape) SetBodyDynamic(idx, -1) - SetBodySize(idx, size) + SetBodyHSize(idx, size) SetBodyPos(idx, pos) SetBodyQuat(idx, rot) SetBodyGroup(idx, -1) // assume static @@ -143,7 +151,9 @@ func (wl *World) SetAsCurrentVars() { BodyJoints = wl.BodyJoints JointDoFs = wl.JointDoFs Dynamics = wl.Dynamics - ContactCount = wl.ContactCount + BroadContactsN = wl.BroadContactsN + BroadContacts = wl.BroadContacts + ContactsN = wl.ContactsN Contacts = wl.Contacts JointControls = wl.JointControls } @@ -160,5 +170,5 @@ func (wl *World) GPUInit() { // the GPU. This is done in GPUInit, and if current switched. func (wl *World) ToGPUInfra() { ToGPUTensorStrides() - ToGPU(ParamsVar, BodiesVar, JointsVar, BodyJointsVar, JointDoFsVar, DynamicsVar, ContactCountVar, ContactsVar) + ToGPU(ParamsVar, BodiesVar, JointsVar, BodyJointsVar, JointDoFsVar, DynamicsVar, BroadContactsNVar, BroadContactsVar, ContactsNVar, ContactsVar) } From a10fd1dd68ef935438e466876ef7cbf297f86bf1 Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Sun, 21 Dec 2025 22:37:43 +0100 Subject: [PATCH 35/97] physics: full collision stack building --- physics/body.go | 9 +- physics/body.goal | 9 +- physics/contact.go | 264 ++++++++++++- physics/contact.goal | 264 ++++++++++++- physics/enumgen.go | 20 +- physics/gosl.go | 97 +++++ physics/params.go | 2 +- physics/shaders/CollisionBroad.wgsl | 79 ++-- physics/shaders/CollisionInit.wgsl | 307 +++++++++++++++ physics/shaders/CollisionNarrow.wgsl | 79 ++-- physics/shaders/DeltasFromContacts.wgsl | 374 +++++++++++++++++++ physics/shaders/DeltasFromJoints.wgsl | 79 ++-- physics/shaders/DynamicsCurToNext.wgsl | 79 ++-- physics/shaders/ForcesFromJoints.wgsl | 79 ++-- physics/shaders/InitDynamics.wgsl | 79 ++-- physics/shaders/StepBodyContacts.wgsl | 457 +++++++++++++++++++++++ physics/shaders/StepBodyDeltas.wgsl | 79 ++-- physics/shaders/StepIntegrateBodies.wgsl | 79 ++-- physics/shaders/StepJointForces.wgsl | 79 ++-- physics/shaders/StepSolveJoints.wgsl | 99 ++--- physics/shapegeom_test.go | 2 +- physics/step.go | 19 + physics/step.goal | 19 + physics/step_joint.go | 20 +- physics/step_joint.goal | 20 +- physics/typegen.go | 2 +- physics/vars.go | 4 +- physics/world.go | 4 +- physics/world.goal | 4 +- physics/world/view.go | 4 +- 30 files changed, 2296 insertions(+), 415 deletions(-) create mode 100644 physics/shaders/CollisionInit.wgsl create mode 100644 physics/shaders/DeltasFromContacts.wgsl create mode 100644 physics/shaders/StepBodyContacts.wgsl diff --git a/physics/body.go b/physics/body.go index b4e7487d..adbee44a 100644 --- a/physics/body.go +++ b/physics/body.go @@ -67,10 +67,15 @@ const ( // i.e., final velocity / initial velocity. BodyBounce - // BodyFriction coefficient: how much friction is generated by transverse motion. - // Additive across the two surfaces. + // BodyFriction is the standard coefficient for linear friction (mu). BodyFriction + // BodyFrictionTortion is resistance to spinning at the contact point. + BodyFrictionTortion + + // BodyFrictionRolling is resistance to rolling motion at contact. + BodyFrictionRolling + // 3D position of body (structural center). BodyPosX BodyPosY diff --git a/physics/body.goal b/physics/body.goal index 527b5c1d..39b92fa5 100644 --- a/physics/body.goal +++ b/physics/body.goal @@ -65,9 +65,14 @@ const ( // i.e., final velocity / initial velocity. BodyBounce - // BodyFriction coefficient: how much friction is generated by transverse motion. - // Additive across the two surfaces. + // BodyFriction is the standard coefficient for linear friction (mu). BodyFriction + + // BodyFrictionTortion is resistance to spinning at the contact point. + BodyFrictionTortion + + // BodyFrictionRolling is resistance to rolling motion at contact. + BodyFrictionRolling // 3D position of body (structural center). BodyPosX diff --git a/physics/contact.go b/physics/contact.go index 80907345..86382fbb 100644 --- a/physics/contact.go +++ b/physics/contact.go @@ -62,10 +62,23 @@ const ( ContactNormY ContactNormZ - // computed contact force vector - ContactForceX - ContactForceY - ContactForceZ + // computed contact deltas, A + ContactADeltaX + ContactADeltaY + ContactADeltaZ + + ContactAAngDeltaX + ContactAAngDeltaY + ContactAAngDeltaZ + + // computed contact deltas, B + ContactBDeltaX + ContactBDeltaY + ContactBDeltaZ + + ContactBAngDeltaX + ContactBAngDeltaY + ContactBAngDeltaZ ) // number of broad-phase contact values: just the indexes @@ -171,14 +184,44 @@ func SetContactNorm(idx int32, pos math32.Vector3) { Contacts.Set(pos.Z, int(idx), int(ContactNormZ)) } -func ContactForce(idx int32) math32.Vector3 { - return math32.Vec3(Contacts.Value(int(idx), int(ContactForceX)), Contacts.Value(int(idx), int(ContactForceY)), Contacts.Value(int(idx), int(ContactForceZ))) +func ContactADelta(idx int32) math32.Vector3 { + return math32.Vec3(Contacts.Value(int(idx), int(ContactADeltaX)), Contacts.Value(int(idx), int(ContactADeltaY)), Contacts.Value(int(idx), int(ContactADeltaZ))) } -func SetContactForce(idx int32, pos math32.Vector3) { - Contacts.Set(pos.X, int(idx), int(ContactForceX)) - Contacts.Set(pos.Y, int(idx), int(ContactForceY)) - Contacts.Set(pos.Z, int(idx), int(ContactForceZ)) +func SetContactADelta(idx int32, pos math32.Vector3) { + Contacts.Set(pos.X, int(idx), int(ContactADeltaX)) + Contacts.Set(pos.Y, int(idx), int(ContactADeltaY)) + Contacts.Set(pos.Z, int(idx), int(ContactADeltaZ)) +} + +func ContactAAngDelta(idx int32) math32.Vector3 { + return math32.Vec3(Contacts.Value(int(idx), int(ContactAAngDeltaX)), Contacts.Value(int(idx), int(ContactAAngDeltaY)), Contacts.Value(int(idx), int(ContactAAngDeltaZ))) +} + +func SetContactAAngDelta(idx int32, pos math32.Vector3) { + Contacts.Set(pos.X, int(idx), int(ContactAAngDeltaX)) + Contacts.Set(pos.Y, int(idx), int(ContactAAngDeltaY)) + Contacts.Set(pos.Z, int(idx), int(ContactAAngDeltaZ)) +} + +func ContactBDelta(idx int32) math32.Vector3 { + return math32.Vec3(Contacts.Value(int(idx), int(ContactBDeltaX)), Contacts.Value(int(idx), int(ContactBDeltaY)), Contacts.Value(int(idx), int(ContactBDeltaZ))) +} + +func SetContactBDelta(idx int32, pos math32.Vector3) { + Contacts.Set(pos.X, int(idx), int(ContactBDeltaX)) + Contacts.Set(pos.Y, int(idx), int(ContactBDeltaY)) + Contacts.Set(pos.Z, int(idx), int(ContactBDeltaZ)) +} + +func ContactBAngDelta(idx int32) math32.Vector3 { + return math32.Vec3(Contacts.Value(int(idx), int(ContactBAngDeltaX)), Contacts.Value(int(idx), int(ContactBAngDeltaY)), Contacts.Value(int(idx), int(ContactBAngDeltaZ))) +} + +func SetContactBAngDelta(idx int32, pos math32.Vector3) { + Contacts.Set(pos.X, int(idx), int(ContactBAngDeltaX)) + Contacts.Set(pos.Y, int(idx), int(ContactBAngDeltaY)) + Contacts.Set(pos.Z, int(idx), int(ContactBAngDeltaZ)) } func WorldsCollide(wa, wb int32) bool { @@ -399,11 +442,211 @@ func CollisionNarrow(i uint32) { //gosl:kernel Contacts.Set(offMagB, int(nci), int(ContactBThick)) } +// newton: solvers/xpbd/kernels.py: solve_body_contact_positions + +// StepBodyContacts generates contact forces for bodies. +func StepBodyContacts(i uint32) { //gosl:kernel + params := GetParams(0) + ci := int32(i) + cmax := ContactsN.Values[0] + if ci >= cmax { + return + } + biA := GetContactA(ci) + biB := GetContactB(ci) + diA := GetBodyDynamic(biA) + diB := GetBodyDynamic(biB) + + r0A := BodyDynamicPos(biA, params.Next) + q0A := BodyDynamicQuat(biA, params.Next) + + r0B := BodyDynamicPos(biB, params.Next) + q0B := BodyDynamicQuat(biB, params.Next) + + ctA := ContactAPoint(ci) + offA := ContactAOff(ci) + ctB := ContactBPoint(ci) + offB := ContactBOff(ci) + + ctAw := slmath.MulSpatialPoint(r0A, q0A, ctA) + ctBw := slmath.MulSpatialPoint(r0B, q0B, ctB) + thickA := Contacts.Value(int(ci), int(ContactAThick)) + thickB := Contacts.Value(int(ci), int(ContactBThick)) + thick := thickA + thickB + norm := ContactNorm(ci) + nnorm := slmath.Negate3(norm) + + d := slmath.Dot3(norm, ctBw.Sub(ctAw)) - thick + if d >= 0.0 { // now separated + return + } + comA := BodyCom(biA) + mInvA := Bodies.Value(int(biA), int(BodyInvMass)) + iInvA := BodyInvInertia(biA) + + comB := BodyCom(biB) + mInvB := Bodies.Value(int(biB), int(BodyInvMass)) + iInvB := BodyInvInertia(biB) + + var w0A, w0B math32.Vector3 + if diA >= 0 { + w0A = DynamicAngDelta(diA, params.Next) + } + if diB >= 0 { + w0B = DynamicAngDelta(diB, params.Next) + } + + // use average contact material properties + mu := 0.5 * (Bodies.Value(int(biA), int(BodyFriction)) + Bodies.Value(int(biB), int(BodyFriction))) + frTors := 0.5 * (Bodies.Value(int(biA), int(BodyFrictionTortion)) + Bodies.Value(int(biB), int(BodyFrictionTortion))) + frRoll := 0.5 * (Bodies.Value(int(biA), int(BodyFrictionRolling)) + Bodies.Value(int(biB), int(BodyFrictionRolling))) + + // moment arms + dA := ctAw.Sub(slmath.MulSpatialPoint(r0A, q0A, comA)) + dB := ctBw.Sub(slmath.MulSpatialPoint(r0B, q0B, comB)) + + angA := slmath.Negate3(slmath.Cross3(dA, norm)) + angB := slmath.Cross3(dB, norm) + + // todo: contact_inv_weight -- requires atomic + + lambdaN := ContactConstraint(d, q0A, q0B, mInvA, mInvB, iInvA, iInvB, nnorm, norm, angA, angB, params.ContactRelax, params.Dt) + + linDeltaA := slmath.Negate3(norm).MulScalar(lambdaN) + linDeltaB := norm.MulScalar(lambdaN) + angDeltaA := angA.MulScalar(lambdaN) + angDeltaB := angB.MulScalar(lambdaN) + + // linear friction + if mu > 0.0 { + // add on displacement from surface offsets, this ensures + // we include any rotational effects due to thickness from feature + // need to use the current rotation to account for friction due to + // angular effects (e.g.: slipping contact) + ctAw = ctAw.Add(slmath.MulQuatVector(q0A, offA)) + ctBw = ctBw.Add(slmath.MulQuatVector(q0B, offB)) + + // update delta + delta := ctBw.Sub(ctAw) + frDelta := delta.Sub(norm.MulScalar(slmath.Dot3(norm, delta))) + + perp := slmath.Normal3(frDelta) + + angA = slmath.Negate3(slmath.Cross3(dA, perp)) + angB = slmath.Cross3(dB, perp) + + err := slmath.Length3(frDelta) + + if err > 0.0 { + lambdaFr := ContactConstraint(err, q0A, q0B, mInvA, mInvB, iInvA, iInvB, slmath.Negate3(perp), perp, angA, angB, params.ContactRelax, params.Dt) + + // limit friction based on incremental normal force, + // good approximation to limiting on total force + lambdaFr = max(lambdaFr, -lambdaN*mu) + + linDeltaA = linDeltaA.Sub(perp.MulScalar(lambdaFr)) + linDeltaB = linDeltaB.Add(perp.MulScalar(lambdaFr)) + angDeltaA = angDeltaA.Add(angA.MulScalar(lambdaFr)) + angDeltaB = angDeltaB.Add(angB.MulScalar(lambdaFr)) + } + } + + deltaW := w0B.Sub(w0A) + + if frTors > 0.0 { + err := slmath.Dot3(deltaW, norm) * params.Dt + + if math32.Abs(err) > 0.0 { + lin := math32.Vec3(0, 0, 0) + lambdaTors := ContactConstraint(err, q0A, q0B, mInvA, mInvB, iInvA, iInvB, lin, lin, nnorm, norm, params.ContactRelax, params.Dt) + + lambdaTors = math32.Clamp(lambdaTors, -lambdaN*frTors, lambdaN*frTors) + angDeltaA = angDeltaA.Sub(norm.MulScalar(lambdaTors)) + angDeltaB = angDeltaB.Add(norm.MulScalar(lambdaTors)) + } + } + + if frRoll > 0.0 { + deltaW = deltaW.Sub(norm.MulScalar(slmath.Dot3(norm, deltaW))) + err := slmath.Length3(deltaW) * params.Dt + if err > 0.0 { + lin := math32.Vec3(0, 0, 0) + rollN := slmath.Normal3(deltaW) + lambdaRoll := ContactConstraint(err, q0A, q0B, mInvA, mInvB, iInvA, iInvB, lin, lin, slmath.Negate3(rollN), rollN, params.ContactRelax, params.Dt) + lambdaRoll = max(lambdaRoll, -lambdaN*frRoll) + + angDeltaA = angDeltaA.Sub(rollN.MulScalar(lambdaRoll)) + angDeltaB = angDeltaB.Add(rollN.MulScalar(lambdaRoll)) + } + } + + SetContactADelta(ci, linDeltaA) + SetContactBDelta(ci, linDeltaB) + SetContactAAngDelta(ci, angDeltaA) + SetContactBAngDelta(ci, angDeltaB) +} + +// DeltasFromContacts gathers deltas, angDeltas from contacts per dynamic. +func DeltasFromContacts(i uint32) { //gosl:kernel + params := GetParams(0) + di := int32(i) + if di >= params.DynamicsN { + return + } + cmax := ContactsN.Values[0] + + bi := DynamicBody(di) + + td := DynamicDelta(di, params.Next) + ta := DynamicAngDelta(di, params.Next) + for ci := range cmax { + biA := GetContactA(ci) + biB := GetContactB(ci) + if biA == bi { + d := ContactADelta(ci) + td = td.Add(d) + a := ContactAAngDelta(ci) + ta = ta.Add(a) + } + if biB == bi { + d := ContactBDelta(ci) + td = td.Add(d) + a := ContactBAngDelta(ci) + ta = ta.Add(a) + } + } + SetDynamicDelta(di, params.Next, td) + SetDynamicAngDelta(di, params.Next, ta) +} + +func ContactConstraint(err float32, q0A, q0B math32.Quat, mInvA, mInvB float32, iInvA, iInvB math32.Matrix3, linA, linB, angA, angB math32.Vector3, relaxation, dt float32) float32 { + denom := float32(0.0) + denom += slmath.LengthSquared3(linA) * mInvA + denom += slmath.LengthSquared3(linB) * mInvB + + // Eq. 2-3 (make sure to project into the frame of the body) + rotAngA := slmath.MulQuatVectorInverse(q0A, angA) + rotAngB := slmath.MulQuatVectorInverse(q0B, angB) + + denom += slmath.Dot3(rotAngA, iInvA.MulVector3(rotAngA)) + denom += slmath.Dot3(rotAngB, iInvB.MulVector3(rotAngB)) + + lambda := -err + if denom > 0.0 { + lambda /= dt * denom + } + + return lambda * relaxation +} + //gosl:end // IsChildDynamic returns true if dic is a direct child // on any joint where dip is the parent. func (wl *World) IsChildDynamic(dip, dic int32) bool { + if dip < 0 || dic < 0 { + return false + } npja := wl.BodyJoints.Value(int(dip), int(0), int(0)) for j := range npja { ji := wl.BodyJoints.Value(int(dip), int(0), int(1+j)) @@ -466,6 +709,7 @@ func (wl *World) ConfigBodyCollidePairs() { params.BodyCollidePairsN = int32(np) pt.SetShapeSizes(np, 2) wl.BodyCollidePairs = pt + BodyCollidePairs = pt fmt.Println("body pairs over alloc", nalc, np) } diff --git a/physics/contact.goal b/physics/contact.goal index 7d990694..feaa5ad0 100644 --- a/physics/contact.goal +++ b/physics/contact.goal @@ -60,10 +60,23 @@ const ( ContactNormY ContactNormZ - // computed contact force vector - ContactForceX - ContactForceY - ContactForceZ + // computed contact deltas, A + ContactADeltaX + ContactADeltaY + ContactADeltaZ + + ContactAAngDeltaX + ContactAAngDeltaY + ContactAAngDeltaZ + + // computed contact deltas, B + ContactBDeltaX + ContactBDeltaY + ContactBDeltaZ + + ContactBAngDeltaX + ContactBAngDeltaY + ContactBAngDeltaZ ) // number of broad-phase contact values: just the indexes @@ -169,14 +182,44 @@ func SetContactNorm(idx int32, pos math32.Vector3) { Contacts[idx, ContactNormZ] = pos.Z } -func ContactForce(idx int32) math32.Vector3 { - return math32.Vec3(Contacts[idx, ContactForceX], Contacts[idx, ContactForceY], Contacts[idx, ContactForceZ]) +func ContactADelta(idx int32) math32.Vector3 { + return math32.Vec3(Contacts[idx, ContactADeltaX], Contacts[idx, ContactADeltaY], Contacts[idx, ContactADeltaZ]) +} + +func SetContactADelta(idx int32, pos math32.Vector3) { + Contacts[idx, ContactADeltaX] = pos.X + Contacts[idx, ContactADeltaY] = pos.Y + Contacts[idx, ContactADeltaZ] = pos.Z +} + +func ContactAAngDelta(idx int32) math32.Vector3 { + return math32.Vec3(Contacts[idx, ContactAAngDeltaX], Contacts[idx, ContactAAngDeltaY], Contacts[idx, ContactAAngDeltaZ]) +} + +func SetContactAAngDelta(idx int32, pos math32.Vector3) { + Contacts[idx, ContactAAngDeltaX] = pos.X + Contacts[idx, ContactAAngDeltaY] = pos.Y + Contacts[idx, ContactAAngDeltaZ] = pos.Z +} + +func ContactBDelta(idx int32) math32.Vector3 { + return math32.Vec3(Contacts[idx, ContactBDeltaX], Contacts[idx, ContactBDeltaY], Contacts[idx, ContactBDeltaZ]) +} + +func SetContactBDelta(idx int32, pos math32.Vector3) { + Contacts[idx, ContactBDeltaX] = pos.X + Contacts[idx, ContactBDeltaY] = pos.Y + Contacts[idx, ContactBDeltaZ] = pos.Z +} + +func ContactBAngDelta(idx int32) math32.Vector3 { + return math32.Vec3(Contacts[idx, ContactBAngDeltaX], Contacts[idx, ContactBAngDeltaY], Contacts[idx, ContactBAngDeltaZ]) } -func SetContactForce(idx int32, pos math32.Vector3) { - Contacts[idx, ContactForceX] = pos.X - Contacts[idx, ContactForceY] = pos.Y - Contacts[idx, ContactForceZ] = pos.Z +func SetContactBAngDelta(idx int32, pos math32.Vector3) { + Contacts[idx, ContactBAngDeltaX] = pos.X + Contacts[idx, ContactBAngDeltaY] = pos.Y + Contacts[idx, ContactBAngDeltaZ] = pos.Z } func WorldsCollide(wa, wb int32) bool { @@ -295,7 +338,6 @@ func AddBroadContacts(biA, biB, nci, ncA, ncB int32) { } } - // newton: geometry/kernels.py: generate_handle_contact_pairs / handle_contact_pairs // CollisionNarrow performs narrow-phase collision on Contacts. @@ -398,12 +440,211 @@ func CollisionNarrow(i uint32) { //gosl:kernel Contacts[nci, ContactBThick] = offMagB } +// newton: solvers/xpbd/kernels.py: solve_body_contact_positions + +// StepBodyContacts generates contact forces for bodies. +func StepBodyContacts(i uint32) { //gosl:kernel + params := GetParams(0) + ci := int32(i) + cmax := ContactsN.Values[0] + if ci >= cmax { + return + } + biA := GetContactA(ci) + biB := GetContactB(ci) + diA := GetBodyDynamic(biA) + diB := GetBodyDynamic(biB) + + r0A := BodyDynamicPos(biA, params.Next) + q0A := BodyDynamicQuat(biA, params.Next) + + r0B := BodyDynamicPos(biB, params.Next) + q0B := BodyDynamicQuat(biB, params.Next) + + ctA := ContactAPoint(ci) + offA := ContactAOff(ci) + ctB := ContactBPoint(ci) + offB := ContactBOff(ci) + + ctAw := slmath.MulSpatialPoint(r0A, q0A, ctA) + ctBw := slmath.MulSpatialPoint(r0B, q0B, ctB) + thickA := Contacts[ci, ContactAThick] + thickB := Contacts[ci, ContactBThick] + thick := thickA + thickB + norm := ContactNorm(ci) + nnorm := slmath.Negate3(norm) + + d := slmath.Dot3(norm, ctBw.Sub(ctAw)) - thick + if d >= 0.0 { // now separated + return + } + comA := BodyCom(biA) + mInvA := Bodies[biA, BodyInvMass] + iInvA := BodyInvInertia(biA) + + comB := BodyCom(biB) + mInvB := Bodies[biB, BodyInvMass] + iInvB := BodyInvInertia(biB) + + var w0A, w0B math32.Vector3 + if diA >= 0 { + w0A = DynamicAngDelta(diA, params.Next) + } + if diB >= 0 { + w0B = DynamicAngDelta(diB, params.Next) + } + + // use average contact material properties + mu := 0.5 * (Bodies[biA, BodyFriction] + Bodies[biB, BodyFriction]) + frTors := 0.5 * (Bodies[biA, BodyFrictionTortion] + Bodies[biB, BodyFrictionTortion]) + frRoll := 0.5 * (Bodies[biA, BodyFrictionRolling] + Bodies[biB, BodyFrictionRolling]) + + // moment arms + dA := ctAw.Sub(slmath.MulSpatialPoint(r0A, q0A, comA)) + dB := ctBw.Sub(slmath.MulSpatialPoint(r0B, q0B, comB)) + + angA := slmath.Negate3(slmath.Cross3(dA, norm)) + angB := slmath.Cross3(dB, norm) + + // todo: contact_inv_weight -- requires atomic + + lambdaN := ContactConstraint(d, q0A, q0B, mInvA, mInvB, iInvA, iInvB, nnorm, norm, angA, angB, params.ContactRelax, params.Dt) + + linDeltaA := slmath.Negate3(norm).MulScalar(lambdaN) + linDeltaB := norm.MulScalar(lambdaN) + angDeltaA := angA.MulScalar(lambdaN) + angDeltaB := angB.MulScalar(lambdaN) + + // linear friction + if mu > 0.0 { + // add on displacement from surface offsets, this ensures + // we include any rotational effects due to thickness from feature + // need to use the current rotation to account for friction due to + // angular effects (e.g.: slipping contact) + ctAw = ctAw.Add(slmath.MulQuatVector(q0A, offA)) + ctBw = ctBw.Add(slmath.MulQuatVector(q0B, offB)) + + // update delta + delta := ctBw.Sub(ctAw) + frDelta := delta.Sub(norm.MulScalar(slmath.Dot3(norm, delta))) + + perp := slmath.Normal3(frDelta) + + angA = slmath.Negate3(slmath.Cross3(dA, perp)) + angB = slmath.Cross3(dB, perp) + + err := slmath.Length3(frDelta) + + if err > 0.0 { + lambdaFr := ContactConstraint(err, q0A, q0B, mInvA, mInvB, iInvA, iInvB, slmath.Negate3(perp), perp, angA, angB, params.ContactRelax, params.Dt) + + // limit friction based on incremental normal force, + // good approximation to limiting on total force + lambdaFr = max(lambdaFr, -lambdaN * mu) + + linDeltaA = linDeltaA.Sub(perp.MulScalar(lambdaFr)) + linDeltaB = linDeltaB.Add(perp.MulScalar(lambdaFr)) + angDeltaA = angDeltaA.Add(angA.MulScalar(lambdaFr)) + angDeltaB = angDeltaB.Add(angB.MulScalar(lambdaFr)) + } + } + + deltaW := w0B.Sub(w0A) + + if frTors > 0.0 { + err := slmath.Dot3(deltaW, norm) * params.Dt + + if math32.Abs(err) > 0.0 { + lin := math32.Vec3(0,0,0) + lambdaTors := ContactConstraint(err, q0A, q0B, mInvA, mInvB, iInvA, iInvB, lin, lin, nnorm, norm, params.ContactRelax, params.Dt) + + lambdaTors = math32.Clamp(lambdaTors, -lambdaN * frTors, lambdaN * frTors) + angDeltaA = angDeltaA.Sub(norm.MulScalar(lambdaTors)) + angDeltaB = angDeltaB.Add(norm.MulScalar(lambdaTors)) + } + } + + if frRoll > 0.0 { + deltaW = deltaW.Sub(norm.MulScalar(slmath.Dot3(norm, deltaW))) + err := slmath.Length3(deltaW) * params.Dt + if err > 0.0 { + lin := math32.Vec3(0,0,0) + rollN := slmath.Normal3(deltaW) + lambdaRoll := ContactConstraint(err, q0A, q0B, mInvA, mInvB, iInvA, iInvB, lin, lin, slmath.Negate3(rollN), rollN, params.ContactRelax, params.Dt) + lambdaRoll = max(lambdaRoll, -lambdaN * frRoll) + + angDeltaA = angDeltaA.Sub(rollN.MulScalar(lambdaRoll)) + angDeltaB = angDeltaB.Add(rollN.MulScalar(lambdaRoll)) + } + } + + SetContactADelta(ci, linDeltaA) + SetContactBDelta(ci, linDeltaB) + SetContactAAngDelta(ci, angDeltaA) + SetContactBAngDelta(ci, angDeltaB) +} + +// DeltasFromContacts gathers deltas, angDeltas from contacts per dynamic. +func DeltasFromContacts(i uint32) { //gosl:kernel + params := GetParams(0) + di := int32(i) + if di >= params.DynamicsN { + return + } + cmax := ContactsN.Values[0] + + bi := DynamicBody(di) + + td := DynamicDelta(di, params.Next) + ta := DynamicAngDelta(di, params.Next) + for ci := range cmax { + biA := GetContactA(ci) + biB := GetContactB(ci) + if biA == bi { + d := ContactADelta(ci) + td = td.Add(d) + a := ContactAAngDelta(ci) + ta = ta.Add(a) + } + if biB == bi { + d := ContactBDelta(ci) + td = td.Add(d) + a := ContactBAngDelta(ci) + ta = ta.Add(a) + } + } + SetDynamicDelta(di, params.Next, td) + SetDynamicAngDelta(di, params.Next, ta) +} + +func ContactConstraint(err float32, q0A, q0B math32.Quat, mInvA, mInvB float32, iInvA, iInvB math32.Matrix3, linA, linB, angA, angB math32.Vector3, relaxation, dt float32) float32 { + denom := float32(0.0) + denom += slmath.LengthSquared3(linA) * mInvA + denom += slmath.LengthSquared3(linB) * mInvB + + // Eq. 2-3 (make sure to project into the frame of the body) + rotAngA := slmath.MulQuatVectorInverse(q0A, angA) + rotAngB := slmath.MulQuatVectorInverse(q0B, angB) + + denom += slmath.Dot3(rotAngA, iInvA.MulVector3(rotAngA)) + denom += slmath.Dot3(rotAngB, iInvB.MulVector3(rotAngB)) + + lambda := -err + if denom > 0.0 { + lambda /= dt * denom + } + + return lambda * relaxation +} //gosl:end // IsChildDynamic returns true if dic is a direct child // on any joint where dip is the parent. func (wl *World) IsChildDynamic(dip, dic int32) bool { + if dip < 0 || dic < 0 { + return false + } npja := wl.BodyJoints[dip, 0, 0] for j := range npja { ji := wl.BodyJoints[dip, 0, 1+j] @@ -466,6 +707,7 @@ func (wl *World) ConfigBodyCollidePairs() { params.BodyCollidePairsN = int32(np) pt.SetShapeSizes(np, 2) wl.BodyCollidePairs = pt + BodyCollidePairs = pt fmt.Println("body pairs over alloc", nalc, np) } diff --git a/physics/enumgen.go b/physics/enumgen.go index 6a412f76..fe25ab53 100644 --- a/physics/enumgen.go +++ b/physics/enumgen.go @@ -6,20 +6,20 @@ import ( "cogentcore.org/core/enums" ) -var _BodyVarsValues = []BodyVars{0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40} +var _BodyVarsValues = []BodyVars{0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42} // BodyVarsN is the highest valid value for type BodyVars, plus one. // //gosl:start -const BodyVarsN BodyVars = 41 +const BodyVarsN BodyVars = 43 //gosl:end -var _BodyVarsValueMap = map[string]BodyVars{`BodyShape`: 0, `BodyDynamic`: 1, `BodyWorld`: 2, `BodyGroup`: 3, `BodyHSizeX`: 4, `BodyHSizeY`: 5, `BodyHSizeZ`: 6, `BodyThick`: 7, `BodyMass`: 8, `BodyInvMass`: 9, `BodyBounce`: 10, `BodyFriction`: 11, `BodyPosX`: 12, `BodyPosY`: 13, `BodyPosZ`: 14, `BodyQuatX`: 15, `BodyQuatY`: 16, `BodyQuatZ`: 17, `BodyQuatW`: 18, `BodyComX`: 19, `BodyComY`: 20, `BodyComZ`: 21, `BodyInertiaXX`: 22, `BodyInertiaYX`: 23, `BodyInertiaZX`: 24, `BodyInertiaXY`: 25, `BodyInertiaYY`: 26, `BodyInertiaZY`: 27, `BodyInertiaXZ`: 28, `BodyInertiaYZ`: 29, `BodyInertiaZZ`: 30, `BodyInvInertiaXX`: 31, `BodyInvInertiaYX`: 32, `BodyInvInertiaZX`: 33, `BodyInvInertiaXY`: 34, `BodyInvInertiaYY`: 35, `BodyInvInertiaZY`: 36, `BodyInvInertiaXZ`: 37, `BodyInvInertiaYZ`: 38, `BodyInvInertiaZZ`: 39, `BodyRadius`: 40} +var _BodyVarsValueMap = map[string]BodyVars{`BodyShape`: 0, `BodyDynamic`: 1, `BodyWorld`: 2, `BodyGroup`: 3, `BodyHSizeX`: 4, `BodyHSizeY`: 5, `BodyHSizeZ`: 6, `BodyThick`: 7, `BodyMass`: 8, `BodyInvMass`: 9, `BodyBounce`: 10, `BodyFriction`: 11, `BodyFrictionTortion`: 12, `BodyFrictionRolling`: 13, `BodyPosX`: 14, `BodyPosY`: 15, `BodyPosZ`: 16, `BodyQuatX`: 17, `BodyQuatY`: 18, `BodyQuatZ`: 19, `BodyQuatW`: 20, `BodyComX`: 21, `BodyComY`: 22, `BodyComZ`: 23, `BodyInertiaXX`: 24, `BodyInertiaYX`: 25, `BodyInertiaZX`: 26, `BodyInertiaXY`: 27, `BodyInertiaYY`: 28, `BodyInertiaZY`: 29, `BodyInertiaXZ`: 30, `BodyInertiaYZ`: 31, `BodyInertiaZZ`: 32, `BodyInvInertiaXX`: 33, `BodyInvInertiaYX`: 34, `BodyInvInertiaZX`: 35, `BodyInvInertiaXY`: 36, `BodyInvInertiaYY`: 37, `BodyInvInertiaZY`: 38, `BodyInvInertiaXZ`: 39, `BodyInvInertiaYZ`: 40, `BodyInvInertiaZZ`: 41, `BodyRadius`: 42} -var _BodyVarsDescMap = map[BodyVars]string{0: `BodyShape is the shape type of the object, as a Shapes type.`, 1: `BodyDynamic is the index into Dynamics for this body, which is -1 for static bodies. Use this to get current Pos and Quat values for a dynamic body.`, 2: `BodyWorld partitions bodies into different worlds for collision detection: Global bodies = -1 can collide with everything; otherwise only items within the same world collide.`, 3: `BodyGroup partitions bodies within worlds into different groups for collision detection. 0 does not collide with anything. Negative numbers are global within a world, except they don't collide amongst themselves (all non-dynamic bodies should go in -1 because they don't collide amongst each-other, but do potentially collide with dynamics). Positive numbers only collide amongst themselves, and with negative groups, but not other positive groups. This is for more special-purpose dynamics: in general use 1 for all dynamic bodies. There is an automatic constraint that the two objects within a single joint do not collide with each other, so this does not need to be handled here.`, 4: `BodyHSize is the size of the object (values depend on shape type).`, 5: ``, 6: ``, 7: `BodyThick is the thickness of the body, as a hollow shape. If 0, then it is a solid shape (default).`, 8: `BodyMass is the mass of the object.`, 9: `BodyInvMass is 1/mass of the object or 0 if no mass.`, 10: `BodyBounce specifies the COR or coefficient of restitution (0..1), which determines how elastic the collision is, i.e., final velocity / initial velocity.`, 11: `BodyFriction coefficient: how much friction is generated by transverse motion. Additive across the two surfaces.`, 12: `3D position of body (structural center).`, 13: ``, 14: ``, 15: `Quaternion rotation of body.`, 16: ``, 17: ``, 18: ``, 19: `Relative center-of-mass offset from 3D position of body.`, 20: ``, 21: ``, 22: `Inertia 3x3 matrix (column matrix organization, r,c labels).`, 23: ``, 24: ``, 25: ``, 26: ``, 27: ``, 28: ``, 29: ``, 30: ``, 31: `InvInertia inverse inertia 3x3 matrix (column matrix organization, r,c labels).`, 32: ``, 33: ``, 34: ``, 35: ``, 36: ``, 37: ``, 38: ``, 39: ``, 40: `radius for broadphase collision`} +var _BodyVarsDescMap = map[BodyVars]string{0: `BodyShape is the shape type of the object, as a Shapes type.`, 1: `BodyDynamic is the index into Dynamics for this body, which is -1 for static bodies. Use this to get current Pos and Quat values for a dynamic body.`, 2: `BodyWorld partitions bodies into different worlds for collision detection: Global bodies = -1 can collide with everything; otherwise only items within the same world collide.`, 3: `BodyGroup partitions bodies within worlds into different groups for collision detection. 0 does not collide with anything. Negative numbers are global within a world, except they don't collide amongst themselves (all non-dynamic bodies should go in -1 because they don't collide amongst each-other, but do potentially collide with dynamics). Positive numbers only collide amongst themselves, and with negative groups, but not other positive groups. This is for more special-purpose dynamics: in general use 1 for all dynamic bodies. There is an automatic constraint that the two objects within a single joint do not collide with each other, so this does not need to be handled here.`, 4: `BodyHSize is the size of the object (values depend on shape type).`, 5: ``, 6: ``, 7: `BodyThick is the thickness of the body, as a hollow shape. If 0, then it is a solid shape (default).`, 8: `BodyMass is the mass of the object.`, 9: `BodyInvMass is 1/mass of the object or 0 if no mass.`, 10: `BodyBounce specifies the COR or coefficient of restitution (0..1), which determines how elastic the collision is, i.e., final velocity / initial velocity.`, 11: `BodyFriction is the standard coefficient for linear friction (mu).`, 12: `BodyFrictionTortion is resistance to spinning at the contact point.`, 13: `BodyFrictionRolling is resistance to rolling motion at contact.`, 14: `3D position of body (structural center).`, 15: ``, 16: ``, 17: `Quaternion rotation of body.`, 18: ``, 19: ``, 20: ``, 21: `Relative center-of-mass offset from 3D position of body.`, 22: ``, 23: ``, 24: `Inertia 3x3 matrix (column matrix organization, r,c labels).`, 25: ``, 26: ``, 27: ``, 28: ``, 29: ``, 30: ``, 31: ``, 32: ``, 33: `InvInertia inverse inertia 3x3 matrix (column matrix organization, r,c labels).`, 34: ``, 35: ``, 36: ``, 37: ``, 38: ``, 39: ``, 40: ``, 41: ``, 42: `radius for broadphase collision`} -var _BodyVarsMap = map[BodyVars]string{0: `BodyShape`, 1: `BodyDynamic`, 2: `BodyWorld`, 3: `BodyGroup`, 4: `BodyHSizeX`, 5: `BodyHSizeY`, 6: `BodyHSizeZ`, 7: `BodyThick`, 8: `BodyMass`, 9: `BodyInvMass`, 10: `BodyBounce`, 11: `BodyFriction`, 12: `BodyPosX`, 13: `BodyPosY`, 14: `BodyPosZ`, 15: `BodyQuatX`, 16: `BodyQuatY`, 17: `BodyQuatZ`, 18: `BodyQuatW`, 19: `BodyComX`, 20: `BodyComY`, 21: `BodyComZ`, 22: `BodyInertiaXX`, 23: `BodyInertiaYX`, 24: `BodyInertiaZX`, 25: `BodyInertiaXY`, 26: `BodyInertiaYY`, 27: `BodyInertiaZY`, 28: `BodyInertiaXZ`, 29: `BodyInertiaYZ`, 30: `BodyInertiaZZ`, 31: `BodyInvInertiaXX`, 32: `BodyInvInertiaYX`, 33: `BodyInvInertiaZX`, 34: `BodyInvInertiaXY`, 35: `BodyInvInertiaYY`, 36: `BodyInvInertiaZY`, 37: `BodyInvInertiaXZ`, 38: `BodyInvInertiaYZ`, 39: `BodyInvInertiaZZ`, 40: `BodyRadius`} +var _BodyVarsMap = map[BodyVars]string{0: `BodyShape`, 1: `BodyDynamic`, 2: `BodyWorld`, 3: `BodyGroup`, 4: `BodyHSizeX`, 5: `BodyHSizeY`, 6: `BodyHSizeZ`, 7: `BodyThick`, 8: `BodyMass`, 9: `BodyInvMass`, 10: `BodyBounce`, 11: `BodyFriction`, 12: `BodyFrictionTortion`, 13: `BodyFrictionRolling`, 14: `BodyPosX`, 15: `BodyPosY`, 16: `BodyPosZ`, 17: `BodyQuatX`, 18: `BodyQuatY`, 19: `BodyQuatZ`, 20: `BodyQuatW`, 21: `BodyComX`, 22: `BodyComY`, 23: `BodyComZ`, 24: `BodyInertiaXX`, 25: `BodyInertiaYX`, 26: `BodyInertiaZX`, 27: `BodyInertiaXY`, 28: `BodyInertiaYY`, 29: `BodyInertiaZY`, 30: `BodyInertiaXZ`, 31: `BodyInertiaYZ`, 32: `BodyInertiaZZ`, 33: `BodyInvInertiaXX`, 34: `BodyInvInertiaYX`, 35: `BodyInvInertiaZX`, 36: `BodyInvInertiaXY`, 37: `BodyInvInertiaYY`, 38: `BodyInvInertiaZY`, 39: `BodyInvInertiaXZ`, 40: `BodyInvInertiaYZ`, 41: `BodyInvInertiaZZ`, 42: `BodyRadius`} // String returns the string representation of this BodyVars value. func (i BodyVars) String() string { return enums.String(i, _BodyVarsMap) } @@ -51,20 +51,20 @@ func (i BodyVars) MarshalText() ([]byte, error) { return []byte(i.String()), nil // UnmarshalText implements the [encoding.TextUnmarshaler] interface. func (i *BodyVars) UnmarshalText(text []byte) error { return enums.UnmarshalText(i, text, "BodyVars") } -var _ContactVarsValues = []ContactVars{0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22} +var _ContactVarsValues = []ContactVars{0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31} // ContactVarsN is the highest valid value for type ContactVars, plus one. // //gosl:start -const ContactVarsN ContactVars = 23 +const ContactVarsN ContactVars = 32 //gosl:end -var _ContactVarsValueMap = map[string]ContactVars{`ContactA`: 0, `ContactB`: 1, `ContactPointIdx`: 2, `ContactAPointX`: 3, `ContactAPointY`: 4, `ContactAPointZ`: 5, `ContactBPointX`: 6, `ContactBPointY`: 7, `ContactBPointZ`: 8, `ContactAOffX`: 9, `ContactAOffY`: 10, `ContactAOffZ`: 11, `ContactBOffX`: 12, `ContactBOffY`: 13, `ContactBOffZ`: 14, `ContactAThick`: 15, `ContactBThick`: 16, `ContactNormX`: 17, `ContactNormY`: 18, `ContactNormZ`: 19, `ContactForceX`: 20, `ContactForceY`: 21, `ContactForceZ`: 22} +var _ContactVarsValueMap = map[string]ContactVars{`ContactA`: 0, `ContactB`: 1, `ContactPointIdx`: 2, `ContactAPointX`: 3, `ContactAPointY`: 4, `ContactAPointZ`: 5, `ContactBPointX`: 6, `ContactBPointY`: 7, `ContactBPointZ`: 8, `ContactAOffX`: 9, `ContactAOffY`: 10, `ContactAOffZ`: 11, `ContactBOffX`: 12, `ContactBOffY`: 13, `ContactBOffZ`: 14, `ContactAThick`: 15, `ContactBThick`: 16, `ContactNormX`: 17, `ContactNormY`: 18, `ContactNormZ`: 19, `ContactADeltaX`: 20, `ContactADeltaY`: 21, `ContactADeltaZ`: 22, `ContactAAngDeltaX`: 23, `ContactAAngDeltaY`: 24, `ContactAAngDeltaZ`: 25, `ContactBDeltaX`: 26, `ContactBDeltaY`: 27, `ContactBDeltaZ`: 28, `ContactBAngDeltaX`: 29, `ContactBAngDeltaY`: 30, `ContactBAngDeltaZ`: 31} -var _ContactVarsDescMap = map[ContactVars]string{0: `first body index`, 1: `the other body index`, 2: `contact point index for A-B pair`, 3: `contact point on body A`, 4: ``, 5: ``, 6: `contact point on body B`, 7: ``, 8: ``, 9: `contact offset on body A`, 10: ``, 11: ``, 12: `contact offset on body B`, 13: ``, 14: ``, 15: `Contact thickness`, 16: ``, 17: `normal pointing from center of B to center of A`, 18: ``, 19: ``, 20: `computed contact force vector`, 21: ``, 22: ``} +var _ContactVarsDescMap = map[ContactVars]string{0: `first body index`, 1: `the other body index`, 2: `contact point index for A-B pair`, 3: `contact point on body A`, 4: ``, 5: ``, 6: `contact point on body B`, 7: ``, 8: ``, 9: `contact offset on body A`, 10: ``, 11: ``, 12: `contact offset on body B`, 13: ``, 14: ``, 15: `Contact thickness`, 16: ``, 17: `normal pointing from center of B to center of A`, 18: ``, 19: ``, 20: `computed contact deltas, A`, 21: ``, 22: ``, 23: ``, 24: ``, 25: ``, 26: `computed contact deltas, B`, 27: ``, 28: ``, 29: ``, 30: ``, 31: ``} -var _ContactVarsMap = map[ContactVars]string{0: `ContactA`, 1: `ContactB`, 2: `ContactPointIdx`, 3: `ContactAPointX`, 4: `ContactAPointY`, 5: `ContactAPointZ`, 6: `ContactBPointX`, 7: `ContactBPointY`, 8: `ContactBPointZ`, 9: `ContactAOffX`, 10: `ContactAOffY`, 11: `ContactAOffZ`, 12: `ContactBOffX`, 13: `ContactBOffY`, 14: `ContactBOffZ`, 15: `ContactAThick`, 16: `ContactBThick`, 17: `ContactNormX`, 18: `ContactNormY`, 19: `ContactNormZ`, 20: `ContactForceX`, 21: `ContactForceY`, 22: `ContactForceZ`} +var _ContactVarsMap = map[ContactVars]string{0: `ContactA`, 1: `ContactB`, 2: `ContactPointIdx`, 3: `ContactAPointX`, 4: `ContactAPointY`, 5: `ContactAPointZ`, 6: `ContactBPointX`, 7: `ContactBPointY`, 8: `ContactBPointZ`, 9: `ContactAOffX`, 10: `ContactAOffY`, 11: `ContactAOffZ`, 12: `ContactBOffX`, 13: `ContactBOffY`, 14: `ContactBOffZ`, 15: `ContactAThick`, 16: `ContactBThick`, 17: `ContactNormX`, 18: `ContactNormY`, 19: `ContactNormZ`, 20: `ContactADeltaX`, 21: `ContactADeltaY`, 22: `ContactADeltaZ`, 23: `ContactAAngDeltaX`, 24: `ContactAAngDeltaY`, 25: `ContactAAngDeltaZ`, 26: `ContactBDeltaX`, 27: `ContactBDeltaY`, 28: `ContactBDeltaZ`, 29: `ContactBAngDeltaX`, 30: `ContactBAngDeltaY`, 31: `ContactBAngDeltaZ`} // String returns the string representation of this ContactVars value. func (i ContactVars) String() string { return enums.String(i, _ContactVarsMap) } diff --git a/physics/gosl.go b/physics/gosl.go index 9924b16c..0c194792 100644 --- a/physics/gosl.go +++ b/physics/gosl.go @@ -137,6 +137,12 @@ func GPUInit() { pl.AddVarUsed(2, "ContactsN") pl.AddVarUsed(2, "Dynamics") pl.AddVarUsed(0, "Params") + pl = gpu.NewComputePipelineShaderFS(shaders, "shaders/DeltasFromContacts.wgsl", sy) + pl.AddVarUsed(0, "TensorStrides") + pl.AddVarUsed(2, "Contacts") + pl.AddVarUsed(2, "ContactsN") + pl.AddVarUsed(2, "Dynamics") + pl.AddVarUsed(0, "Params") pl = gpu.NewComputePipelineShaderFS(shaders, "shaders/DeltasFromJoints.wgsl", sy) pl.AddVarUsed(0, "TensorStrides") pl.AddVarUsed(1, "BodyJoints") @@ -158,6 +164,13 @@ func GPUInit() { pl.AddVarUsed(1, "Bodies") pl.AddVarUsed(2, "Dynamics") pl.AddVarUsed(0, "Params") + pl = gpu.NewComputePipelineShaderFS(shaders, "shaders/StepBodyContacts.wgsl", sy) + pl.AddVarUsed(0, "TensorStrides") + pl.AddVarUsed(1, "Bodies") + pl.AddVarUsed(2, "Contacts") + pl.AddVarUsed(2, "ContactsN") + pl.AddVarUsed(2, "Dynamics") + pl.AddVarUsed(0, "Params") pl = gpu.NewComputePipelineShaderFS(shaders, "shaders/StepBodyDeltas.wgsl", sy) pl.AddVarUsed(0, "TensorStrides") pl.AddVarUsed(1, "Bodies") @@ -329,6 +342,48 @@ func RunOneCollisionNarrow(n int, syncVars ...GPUVars) { RunCollisionNarrowCPU(n) } } +// RunDeltasFromContacts runs the DeltasFromContacts kernel with given number of elements, +// on either the CPU or GPU depending on the UseGPU variable. +// Can call multiple Run* kernels in a row, which are then all launched +// in the same command submission on the GPU, which is by far the most efficient. +// MUST call RunDone (with optional vars to sync) after all Run calls. +// Alternatively, a single-shot RunOneDeltasFromContacts call does Run and Done for a +// single run-and-sync case. +func RunDeltasFromContacts(n int) { + if UseGPU { + RunDeltasFromContactsGPU(n) + } else { + RunDeltasFromContactsCPU(n) + } +} + +// RunDeltasFromContactsGPU runs the DeltasFromContacts kernel on the GPU. See [RunDeltasFromContacts] for more info. +func RunDeltasFromContactsGPU(n int) { + sy := GPUSystem + pl := sy.ComputePipelines["DeltasFromContacts"] + ce, _ := sy.BeginComputePass() + pl.Dispatch1D(ce, n, 64) +} + +// RunDeltasFromContactsCPU runs the DeltasFromContacts kernel on the CPU. +func RunDeltasFromContactsCPU(n int) { + gpu.VectorizeFunc(0, n, DeltasFromContacts) +} + +// RunOneDeltasFromContacts runs the DeltasFromContacts kernel with given number of elements, +// on either the CPU or GPU depending on the UseGPU variable. +// This version then calls RunDone with the given variables to sync +// after the Run, for a single-shot Run-and-Done call. If multiple kernels +// can be run in sequence, it is much more efficient to do multiple Run* +// calls followed by a RunDone call. +func RunOneDeltasFromContacts(n int, syncVars ...GPUVars) { + if UseGPU { + RunDeltasFromContactsGPU(n) + RunDone(syncVars...) + } else { + RunDeltasFromContactsCPU(n) + } +} // RunDeltasFromJoints runs the DeltasFromJoints kernel with given number of elements, // on either the CPU or GPU depending on the UseGPU variable. // Can call multiple Run* kernels in a row, which are then all launched @@ -497,6 +552,48 @@ func RunOneInitDynamics(n int, syncVars ...GPUVars) { RunInitDynamicsCPU(n) } } +// RunStepBodyContacts runs the StepBodyContacts kernel with given number of elements, +// on either the CPU or GPU depending on the UseGPU variable. +// Can call multiple Run* kernels in a row, which are then all launched +// in the same command submission on the GPU, which is by far the most efficient. +// MUST call RunDone (with optional vars to sync) after all Run calls. +// Alternatively, a single-shot RunOneStepBodyContacts call does Run and Done for a +// single run-and-sync case. +func RunStepBodyContacts(n int) { + if UseGPU { + RunStepBodyContactsGPU(n) + } else { + RunStepBodyContactsCPU(n) + } +} + +// RunStepBodyContactsGPU runs the StepBodyContacts kernel on the GPU. See [RunStepBodyContacts] for more info. +func RunStepBodyContactsGPU(n int) { + sy := GPUSystem + pl := sy.ComputePipelines["StepBodyContacts"] + ce, _ := sy.BeginComputePass() + pl.Dispatch1D(ce, n, 64) +} + +// RunStepBodyContactsCPU runs the StepBodyContacts kernel on the CPU. +func RunStepBodyContactsCPU(n int) { + gpu.VectorizeFunc(0, n, StepBodyContacts) +} + +// RunOneStepBodyContacts runs the StepBodyContacts kernel with given number of elements, +// on either the CPU or GPU depending on the UseGPU variable. +// This version then calls RunDone with the given variables to sync +// after the Run, for a single-shot Run-and-Done call. If multiple kernels +// can be run in sequence, it is much more efficient to do multiple Run* +// calls followed by a RunDone call. +func RunOneStepBodyContacts(n int, syncVars ...GPUVars) { + if UseGPU { + RunStepBodyContactsGPU(n) + RunDone(syncVars...) + } else { + RunStepBodyContactsCPU(n) + } +} // RunStepBodyDeltas runs the StepBodyDeltas kernel with given number of elements, // on either the CPU or GPU depending on the UseGPU variable. // Can call multiple Run* kernels in a row, which are then all launched diff --git a/physics/params.go b/physics/params.go index 6d801721..11cdc537 100644 --- a/physics/params.go +++ b/physics/params.go @@ -40,7 +40,7 @@ type PhysParams struct { // AngularDamping is damping of angular motion. AngularDamping float32 `default:"0"` - // Contact weighting + // Contact weighting: todo: requires atomic add ContactWeighting slbool.Bool `default:"true"` // Restitution diff --git a/physics/shaders/CollisionBroad.wgsl b/physics/shaders/CollisionBroad.wgsl index 264e36e4..a39bbe92 100644 --- a/physics/shaders/CollisionBroad.wgsl +++ b/physics/shaders/CollisionBroad.wgsl @@ -57,35 +57,37 @@ const BodyMass: BodyVars = 8; const BodyInvMass: BodyVars = 9; const BodyBounce: BodyVars = 10; const BodyFriction: BodyVars = 11; -const BodyPosX: BodyVars = 12; -const BodyPosY: BodyVars = 13; -const BodyPosZ: BodyVars = 14; -const BodyQuatX: BodyVars = 15; -const BodyQuatY: BodyVars = 16; -const BodyQuatZ: BodyVars = 17; -const BodyQuatW: BodyVars = 18; -const BodyComX: BodyVars = 19; -const BodyComY: BodyVars = 20; -const BodyComZ: BodyVars = 21; -const BodyInertiaXX: BodyVars = 22; -const BodyInertiaYX: BodyVars = 23; -const BodyInertiaZX: BodyVars = 24; -const BodyInertiaXY: BodyVars = 25; -const BodyInertiaYY: BodyVars = 26; -const BodyInertiaZY: BodyVars = 27; -const BodyInertiaXZ: BodyVars = 28; -const BodyInertiaYZ: BodyVars = 29; -const BodyInertiaZZ: BodyVars = 30; -const BodyInvInertiaXX: BodyVars = 31; -const BodyInvInertiaYX: BodyVars = 32; -const BodyInvInertiaZX: BodyVars = 33; -const BodyInvInertiaXY: BodyVars = 34; -const BodyInvInertiaYY: BodyVars = 35; -const BodyInvInertiaZY: BodyVars = 36; -const BodyInvInertiaXZ: BodyVars = 37; -const BodyInvInertiaYZ: BodyVars = 38; -const BodyInvInertiaZZ: BodyVars = 39; -const BodyRadius: BodyVars = 40; +const BodyFrictionTortion: BodyVars = 12; +const BodyFrictionRolling: BodyVars = 13; +const BodyPosX: BodyVars = 14; +const BodyPosY: BodyVars = 15; +const BodyPosZ: BodyVars = 16; +const BodyQuatX: BodyVars = 17; +const BodyQuatY: BodyVars = 18; +const BodyQuatZ: BodyVars = 19; +const BodyQuatW: BodyVars = 20; +const BodyComX: BodyVars = 21; +const BodyComY: BodyVars = 22; +const BodyComZ: BodyVars = 23; +const BodyInertiaXX: BodyVars = 24; +const BodyInertiaYX: BodyVars = 25; +const BodyInertiaZX: BodyVars = 26; +const BodyInertiaXY: BodyVars = 27; +const BodyInertiaYY: BodyVars = 28; +const BodyInertiaZY: BodyVars = 29; +const BodyInertiaXZ: BodyVars = 30; +const BodyInertiaYZ: BodyVars = 31; +const BodyInertiaZZ: BodyVars = 32; +const BodyInvInertiaXX: BodyVars = 33; +const BodyInvInertiaYX: BodyVars = 34; +const BodyInvInertiaZX: BodyVars = 35; +const BodyInvInertiaXY: BodyVars = 36; +const BodyInvInertiaYY: BodyVars = 37; +const BodyInvInertiaZY: BodyVars = 38; +const BodyInvInertiaXZ: BodyVars = 39; +const BodyInvInertiaYZ: BodyVars = 40; +const BodyInvInertiaZZ: BodyVars = 41; +const BodyRadius: BodyVars = 42; fn GetBodyShape(idx: i32) -> Shapes { return Shapes(bitcast(Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyShape))])); } @@ -136,9 +138,18 @@ const ContactBThick: ContactVars = 16; const ContactNormX: ContactVars = 17; const ContactNormY: ContactVars = 18; const ContactNormZ: ContactVars = 19; -const ContactForceX: ContactVars = 20; -const ContactForceY: ContactVars = 21; -const ContactForceZ: ContactVars = 22; +const ContactADeltaX: ContactVars = 20; +const ContactADeltaY: ContactVars = 21; +const ContactADeltaZ: ContactVars = 22; +const ContactAAngDeltaX: ContactVars = 23; +const ContactAAngDeltaY: ContactVars = 24; +const ContactAAngDeltaZ: ContactVars = 25; +const ContactBDeltaX: ContactVars = 26; +const ContactBDeltaY: ContactVars = 27; +const ContactBDeltaZ: ContactVars = 28; +const ContactBAngDeltaX: ContactVars = 29; +const ContactBAngDeltaY: ContactVars = 30; +const ContactBAngDeltaZ: ContactVars = 31; const BroadContactVarsN = ContactAPointX; fn SetBroadContactA(idx: i32,bodIdx: i32) { BroadContacts[Index2D(TensorStrides[70], TensorStrides[71], u32(idx), u32(ContactA))] = bitcast(u32(bodIdx)); @@ -255,8 +266,8 @@ fn DynamicQuat(idx: i32,cni: i32) -> vec4 { } //////// import: "enumgen.go" -const BodyVarsN: BodyVars = 41; -const ContactVarsN: ContactVars = 23; +const BodyVarsN: BodyVars = 43; +const ContactVarsN: ContactVars = 32; const JointControlVarsN: JointControlVars = 3; const DynamicVarsN: DynamicVars = 32; const GPUVarsN: GPUVars = 12; diff --git a/physics/shaders/CollisionInit.wgsl b/physics/shaders/CollisionInit.wgsl new file mode 100644 index 00000000..55dd83d8 --- /dev/null +++ b/physics/shaders/CollisionInit.wgsl @@ -0,0 +1,307 @@ +// Code generated by "gosl"; DO NOT EDIT +// kernel: CollisionInit + +// // Params are global parameters. +@group(0) @binding(0) +var TensorStrides: array; +// // Bodies are the rigid body elements (dynamic and static), // specifying the constant, non-dynamic properties, // which is initial state for dynamics. // [body][BodyVarsN] +// // Dynamics are the dynamic rigid body elements: these actually move. // Two alternating states are used: Params.Cur and Params.Next. // [dyn body][cur/next][DynamicVarsN] +@group(2) @binding(1) +var BroadContactsN: array; +@group(2) @binding(3) +var ContactsN: array; +// // JointControls are dynamic joint control inputs, per joint DoF // (in correspondence with [JointDoFs]). This can be uploaded to the // GPU at every step. // [dof][JointControlVarsN] + +alias GPUVars = i32; + +@compute @workgroup_size(64, 1, 1) +fn main(@builtin(workgroup_id) wgid: vec3, @builtin(num_workgroups) nwg: vec3, @builtin(local_invocation_index) loci: u32) { + let idx = loci + (wgid.x + wgid.y * nwg.x + wgid.z * nwg.x * nwg.y) * 64; + CollisionInit(idx); +} + +fn Index1D(s0: u32, i0: u32) -> u32 { + return s0 * i0; +} + + +//////// import: "vars.go" + +//////// import: "body.go" +alias BodyVars = i32; //enums:enum +const BodyShape: BodyVars = 0; +const BodyDynamic: BodyVars = 1; +const BodyWorld: BodyVars = 2; +const BodyGroup: BodyVars = 3; +const BodyHSizeX: BodyVars = 4; +const BodyHSizeY: BodyVars = 5; +const BodyHSizeZ: BodyVars = 6; +const BodyThick: BodyVars = 7; +const BodyMass: BodyVars = 8; +const BodyInvMass: BodyVars = 9; +const BodyBounce: BodyVars = 10; +const BodyFriction: BodyVars = 11; +const BodyFrictionTortion: BodyVars = 12; +const BodyFrictionRolling: BodyVars = 13; +const BodyPosX: BodyVars = 14; +const BodyPosY: BodyVars = 15; +const BodyPosZ: BodyVars = 16; +const BodyQuatX: BodyVars = 17; +const BodyQuatY: BodyVars = 18; +const BodyQuatZ: BodyVars = 19; +const BodyQuatW: BodyVars = 20; +const BodyComX: BodyVars = 21; +const BodyComY: BodyVars = 22; +const BodyComZ: BodyVars = 23; +const BodyInertiaXX: BodyVars = 24; +const BodyInertiaYX: BodyVars = 25; +const BodyInertiaZX: BodyVars = 26; +const BodyInertiaXY: BodyVars = 27; +const BodyInertiaYY: BodyVars = 28; +const BodyInertiaZY: BodyVars = 29; +const BodyInertiaXZ: BodyVars = 30; +const BodyInertiaYZ: BodyVars = 31; +const BodyInertiaZZ: BodyVars = 32; +const BodyInvInertiaXX: BodyVars = 33; +const BodyInvInertiaYX: BodyVars = 34; +const BodyInvInertiaZX: BodyVars = 35; +const BodyInvInertiaXY: BodyVars = 36; +const BodyInvInertiaYY: BodyVars = 37; +const BodyInvInertiaZY: BodyVars = 38; +const BodyInvInertiaXZ: BodyVars = 39; +const BodyInvInertiaYZ: BodyVars = 40; +const BodyInvInertiaZZ: BodyVars = 41; +const BodyRadius: BodyVars = 42; + +//////// import: "contact.go" +alias ContactVars = i32; //enums:enum +const ContactA: ContactVars = 0; +const ContactB: ContactVars = 1; +const ContactPointIdx: ContactVars = 2; +const ContactAPointX: ContactVars = 3; +const ContactAPointY: ContactVars = 4; +const ContactAPointZ: ContactVars = 5; +const ContactBPointX: ContactVars = 6; +const ContactBPointY: ContactVars = 7; +const ContactBPointZ: ContactVars = 8; +const ContactAOffX: ContactVars = 9; +const ContactAOffY: ContactVars = 10; +const ContactAOffZ: ContactVars = 11; +const ContactBOffX: ContactVars = 12; +const ContactBOffY: ContactVars = 13; +const ContactBOffZ: ContactVars = 14; +const ContactAThick: ContactVars = 15; +const ContactBThick: ContactVars = 16; +const ContactNormX: ContactVars = 17; +const ContactNormY: ContactVars = 18; +const ContactNormZ: ContactVars = 19; +const ContactADeltaX: ContactVars = 20; +const ContactADeltaY: ContactVars = 21; +const ContactADeltaZ: ContactVars = 22; +const ContactAAngDeltaX: ContactVars = 23; +const ContactAAngDeltaY: ContactVars = 24; +const ContactAAngDeltaZ: ContactVars = 25; +const ContactBDeltaX: ContactVars = 26; +const ContactBDeltaY: ContactVars = 27; +const ContactBDeltaZ: ContactVars = 28; +const ContactBAngDeltaX: ContactVars = 29; +const ContactBAngDeltaY: ContactVars = 30; +const ContactBAngDeltaZ: ContactVars = 31; +const BroadContactVarsN = ContactAPointX; +fn CollisionInit(i: u32) { //gosl:kernel + if (i > 0) { + return; + } + BroadContactsN[0] = 0; + ContactsN[0] = 0; +} + +//////// import: "control.go" +alias JointControlVars = i32; //enums:enum +const JointControlForce: JointControlVars = 0; +const JointTargetPos: JointControlVars = 1; +const JointTargetVel: JointControlVars = 2; + +//////// import: "dynamics.go" +alias DynamicVars = i32; //enums:enum +const DynBody: DynamicVars = 0; +const DynPosX: DynamicVars = 1; +const DynPosY: DynamicVars = 2; +const DynPosZ: DynamicVars = 3; +const DynQuatX: DynamicVars = 4; +const DynQuatY: DynamicVars = 5; +const DynQuatZ: DynamicVars = 6; +const DynQuatW: DynamicVars = 7; +const DynVelX: DynamicVars = 8; +const DynVelY: DynamicVars = 9; +const DynVelZ: DynamicVars = 10; +const DynAngVelX: DynamicVars = 11; +const DynAngVelY: DynamicVars = 12; +const DynAngVelZ: DynamicVars = 13; +const DynAccX: DynamicVars = 14; +const DynAccY: DynamicVars = 15; +const DynAccZ: DynamicVars = 16; +const DynAngAccX: DynamicVars = 17; +const DynAngAccY: DynamicVars = 18; +const DynAngAccZ: DynamicVars = 19; +const DynForceX: DynamicVars = 20; +const DynForceY: DynamicVars = 21; +const DynForceZ: DynamicVars = 22; +const DynTorqueX: DynamicVars = 23; +const DynTorqueY: DynamicVars = 24; +const DynTorqueZ: DynamicVars = 25; +const DynDeltaX: DynamicVars = 26; +const DynDeltaY: DynamicVars = 27; +const DynDeltaZ: DynamicVars = 28; +const DynAngDeltaX: DynamicVars = 29; +const DynAngDeltaY: DynamicVars = 30; +const DynAngDeltaZ: DynamicVars = 31; + +//////// import: "enumgen.go" +const BodyVarsN: BodyVars = 43; +const ContactVarsN: ContactVars = 32; +const JointControlVarsN: JointControlVars = 3; +const DynamicVarsN: DynamicVars = 32; +const GPUVarsN: GPUVars = 12; +const JointTypesN: JointTypes = 7; +const JointVarsN: JointVars = 50; +const JointDoFVarsN: JointDoFVars = 7; +const ShapesN: Shapes = 6; + +//////// import: "joint.go" +const JointLimitUnlimited = 1e10; +alias JointTypes = i32; //enums:enum +const Prismatic: JointTypes = 0; +const Revolute: JointTypes = 1; +const Ball: JointTypes = 2; +const Fixed: JointTypes = 3; +const Free: JointTypes = 4; +const Distance: JointTypes = 5; +const D6: JointTypes = 6; +alias JointVars = i32; //enums:enum +const JointType: JointVars = 0; +const JointEnabled: JointVars = 1; +const JointParent: JointVars = 2; +const JointChild: JointVars = 3; +const JointPPosX: JointVars = 4; +const JointPPosY: JointVars = 5; +const JointPPosZ: JointVars = 6; +const JointPQuatX: JointVars = 7; +const JointPQuatY: JointVars = 8; +const JointPQuatZ: JointVars = 9; +const JointPQuatW: JointVars = 10; +const JointCPosX: JointVars = 11; +const JointCPosY: JointVars = 12; +const JointCPosZ: JointVars = 13; +const JointCQuatX: JointVars = 14; +const JointCQuatY: JointVars = 15; +const JointCQuatZ: JointVars = 16; +const JointCQuatW: JointVars = 17; +const JointLinearDoFN: JointVars = 18; +const JointAngularDoFN: JointVars = 19; +const JointDoF1: JointVars = 20; +const JointDoF2: JointVars = 21; +const JointDoF3: JointVars = 22; +const JointDoF4: JointVars = 23; +const JointDoF5: JointVars = 24; +const JointDoF6: JointVars = 25; +const JointPForceX: JointVars = 26; +const JointPForceY: JointVars = 27; +const JointPForceZ: JointVars = 28; +const JointPTorqueX: JointVars = 29; +const JointPTorqueY: JointVars = 30; +const JointPTorqueZ: JointVars = 31; +const JointCForceX: JointVars = 32; +const JointCForceY: JointVars = 33; +const JointCForceZ: JointVars = 34; +const JointCTorqueX: JointVars = 35; +const JointCTorqueY: JointVars = 36; +const JointCTorqueZ: JointVars = 37; +const JointPDeltaX: JointVars = 38; +const JointPDeltaY: JointVars = 39; +const JointPDeltaZ: JointVars = 40; +const JointPAngDeltaX: JointVars = 41; +const JointPAngDeltaY: JointVars = 42; +const JointPAngDeltaZ: JointVars = 43; +const JointCDeltaX: JointVars = 44; +const JointCDeltaY: JointVars = 45; +const JointCDeltaZ: JointVars = 46; +const JointCAngDeltaX: JointVars = 47; +const JointCAngDeltaY: JointVars = 48; +const JointCAngDeltaZ: JointVars = 49; +alias JointDoFVars = i32; //enums:enum +const JointAxisX: JointDoFVars = 0; +const JointAxisY: JointDoFVars = 1; +const JointAxisZ: JointDoFVars = 2; +const JointLimitLower: JointDoFVars = 3; +const JointLimitUpper: JointDoFVars = 4; +const JointStiff: JointDoFVars = 5; +const JointDamp: JointDoFVars = 6; + +//////// import: "params.go" +struct PhysParams { + Iterations: i32, + Dt: f32, + SoftRelax: f32, + JointLinearRelax: f32, + JointAngularRelax: f32, + JointLinearComply: f32, + JointAngularComply: f32, + ContactRelax: f32, + AngularDamping: f32, + ContactWeighting: i32, + Restitution: i32, + MaxGeomIter: i32, + ContactMargin: f32, + ContactsMax: i32, + Cur: i32, + Next: i32, + BodiesN: i32, + DynamicsN: i32, + JointsN: i32, + JointDoFsN: i32, + BodyJointsMax: i32, + BodyCollidePairsN: i32, + pad: i32, + pad1: i32, + Gravity: vec4, +} + +//////// import: "shapecollide.go" +struct GeomData { + BodyIdx: i32, + Shape: Shapes, + MinSize: f32, + Thick: f32, + Radius: f32, + Size: vec3, + WbR: vec3, + WbQ: vec4, + BwR: vec3, + BwQ: vec4, +} + +//////// import: "shapegeom.go" + +//////// import: "shapes.go" +alias Shapes = i32; //enums:enum +const Plane: Shapes = 0; +const Sphere: Shapes = 1; +const Capsule: Shapes = 2; +const Cylinder: Shapes = 3; +const Box: Shapes = 4; +const Cone: Shapes = 5; + +//////// import: "slmath-matrix3.go" + +//////// import: "slmath-quaternion.go" + +//////// import: "slmath-vector2.go" + +//////// import: "slmath-vector3.go" + +//////// import: "step.go" + +//////// import: "step_body.go" + +//////// import: "step_joint.go" \ No newline at end of file diff --git a/physics/shaders/CollisionNarrow.wgsl b/physics/shaders/CollisionNarrow.wgsl index bd1572a4..1c70a718 100644 --- a/physics/shaders/CollisionNarrow.wgsl +++ b/physics/shaders/CollisionNarrow.wgsl @@ -59,35 +59,37 @@ const BodyMass: BodyVars = 8; const BodyInvMass: BodyVars = 9; const BodyBounce: BodyVars = 10; const BodyFriction: BodyVars = 11; -const BodyPosX: BodyVars = 12; -const BodyPosY: BodyVars = 13; -const BodyPosZ: BodyVars = 14; -const BodyQuatX: BodyVars = 15; -const BodyQuatY: BodyVars = 16; -const BodyQuatZ: BodyVars = 17; -const BodyQuatW: BodyVars = 18; -const BodyComX: BodyVars = 19; -const BodyComY: BodyVars = 20; -const BodyComZ: BodyVars = 21; -const BodyInertiaXX: BodyVars = 22; -const BodyInertiaYX: BodyVars = 23; -const BodyInertiaZX: BodyVars = 24; -const BodyInertiaXY: BodyVars = 25; -const BodyInertiaYY: BodyVars = 26; -const BodyInertiaZY: BodyVars = 27; -const BodyInertiaXZ: BodyVars = 28; -const BodyInertiaYZ: BodyVars = 29; -const BodyInertiaZZ: BodyVars = 30; -const BodyInvInertiaXX: BodyVars = 31; -const BodyInvInertiaYX: BodyVars = 32; -const BodyInvInertiaZX: BodyVars = 33; -const BodyInvInertiaXY: BodyVars = 34; -const BodyInvInertiaYY: BodyVars = 35; -const BodyInvInertiaZY: BodyVars = 36; -const BodyInvInertiaXZ: BodyVars = 37; -const BodyInvInertiaYZ: BodyVars = 38; -const BodyInvInertiaZZ: BodyVars = 39; -const BodyRadius: BodyVars = 40; +const BodyFrictionTortion: BodyVars = 12; +const BodyFrictionRolling: BodyVars = 13; +const BodyPosX: BodyVars = 14; +const BodyPosY: BodyVars = 15; +const BodyPosZ: BodyVars = 16; +const BodyQuatX: BodyVars = 17; +const BodyQuatY: BodyVars = 18; +const BodyQuatZ: BodyVars = 19; +const BodyQuatW: BodyVars = 20; +const BodyComX: BodyVars = 21; +const BodyComY: BodyVars = 22; +const BodyComZ: BodyVars = 23; +const BodyInertiaXX: BodyVars = 24; +const BodyInertiaYX: BodyVars = 25; +const BodyInertiaZX: BodyVars = 26; +const BodyInertiaXY: BodyVars = 27; +const BodyInertiaYY: BodyVars = 28; +const BodyInertiaZY: BodyVars = 29; +const BodyInertiaXZ: BodyVars = 30; +const BodyInertiaYZ: BodyVars = 31; +const BodyInertiaZZ: BodyVars = 32; +const BodyInvInertiaXX: BodyVars = 33; +const BodyInvInertiaYX: BodyVars = 34; +const BodyInvInertiaZX: BodyVars = 35; +const BodyInvInertiaXY: BodyVars = 36; +const BodyInvInertiaYY: BodyVars = 37; +const BodyInvInertiaZY: BodyVars = 38; +const BodyInvInertiaXZ: BodyVars = 39; +const BodyInvInertiaYZ: BodyVars = 40; +const BodyInvInertiaZZ: BodyVars = 41; +const BodyRadius: BodyVars = 42; fn GetBodyShape(idx: i32) -> Shapes { return Shapes(bitcast(Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyShape))])); } @@ -138,9 +140,18 @@ const ContactBThick: ContactVars = 16; const ContactNormX: ContactVars = 17; const ContactNormY: ContactVars = 18; const ContactNormZ: ContactVars = 19; -const ContactForceX: ContactVars = 20; -const ContactForceY: ContactVars = 21; -const ContactForceZ: ContactVars = 22; +const ContactADeltaX: ContactVars = 20; +const ContactADeltaY: ContactVars = 21; +const ContactADeltaZ: ContactVars = 22; +const ContactAAngDeltaX: ContactVars = 23; +const ContactAAngDeltaY: ContactVars = 24; +const ContactAAngDeltaZ: ContactVars = 25; +const ContactBDeltaX: ContactVars = 26; +const ContactBDeltaY: ContactVars = 27; +const ContactBDeltaZ: ContactVars = 28; +const ContactBAngDeltaX: ContactVars = 29; +const ContactBAngDeltaY: ContactVars = 30; +const ContactBAngDeltaZ: ContactVars = 31; const BroadContactVarsN = ContactAPointX; fn GetBroadContactA(idx: i32) -> i32 { return i32(bitcast(BroadContacts[Index2D(TensorStrides[70], TensorStrides[71], u32(idx), u32(ContactA))])); @@ -344,8 +355,8 @@ fn DynamicQuat(idx: i32,cni: i32) -> vec4 { } //////// import: "enumgen.go" -const BodyVarsN: BodyVars = 41; -const ContactVarsN: ContactVars = 23; +const BodyVarsN: BodyVars = 43; +const ContactVarsN: ContactVars = 32; const JointControlVarsN: JointControlVars = 3; const DynamicVarsN: DynamicVars = 32; const GPUVarsN: GPUVars = 12; diff --git a/physics/shaders/DeltasFromContacts.wgsl b/physics/shaders/DeltasFromContacts.wgsl new file mode 100644 index 00000000..7581c18b --- /dev/null +++ b/physics/shaders/DeltasFromContacts.wgsl @@ -0,0 +1,374 @@ +// Code generated by "gosl"; DO NOT EDIT +// kernel: DeltasFromContacts + +// // Params are global parameters. +@group(0) @binding(0) +var TensorStrides: array; +@group(0) @binding(1) +var Params: array; +// // Bodies are the rigid body elements (dynamic and static), // specifying the constant, non-dynamic properties, // which is initial state for dynamics. // [body][BodyVarsN] +// // Dynamics are the dynamic rigid body elements: these actually move. // Two alternating states are used: Params.Cur and Params.Next. // [dyn body][cur/next][DynamicVarsN] +@group(2) @binding(0) +var Dynamics: array; +@group(2) @binding(3) +var ContactsN: array; +@group(2) @binding(4) +var Contacts: array; +// // JointControls are dynamic joint control inputs, per joint DoF // (in correspondence with [JointDoFs]). This can be uploaded to the // GPU at every step. // [dof][JointControlVarsN] + +alias GPUVars = i32; + +@compute @workgroup_size(64, 1, 1) +fn main(@builtin(workgroup_id) wgid: vec3, @builtin(num_workgroups) nwg: vec3, @builtin(local_invocation_index) loci: u32) { + let idx = loci + (wgid.x + wgid.y * nwg.x + wgid.z * nwg.x * nwg.y) * 64; + DeltasFromContacts(idx); +} + +fn Index3D(s0: u32, s1: u32, s2: u32, i0: u32, i1: u32, i2: u32) -> u32 { + return s0 * i0 + s1 * i1 + s2 * i2; +} + +fn Index1D(s0: u32, i0: u32) -> u32 { + return s0 * i0; +} + +fn Index2D(s0: u32, s1: u32, i0: u32, i1: u32) -> u32 { + return s0 * i0 + s1 * i1; +} + + +//////// import: "vars.go" + +//////// import: "body.go" +alias BodyVars = i32; //enums:enum +const BodyShape: BodyVars = 0; +const BodyDynamic: BodyVars = 1; +const BodyWorld: BodyVars = 2; +const BodyGroup: BodyVars = 3; +const BodyHSizeX: BodyVars = 4; +const BodyHSizeY: BodyVars = 5; +const BodyHSizeZ: BodyVars = 6; +const BodyThick: BodyVars = 7; +const BodyMass: BodyVars = 8; +const BodyInvMass: BodyVars = 9; +const BodyBounce: BodyVars = 10; +const BodyFriction: BodyVars = 11; +const BodyFrictionTortion: BodyVars = 12; +const BodyFrictionRolling: BodyVars = 13; +const BodyPosX: BodyVars = 14; +const BodyPosY: BodyVars = 15; +const BodyPosZ: BodyVars = 16; +const BodyQuatX: BodyVars = 17; +const BodyQuatY: BodyVars = 18; +const BodyQuatZ: BodyVars = 19; +const BodyQuatW: BodyVars = 20; +const BodyComX: BodyVars = 21; +const BodyComY: BodyVars = 22; +const BodyComZ: BodyVars = 23; +const BodyInertiaXX: BodyVars = 24; +const BodyInertiaYX: BodyVars = 25; +const BodyInertiaZX: BodyVars = 26; +const BodyInertiaXY: BodyVars = 27; +const BodyInertiaYY: BodyVars = 28; +const BodyInertiaZY: BodyVars = 29; +const BodyInertiaXZ: BodyVars = 30; +const BodyInertiaYZ: BodyVars = 31; +const BodyInertiaZZ: BodyVars = 32; +const BodyInvInertiaXX: BodyVars = 33; +const BodyInvInertiaYX: BodyVars = 34; +const BodyInvInertiaZX: BodyVars = 35; +const BodyInvInertiaXY: BodyVars = 36; +const BodyInvInertiaYY: BodyVars = 37; +const BodyInvInertiaZY: BodyVars = 38; +const BodyInvInertiaXZ: BodyVars = 39; +const BodyInvInertiaYZ: BodyVars = 40; +const BodyInvInertiaZZ: BodyVars = 41; +const BodyRadius: BodyVars = 42; + +//////// import: "contact.go" +alias ContactVars = i32; //enums:enum +const ContactA: ContactVars = 0; +const ContactB: ContactVars = 1; +const ContactPointIdx: ContactVars = 2; +const ContactAPointX: ContactVars = 3; +const ContactAPointY: ContactVars = 4; +const ContactAPointZ: ContactVars = 5; +const ContactBPointX: ContactVars = 6; +const ContactBPointY: ContactVars = 7; +const ContactBPointZ: ContactVars = 8; +const ContactAOffX: ContactVars = 9; +const ContactAOffY: ContactVars = 10; +const ContactAOffZ: ContactVars = 11; +const ContactBOffX: ContactVars = 12; +const ContactBOffY: ContactVars = 13; +const ContactBOffZ: ContactVars = 14; +const ContactAThick: ContactVars = 15; +const ContactBThick: ContactVars = 16; +const ContactNormX: ContactVars = 17; +const ContactNormY: ContactVars = 18; +const ContactNormZ: ContactVars = 19; +const ContactADeltaX: ContactVars = 20; +const ContactADeltaY: ContactVars = 21; +const ContactADeltaZ: ContactVars = 22; +const ContactAAngDeltaX: ContactVars = 23; +const ContactAAngDeltaY: ContactVars = 24; +const ContactAAngDeltaZ: ContactVars = 25; +const ContactBDeltaX: ContactVars = 26; +const ContactBDeltaY: ContactVars = 27; +const ContactBDeltaZ: ContactVars = 28; +const ContactBAngDeltaX: ContactVars = 29; +const ContactBAngDeltaY: ContactVars = 30; +const ContactBAngDeltaZ: ContactVars = 31; +const BroadContactVarsN = ContactAPointX; +fn GetContactA(idx: i32) -> i32 { return i32(bitcast(Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactA))])); } +fn GetContactB(idx: i32) -> i32 { return i32(bitcast(Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactB))])); } +fn ContactADelta(idx: i32) -> vec3 { + return vec3(Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactADeltaX))], Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactADeltaY))], Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactADeltaZ))]); +} +fn ContactAAngDelta(idx: i32) -> vec3 { + return vec3(Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactAAngDeltaX))], Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactAAngDeltaY))], Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactAAngDeltaZ))]); +} +fn ContactBDelta(idx: i32) -> vec3 { + return vec3(Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactBDeltaX))], Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactBDeltaY))], Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactBDeltaZ))]); +} +fn ContactBAngDelta(idx: i32) -> vec3 { + return vec3(Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactBAngDeltaX))], Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactBAngDeltaY))], Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactBAngDeltaZ))]); +} +fn DeltasFromContacts(i: u32) { //gosl:kernel + let params = Params[0]; + var di = i32(i); + if (di >= params.DynamicsN) { + return; + } + var cmax = ContactsN[0]; + var bi = DynamicBody(di); + var td = DynamicDelta(di, params.Next); + var ta = DynamicAngDelta(di, params.Next); + for (var ci=0; ci i32 { + return i32(bitcast(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(0), u32(DynBody))])); +} +fn DynamicDelta(idx: i32,cni: i32) -> vec3 { + return vec3(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaZ))]); +} +fn SetDynamicDelta(idx: i32,cni: i32, delta: vec3) { + Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaX))] = delta.x; + Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaY))] = delta.y; + Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaZ))] = delta.z; +} +fn DynamicAngDelta(idx: i32,cni: i32) -> vec3 { + return vec3(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaZ))]); +} +fn SetDynamicAngDelta(idx: i32,cni: i32, angDelta: vec3) { + Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaX))] = angDelta.x; + Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaY))] = angDelta.y; + Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaZ))] = angDelta.z; +} + +//////// import: "enumgen.go" +const BodyVarsN: BodyVars = 43; +const ContactVarsN: ContactVars = 32; +const JointControlVarsN: JointControlVars = 3; +const DynamicVarsN: DynamicVars = 32; +const GPUVarsN: GPUVars = 12; +const JointTypesN: JointTypes = 7; +const JointVarsN: JointVars = 50; +const JointDoFVarsN: JointDoFVars = 7; +const ShapesN: Shapes = 6; + +//////// import: "joint.go" +const JointLimitUnlimited = 1e10; +alias JointTypes = i32; //enums:enum +const Prismatic: JointTypes = 0; +const Revolute: JointTypes = 1; +const Ball: JointTypes = 2; +const Fixed: JointTypes = 3; +const Free: JointTypes = 4; +const Distance: JointTypes = 5; +const D6: JointTypes = 6; +alias JointVars = i32; //enums:enum +const JointType: JointVars = 0; +const JointEnabled: JointVars = 1; +const JointParent: JointVars = 2; +const JointChild: JointVars = 3; +const JointPPosX: JointVars = 4; +const JointPPosY: JointVars = 5; +const JointPPosZ: JointVars = 6; +const JointPQuatX: JointVars = 7; +const JointPQuatY: JointVars = 8; +const JointPQuatZ: JointVars = 9; +const JointPQuatW: JointVars = 10; +const JointCPosX: JointVars = 11; +const JointCPosY: JointVars = 12; +const JointCPosZ: JointVars = 13; +const JointCQuatX: JointVars = 14; +const JointCQuatY: JointVars = 15; +const JointCQuatZ: JointVars = 16; +const JointCQuatW: JointVars = 17; +const JointLinearDoFN: JointVars = 18; +const JointAngularDoFN: JointVars = 19; +const JointDoF1: JointVars = 20; +const JointDoF2: JointVars = 21; +const JointDoF3: JointVars = 22; +const JointDoF4: JointVars = 23; +const JointDoF5: JointVars = 24; +const JointDoF6: JointVars = 25; +const JointPForceX: JointVars = 26; +const JointPForceY: JointVars = 27; +const JointPForceZ: JointVars = 28; +const JointPTorqueX: JointVars = 29; +const JointPTorqueY: JointVars = 30; +const JointPTorqueZ: JointVars = 31; +const JointCForceX: JointVars = 32; +const JointCForceY: JointVars = 33; +const JointCForceZ: JointVars = 34; +const JointCTorqueX: JointVars = 35; +const JointCTorqueY: JointVars = 36; +const JointCTorqueZ: JointVars = 37; +const JointPDeltaX: JointVars = 38; +const JointPDeltaY: JointVars = 39; +const JointPDeltaZ: JointVars = 40; +const JointPAngDeltaX: JointVars = 41; +const JointPAngDeltaY: JointVars = 42; +const JointPAngDeltaZ: JointVars = 43; +const JointCDeltaX: JointVars = 44; +const JointCDeltaY: JointVars = 45; +const JointCDeltaZ: JointVars = 46; +const JointCAngDeltaX: JointVars = 47; +const JointCAngDeltaY: JointVars = 48; +const JointCAngDeltaZ: JointVars = 49; +alias JointDoFVars = i32; //enums:enum +const JointAxisX: JointDoFVars = 0; +const JointAxisY: JointDoFVars = 1; +const JointAxisZ: JointDoFVars = 2; +const JointLimitLower: JointDoFVars = 3; +const JointLimitUpper: JointDoFVars = 4; +const JointStiff: JointDoFVars = 5; +const JointDamp: JointDoFVars = 6; + +//////// import: "params.go" +struct PhysParams { + Iterations: i32, + Dt: f32, + SoftRelax: f32, + JointLinearRelax: f32, + JointAngularRelax: f32, + JointLinearComply: f32, + JointAngularComply: f32, + ContactRelax: f32, + AngularDamping: f32, + ContactWeighting: i32, + Restitution: i32, + MaxGeomIter: i32, + ContactMargin: f32, + ContactsMax: i32, + Cur: i32, + Next: i32, + BodiesN: i32, + DynamicsN: i32, + JointsN: i32, + JointDoFsN: i32, + BodyJointsMax: i32, + BodyCollidePairsN: i32, + pad: i32, + pad1: i32, + Gravity: vec4, +} + +//////// import: "shapecollide.go" +struct GeomData { + BodyIdx: i32, + Shape: Shapes, + MinSize: f32, + Thick: f32, + Radius: f32, + Size: vec3, + WbR: vec3, + WbQ: vec4, + BwR: vec3, + BwQ: vec4, +} + +//////// import: "shapegeom.go" + +//////// import: "shapes.go" +alias Shapes = i32; //enums:enum +const Plane: Shapes = 0; +const Sphere: Shapes = 1; +const Capsule: Shapes = 2; +const Cylinder: Shapes = 3; +const Box: Shapes = 4; +const Cone: Shapes = 5; + +//////// import: "slmath-matrix3.go" + +//////// import: "slmath-quaternion.go" + +//////// import: "slmath-vector2.go" + +//////// import: "slmath-vector3.go" + +//////// import: "step.go" + +//////// import: "step_body.go" + +//////// import: "step_joint.go" \ No newline at end of file diff --git a/physics/shaders/DeltasFromJoints.wgsl b/physics/shaders/DeltasFromJoints.wgsl index 4d9d47cd..31e7a5b8 100644 --- a/physics/shaders/DeltasFromJoints.wgsl +++ b/physics/shaders/DeltasFromJoints.wgsl @@ -49,35 +49,37 @@ const BodyMass: BodyVars = 8; const BodyInvMass: BodyVars = 9; const BodyBounce: BodyVars = 10; const BodyFriction: BodyVars = 11; -const BodyPosX: BodyVars = 12; -const BodyPosY: BodyVars = 13; -const BodyPosZ: BodyVars = 14; -const BodyQuatX: BodyVars = 15; -const BodyQuatY: BodyVars = 16; -const BodyQuatZ: BodyVars = 17; -const BodyQuatW: BodyVars = 18; -const BodyComX: BodyVars = 19; -const BodyComY: BodyVars = 20; -const BodyComZ: BodyVars = 21; -const BodyInertiaXX: BodyVars = 22; -const BodyInertiaYX: BodyVars = 23; -const BodyInertiaZX: BodyVars = 24; -const BodyInertiaXY: BodyVars = 25; -const BodyInertiaYY: BodyVars = 26; -const BodyInertiaZY: BodyVars = 27; -const BodyInertiaXZ: BodyVars = 28; -const BodyInertiaYZ: BodyVars = 29; -const BodyInertiaZZ: BodyVars = 30; -const BodyInvInertiaXX: BodyVars = 31; -const BodyInvInertiaYX: BodyVars = 32; -const BodyInvInertiaZX: BodyVars = 33; -const BodyInvInertiaXY: BodyVars = 34; -const BodyInvInertiaYY: BodyVars = 35; -const BodyInvInertiaZY: BodyVars = 36; -const BodyInvInertiaXZ: BodyVars = 37; -const BodyInvInertiaYZ: BodyVars = 38; -const BodyInvInertiaZZ: BodyVars = 39; -const BodyRadius: BodyVars = 40; +const BodyFrictionTortion: BodyVars = 12; +const BodyFrictionRolling: BodyVars = 13; +const BodyPosX: BodyVars = 14; +const BodyPosY: BodyVars = 15; +const BodyPosZ: BodyVars = 16; +const BodyQuatX: BodyVars = 17; +const BodyQuatY: BodyVars = 18; +const BodyQuatZ: BodyVars = 19; +const BodyQuatW: BodyVars = 20; +const BodyComX: BodyVars = 21; +const BodyComY: BodyVars = 22; +const BodyComZ: BodyVars = 23; +const BodyInertiaXX: BodyVars = 24; +const BodyInertiaYX: BodyVars = 25; +const BodyInertiaZX: BodyVars = 26; +const BodyInertiaXY: BodyVars = 27; +const BodyInertiaYY: BodyVars = 28; +const BodyInertiaZY: BodyVars = 29; +const BodyInertiaXZ: BodyVars = 30; +const BodyInertiaYZ: BodyVars = 31; +const BodyInertiaZZ: BodyVars = 32; +const BodyInvInertiaXX: BodyVars = 33; +const BodyInvInertiaYX: BodyVars = 34; +const BodyInvInertiaZX: BodyVars = 35; +const BodyInvInertiaXY: BodyVars = 36; +const BodyInvInertiaYY: BodyVars = 37; +const BodyInvInertiaZY: BodyVars = 38; +const BodyInvInertiaXZ: BodyVars = 39; +const BodyInvInertiaYZ: BodyVars = 40; +const BodyInvInertiaZZ: BodyVars = 41; +const BodyRadius: BodyVars = 42; //////// import: "contact.go" alias ContactVars = i32; //enums:enum @@ -101,9 +103,18 @@ const ContactBThick: ContactVars = 16; const ContactNormX: ContactVars = 17; const ContactNormY: ContactVars = 18; const ContactNormZ: ContactVars = 19; -const ContactForceX: ContactVars = 20; -const ContactForceY: ContactVars = 21; -const ContactForceZ: ContactVars = 22; +const ContactADeltaX: ContactVars = 20; +const ContactADeltaY: ContactVars = 21; +const ContactADeltaZ: ContactVars = 22; +const ContactAAngDeltaX: ContactVars = 23; +const ContactAAngDeltaY: ContactVars = 24; +const ContactAAngDeltaZ: ContactVars = 25; +const ContactBDeltaX: ContactVars = 26; +const ContactBDeltaY: ContactVars = 27; +const ContactBDeltaZ: ContactVars = 28; +const ContactBAngDeltaX: ContactVars = 29; +const ContactBAngDeltaY: ContactVars = 30; +const ContactBAngDeltaZ: ContactVars = 31; const BroadContactVarsN = ContactAPointX; //////// import: "control.go" @@ -158,8 +169,8 @@ fn SetDynamicAngDelta(idx: i32,cni: i32, angDelta: vec3) { } //////// import: "enumgen.go" -const BodyVarsN: BodyVars = 41; -const ContactVarsN: ContactVars = 23; +const BodyVarsN: BodyVars = 43; +const ContactVarsN: ContactVars = 32; const JointControlVarsN: JointControlVars = 3; const DynamicVarsN: DynamicVars = 32; const GPUVarsN: GPUVars = 12; diff --git a/physics/shaders/DynamicsCurToNext.wgsl b/physics/shaders/DynamicsCurToNext.wgsl index 96657ffb..3f828a06 100644 --- a/physics/shaders/DynamicsCurToNext.wgsl +++ b/physics/shaders/DynamicsCurToNext.wgsl @@ -41,35 +41,37 @@ const BodyMass: BodyVars = 8; const BodyInvMass: BodyVars = 9; const BodyBounce: BodyVars = 10; const BodyFriction: BodyVars = 11; -const BodyPosX: BodyVars = 12; -const BodyPosY: BodyVars = 13; -const BodyPosZ: BodyVars = 14; -const BodyQuatX: BodyVars = 15; -const BodyQuatY: BodyVars = 16; -const BodyQuatZ: BodyVars = 17; -const BodyQuatW: BodyVars = 18; -const BodyComX: BodyVars = 19; -const BodyComY: BodyVars = 20; -const BodyComZ: BodyVars = 21; -const BodyInertiaXX: BodyVars = 22; -const BodyInertiaYX: BodyVars = 23; -const BodyInertiaZX: BodyVars = 24; -const BodyInertiaXY: BodyVars = 25; -const BodyInertiaYY: BodyVars = 26; -const BodyInertiaZY: BodyVars = 27; -const BodyInertiaXZ: BodyVars = 28; -const BodyInertiaYZ: BodyVars = 29; -const BodyInertiaZZ: BodyVars = 30; -const BodyInvInertiaXX: BodyVars = 31; -const BodyInvInertiaYX: BodyVars = 32; -const BodyInvInertiaZX: BodyVars = 33; -const BodyInvInertiaXY: BodyVars = 34; -const BodyInvInertiaYY: BodyVars = 35; -const BodyInvInertiaZY: BodyVars = 36; -const BodyInvInertiaXZ: BodyVars = 37; -const BodyInvInertiaYZ: BodyVars = 38; -const BodyInvInertiaZZ: BodyVars = 39; -const BodyRadius: BodyVars = 40; +const BodyFrictionTortion: BodyVars = 12; +const BodyFrictionRolling: BodyVars = 13; +const BodyPosX: BodyVars = 14; +const BodyPosY: BodyVars = 15; +const BodyPosZ: BodyVars = 16; +const BodyQuatX: BodyVars = 17; +const BodyQuatY: BodyVars = 18; +const BodyQuatZ: BodyVars = 19; +const BodyQuatW: BodyVars = 20; +const BodyComX: BodyVars = 21; +const BodyComY: BodyVars = 22; +const BodyComZ: BodyVars = 23; +const BodyInertiaXX: BodyVars = 24; +const BodyInertiaYX: BodyVars = 25; +const BodyInertiaZX: BodyVars = 26; +const BodyInertiaXY: BodyVars = 27; +const BodyInertiaYY: BodyVars = 28; +const BodyInertiaZY: BodyVars = 29; +const BodyInertiaXZ: BodyVars = 30; +const BodyInertiaYZ: BodyVars = 31; +const BodyInertiaZZ: BodyVars = 32; +const BodyInvInertiaXX: BodyVars = 33; +const BodyInvInertiaYX: BodyVars = 34; +const BodyInvInertiaZX: BodyVars = 35; +const BodyInvInertiaXY: BodyVars = 36; +const BodyInvInertiaYY: BodyVars = 37; +const BodyInvInertiaZY: BodyVars = 38; +const BodyInvInertiaXZ: BodyVars = 39; +const BodyInvInertiaYZ: BodyVars = 40; +const BodyInvInertiaZZ: BodyVars = 41; +const BodyRadius: BodyVars = 42; //////// import: "contact.go" alias ContactVars = i32; //enums:enum @@ -93,9 +95,18 @@ const ContactBThick: ContactVars = 16; const ContactNormX: ContactVars = 17; const ContactNormY: ContactVars = 18; const ContactNormZ: ContactVars = 19; -const ContactForceX: ContactVars = 20; -const ContactForceY: ContactVars = 21; -const ContactForceZ: ContactVars = 22; +const ContactADeltaX: ContactVars = 20; +const ContactADeltaY: ContactVars = 21; +const ContactADeltaZ: ContactVars = 22; +const ContactAAngDeltaX: ContactVars = 23; +const ContactAAngDeltaY: ContactVars = 24; +const ContactAAngDeltaZ: ContactVars = 25; +const ContactBDeltaX: ContactVars = 26; +const ContactBDeltaY: ContactVars = 27; +const ContactBDeltaZ: ContactVars = 28; +const ContactBAngDeltaX: ContactVars = 29; +const ContactBAngDeltaY: ContactVars = 30; +const ContactBAngDeltaZ: ContactVars = 31; const BroadContactVarsN = ContactAPointX; //////// import: "control.go" @@ -140,8 +151,8 @@ const DynAngDeltaY: DynamicVars = 30; const DynAngDeltaZ: DynamicVars = 31; //////// import: "enumgen.go" -const BodyVarsN: BodyVars = 41; -const ContactVarsN: ContactVars = 23; +const BodyVarsN: BodyVars = 43; +const ContactVarsN: ContactVars = 32; const JointControlVarsN: JointControlVars = 3; const DynamicVarsN: DynamicVars = 32; const GPUVarsN: GPUVars = 12; diff --git a/physics/shaders/ForcesFromJoints.wgsl b/physics/shaders/ForcesFromJoints.wgsl index 7feb8aca..948bd12a 100644 --- a/physics/shaders/ForcesFromJoints.wgsl +++ b/physics/shaders/ForcesFromJoints.wgsl @@ -49,35 +49,37 @@ const BodyMass: BodyVars = 8; const BodyInvMass: BodyVars = 9; const BodyBounce: BodyVars = 10; const BodyFriction: BodyVars = 11; -const BodyPosX: BodyVars = 12; -const BodyPosY: BodyVars = 13; -const BodyPosZ: BodyVars = 14; -const BodyQuatX: BodyVars = 15; -const BodyQuatY: BodyVars = 16; -const BodyQuatZ: BodyVars = 17; -const BodyQuatW: BodyVars = 18; -const BodyComX: BodyVars = 19; -const BodyComY: BodyVars = 20; -const BodyComZ: BodyVars = 21; -const BodyInertiaXX: BodyVars = 22; -const BodyInertiaYX: BodyVars = 23; -const BodyInertiaZX: BodyVars = 24; -const BodyInertiaXY: BodyVars = 25; -const BodyInertiaYY: BodyVars = 26; -const BodyInertiaZY: BodyVars = 27; -const BodyInertiaXZ: BodyVars = 28; -const BodyInertiaYZ: BodyVars = 29; -const BodyInertiaZZ: BodyVars = 30; -const BodyInvInertiaXX: BodyVars = 31; -const BodyInvInertiaYX: BodyVars = 32; -const BodyInvInertiaZX: BodyVars = 33; -const BodyInvInertiaXY: BodyVars = 34; -const BodyInvInertiaYY: BodyVars = 35; -const BodyInvInertiaZY: BodyVars = 36; -const BodyInvInertiaXZ: BodyVars = 37; -const BodyInvInertiaYZ: BodyVars = 38; -const BodyInvInertiaZZ: BodyVars = 39; -const BodyRadius: BodyVars = 40; +const BodyFrictionTortion: BodyVars = 12; +const BodyFrictionRolling: BodyVars = 13; +const BodyPosX: BodyVars = 14; +const BodyPosY: BodyVars = 15; +const BodyPosZ: BodyVars = 16; +const BodyQuatX: BodyVars = 17; +const BodyQuatY: BodyVars = 18; +const BodyQuatZ: BodyVars = 19; +const BodyQuatW: BodyVars = 20; +const BodyComX: BodyVars = 21; +const BodyComY: BodyVars = 22; +const BodyComZ: BodyVars = 23; +const BodyInertiaXX: BodyVars = 24; +const BodyInertiaYX: BodyVars = 25; +const BodyInertiaZX: BodyVars = 26; +const BodyInertiaXY: BodyVars = 27; +const BodyInertiaYY: BodyVars = 28; +const BodyInertiaZY: BodyVars = 29; +const BodyInertiaXZ: BodyVars = 30; +const BodyInertiaYZ: BodyVars = 31; +const BodyInertiaZZ: BodyVars = 32; +const BodyInvInertiaXX: BodyVars = 33; +const BodyInvInertiaYX: BodyVars = 34; +const BodyInvInertiaZX: BodyVars = 35; +const BodyInvInertiaXY: BodyVars = 36; +const BodyInvInertiaYY: BodyVars = 37; +const BodyInvInertiaZY: BodyVars = 38; +const BodyInvInertiaXZ: BodyVars = 39; +const BodyInvInertiaYZ: BodyVars = 40; +const BodyInvInertiaZZ: BodyVars = 41; +const BodyRadius: BodyVars = 42; //////// import: "contact.go" alias ContactVars = i32; //enums:enum @@ -101,9 +103,18 @@ const ContactBThick: ContactVars = 16; const ContactNormX: ContactVars = 17; const ContactNormY: ContactVars = 18; const ContactNormZ: ContactVars = 19; -const ContactForceX: ContactVars = 20; -const ContactForceY: ContactVars = 21; -const ContactForceZ: ContactVars = 22; +const ContactADeltaX: ContactVars = 20; +const ContactADeltaY: ContactVars = 21; +const ContactADeltaZ: ContactVars = 22; +const ContactAAngDeltaX: ContactVars = 23; +const ContactAAngDeltaY: ContactVars = 24; +const ContactAAngDeltaZ: ContactVars = 25; +const ContactBDeltaX: ContactVars = 26; +const ContactBDeltaY: ContactVars = 27; +const ContactBDeltaZ: ContactVars = 28; +const ContactBAngDeltaX: ContactVars = 29; +const ContactBAngDeltaY: ContactVars = 30; +const ContactBAngDeltaZ: ContactVars = 31; const BroadContactVarsN = ContactAPointX; //////// import: "control.go" @@ -158,8 +169,8 @@ fn SetDynamicTorque(idx: i32,cni: i32, torque: vec3) { } //////// import: "enumgen.go" -const BodyVarsN: BodyVars = 41; -const ContactVarsN: ContactVars = 23; +const BodyVarsN: BodyVars = 43; +const ContactVarsN: ContactVars = 32; const JointControlVarsN: JointControlVars = 3; const DynamicVarsN: DynamicVars = 32; const GPUVarsN: GPUVars = 12; diff --git a/physics/shaders/InitDynamics.wgsl b/physics/shaders/InitDynamics.wgsl index d8636ea7..d349e3f6 100644 --- a/physics/shaders/InitDynamics.wgsl +++ b/physics/shaders/InitDynamics.wgsl @@ -47,35 +47,37 @@ const BodyMass: BodyVars = 8; const BodyInvMass: BodyVars = 9; const BodyBounce: BodyVars = 10; const BodyFriction: BodyVars = 11; -const BodyPosX: BodyVars = 12; -const BodyPosY: BodyVars = 13; -const BodyPosZ: BodyVars = 14; -const BodyQuatX: BodyVars = 15; -const BodyQuatY: BodyVars = 16; -const BodyQuatZ: BodyVars = 17; -const BodyQuatW: BodyVars = 18; -const BodyComX: BodyVars = 19; -const BodyComY: BodyVars = 20; -const BodyComZ: BodyVars = 21; -const BodyInertiaXX: BodyVars = 22; -const BodyInertiaYX: BodyVars = 23; -const BodyInertiaZX: BodyVars = 24; -const BodyInertiaXY: BodyVars = 25; -const BodyInertiaYY: BodyVars = 26; -const BodyInertiaZY: BodyVars = 27; -const BodyInertiaXZ: BodyVars = 28; -const BodyInertiaYZ: BodyVars = 29; -const BodyInertiaZZ: BodyVars = 30; -const BodyInvInertiaXX: BodyVars = 31; -const BodyInvInertiaYX: BodyVars = 32; -const BodyInvInertiaZX: BodyVars = 33; -const BodyInvInertiaXY: BodyVars = 34; -const BodyInvInertiaYY: BodyVars = 35; -const BodyInvInertiaZY: BodyVars = 36; -const BodyInvInertiaXZ: BodyVars = 37; -const BodyInvInertiaYZ: BodyVars = 38; -const BodyInvInertiaZZ: BodyVars = 39; -const BodyRadius: BodyVars = 40; +const BodyFrictionTortion: BodyVars = 12; +const BodyFrictionRolling: BodyVars = 13; +const BodyPosX: BodyVars = 14; +const BodyPosY: BodyVars = 15; +const BodyPosZ: BodyVars = 16; +const BodyQuatX: BodyVars = 17; +const BodyQuatY: BodyVars = 18; +const BodyQuatZ: BodyVars = 19; +const BodyQuatW: BodyVars = 20; +const BodyComX: BodyVars = 21; +const BodyComY: BodyVars = 22; +const BodyComZ: BodyVars = 23; +const BodyInertiaXX: BodyVars = 24; +const BodyInertiaYX: BodyVars = 25; +const BodyInertiaZX: BodyVars = 26; +const BodyInertiaXY: BodyVars = 27; +const BodyInertiaYY: BodyVars = 28; +const BodyInertiaZY: BodyVars = 29; +const BodyInertiaXZ: BodyVars = 30; +const BodyInertiaYZ: BodyVars = 31; +const BodyInertiaZZ: BodyVars = 32; +const BodyInvInertiaXX: BodyVars = 33; +const BodyInvInertiaYX: BodyVars = 34; +const BodyInvInertiaZX: BodyVars = 35; +const BodyInvInertiaXY: BodyVars = 36; +const BodyInvInertiaYY: BodyVars = 37; +const BodyInvInertiaZY: BodyVars = 38; +const BodyInvInertiaXZ: BodyVars = 39; +const BodyInvInertiaYZ: BodyVars = 40; +const BodyInvInertiaZZ: BodyVars = 41; +const BodyRadius: BodyVars = 42; //////// import: "contact.go" alias ContactVars = i32; //enums:enum @@ -99,9 +101,18 @@ const ContactBThick: ContactVars = 16; const ContactNormX: ContactVars = 17; const ContactNormY: ContactVars = 18; const ContactNormZ: ContactVars = 19; -const ContactForceX: ContactVars = 20; -const ContactForceY: ContactVars = 21; -const ContactForceZ: ContactVars = 22; +const ContactADeltaX: ContactVars = 20; +const ContactADeltaY: ContactVars = 21; +const ContactADeltaZ: ContactVars = 22; +const ContactAAngDeltaX: ContactVars = 23; +const ContactAAngDeltaY: ContactVars = 24; +const ContactAAngDeltaZ: ContactVars = 25; +const ContactBDeltaX: ContactVars = 26; +const ContactBDeltaY: ContactVars = 27; +const ContactBDeltaZ: ContactVars = 28; +const ContactBAngDeltaX: ContactVars = 29; +const ContactBAngDeltaY: ContactVars = 30; +const ContactBAngDeltaZ: ContactVars = 31; const BroadContactVarsN = ContactAPointX; //////// import: "control.go" @@ -149,8 +160,8 @@ fn DynamicBody(idx: i32) -> i32 { } //////// import: "enumgen.go" -const BodyVarsN: BodyVars = 41; -const ContactVarsN: ContactVars = 23; +const BodyVarsN: BodyVars = 43; +const ContactVarsN: ContactVars = 32; const JointControlVarsN: JointControlVars = 3; const DynamicVarsN: DynamicVars = 32; const GPUVarsN: GPUVars = 12; diff --git a/physics/shaders/StepBodyContacts.wgsl b/physics/shaders/StepBodyContacts.wgsl new file mode 100644 index 00000000..6fa08b2e --- /dev/null +++ b/physics/shaders/StepBodyContacts.wgsl @@ -0,0 +1,457 @@ +// Code generated by "gosl"; DO NOT EDIT +// kernel: StepBodyContacts + +// // Params are global parameters. +@group(0) @binding(0) +var TensorStrides: array; +@group(0) @binding(1) +var Params: array; +// // Bodies are the rigid body elements (dynamic and static), // specifying the constant, non-dynamic properties, // which is initial state for dynamics. // [body][BodyVarsN] +@group(1) @binding(0) +var Bodies: array; +// // Dynamics are the dynamic rigid body elements: these actually move. // Two alternating states are used: Params.Cur and Params.Next. // [dyn body][cur/next][DynamicVarsN] +@group(2) @binding(0) +var Dynamics: array; +@group(2) @binding(3) +var ContactsN: array; +@group(2) @binding(4) +var Contacts: array; +// // JointControls are dynamic joint control inputs, per joint DoF // (in correspondence with [JointDoFs]). This can be uploaded to the // GPU at every step. // [dof][JointControlVarsN] + +alias GPUVars = i32; + +@compute @workgroup_size(64, 1, 1) +fn main(@builtin(workgroup_id) wgid: vec3, @builtin(num_workgroups) nwg: vec3, @builtin(local_invocation_index) loci: u32) { + let idx = loci + (wgid.x + wgid.y * nwg.x + wgid.z * nwg.x * nwg.y) * 64; + StepBodyContacts(idx); +} + +fn Index2D(s0: u32, s1: u32, i0: u32, i1: u32) -> u32 { + return s0 * i0 + s1 * i1; +} + +fn Index3D(s0: u32, s1: u32, s2: u32, i0: u32, i1: u32, i2: u32) -> u32 { + return s0 * i0 + s1 * i1 + s2 * i2; +} + +fn Index1D(s0: u32, i0: u32) -> u32 { + return s0 * i0; +} + + +//////// import: "vars.go" + +//////// import: "body.go" +alias BodyVars = i32; //enums:enum +const BodyShape: BodyVars = 0; +const BodyDynamic: BodyVars = 1; +const BodyWorld: BodyVars = 2; +const BodyGroup: BodyVars = 3; +const BodyHSizeX: BodyVars = 4; +const BodyHSizeY: BodyVars = 5; +const BodyHSizeZ: BodyVars = 6; +const BodyThick: BodyVars = 7; +const BodyMass: BodyVars = 8; +const BodyInvMass: BodyVars = 9; +const BodyBounce: BodyVars = 10; +const BodyFriction: BodyVars = 11; +const BodyFrictionTortion: BodyVars = 12; +const BodyFrictionRolling: BodyVars = 13; +const BodyPosX: BodyVars = 14; +const BodyPosY: BodyVars = 15; +const BodyPosZ: BodyVars = 16; +const BodyQuatX: BodyVars = 17; +const BodyQuatY: BodyVars = 18; +const BodyQuatZ: BodyVars = 19; +const BodyQuatW: BodyVars = 20; +const BodyComX: BodyVars = 21; +const BodyComY: BodyVars = 22; +const BodyComZ: BodyVars = 23; +const BodyInertiaXX: BodyVars = 24; +const BodyInertiaYX: BodyVars = 25; +const BodyInertiaZX: BodyVars = 26; +const BodyInertiaXY: BodyVars = 27; +const BodyInertiaYY: BodyVars = 28; +const BodyInertiaZY: BodyVars = 29; +const BodyInertiaXZ: BodyVars = 30; +const BodyInertiaYZ: BodyVars = 31; +const BodyInertiaZZ: BodyVars = 32; +const BodyInvInertiaXX: BodyVars = 33; +const BodyInvInertiaYX: BodyVars = 34; +const BodyInvInertiaZX: BodyVars = 35; +const BodyInvInertiaXY: BodyVars = 36; +const BodyInvInertiaYY: BodyVars = 37; +const BodyInvInertiaZY: BodyVars = 38; +const BodyInvInertiaXZ: BodyVars = 39; +const BodyInvInertiaYZ: BodyVars = 40; +const BodyInvInertiaZZ: BodyVars = 41; +const BodyRadius: BodyVars = 42; +fn GetBodyDynamic(idx: i32) -> i32 { + return i32(bitcast(Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyDynamic))])); +} +fn BodyPos(idx: i32) -> vec3 { + return vec3(Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyPosX))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyPosY))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyPosZ))]); +} +fn BodyQuat(idx: i32) -> vec4 { + return vec4(Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyQuatX))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyQuatY))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyQuatZ))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyQuatW))]); +} +fn BodyDynamicPos(idx: i32,cni: i32) -> vec3 { + var didx = GetBodyDynamic(idx); + if (didx < 0) { + return BodyPos(idx); + }return DynamicPos(didx, cni); +} +fn BodyDynamicQuat(idx: i32,cni: i32) -> vec4 { + var didx = GetBodyDynamic(idx); + if (didx < 0) { + return BodyQuat(idx); + }return DynamicQuat(didx, cni); +} +fn BodyCom(idx: i32) -> vec3 { + return vec3(Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComX))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComY))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComZ))]); +} +fn BodyInvInertia(idx: i32) -> mat3x3f { + return mat3x3f(Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInvInertiaXX))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInvInertiaYX))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInvInertiaZX))], + Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInvInertiaXY))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInvInertiaYY))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInvInertiaZY))], + Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInvInertiaXZ))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInvInertiaYZ))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInvInertiaZZ))]); +} + +//////// import: "contact.go" +alias ContactVars = i32; //enums:enum +const ContactA: ContactVars = 0; +const ContactB: ContactVars = 1; +const ContactPointIdx: ContactVars = 2; +const ContactAPointX: ContactVars = 3; +const ContactAPointY: ContactVars = 4; +const ContactAPointZ: ContactVars = 5; +const ContactBPointX: ContactVars = 6; +const ContactBPointY: ContactVars = 7; +const ContactBPointZ: ContactVars = 8; +const ContactAOffX: ContactVars = 9; +const ContactAOffY: ContactVars = 10; +const ContactAOffZ: ContactVars = 11; +const ContactBOffX: ContactVars = 12; +const ContactBOffY: ContactVars = 13; +const ContactBOffZ: ContactVars = 14; +const ContactAThick: ContactVars = 15; +const ContactBThick: ContactVars = 16; +const ContactNormX: ContactVars = 17; +const ContactNormY: ContactVars = 18; +const ContactNormZ: ContactVars = 19; +const ContactADeltaX: ContactVars = 20; +const ContactADeltaY: ContactVars = 21; +const ContactADeltaZ: ContactVars = 22; +const ContactAAngDeltaX: ContactVars = 23; +const ContactAAngDeltaY: ContactVars = 24; +const ContactAAngDeltaZ: ContactVars = 25; +const ContactBDeltaX: ContactVars = 26; +const ContactBDeltaY: ContactVars = 27; +const ContactBDeltaZ: ContactVars = 28; +const ContactBAngDeltaX: ContactVars = 29; +const ContactBAngDeltaY: ContactVars = 30; +const ContactBAngDeltaZ: ContactVars = 31; +const BroadContactVarsN = ContactAPointX; +fn GetContactA(idx: i32) -> i32 { return i32(bitcast(Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactA))])); } +fn GetContactB(idx: i32) -> i32 { return i32(bitcast(Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactB))])); } +fn ContactAPoint(idx: i32) -> vec3 { return vec3(Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactAPointX))], Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactAPointY))], Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactAPointZ))]); } +fn ContactBPoint(idx: i32) -> vec3 { return vec3(Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactBPointX))], Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactBPointY))], Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactBPointZ))]); } +fn ContactAOff(idx: i32) -> vec3 { return vec3(Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactAOffX))], Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactAOffY))], Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactAOffZ))]); } +fn ContactBOff(idx: i32) -> vec3 { return vec3(Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactBOffX))], Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactBOffY))], Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactBOffZ))]); } +fn ContactNorm(idx: i32) -> vec3 { return vec3(Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactNormX))], Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactNormY))], Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactNormZ))]); } +fn SetContactADelta(idx: i32, pos: vec3) { Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactADeltaX))] = pos.x;; Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactADeltaY))] = pos.y;; Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactADeltaZ))] = pos.z; } +fn SetContactAAngDelta(idx: i32, pos: vec3) { Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactAAngDeltaX))] = pos.x;; Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactAAngDeltaY))] = pos.y;; Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactAAngDeltaZ))] = pos.z; } +fn SetContactBDelta(idx: i32, pos: vec3) { Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactBDeltaX))] = pos.x;; Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactBDeltaY))] = pos.y;; Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactBDeltaZ))] = pos.z; } +fn SetContactBAngDelta(idx: i32, pos: vec3) { Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactBAngDeltaX))] = pos.x;; Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactBAngDeltaY))] = pos.y;; Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactBAngDeltaZ))] = pos.z; } +fn StepBodyContacts(i: u32) { //gosl:kernel +let params = Params[0];; var ci = i32(i);; var cmax = ContactsN[0];; if (ci >= cmax) { + return; +}; var biA = GetContactA(ci);; var biB = GetContactB(ci);; var diA = GetBodyDynamic(biA);; var diB = GetBodyDynamic(biB);; var r0A = BodyDynamicPos(biA, params.Next);; var q0A = BodyDynamicQuat(biA, params.Next);; var r0B = BodyDynamicPos(biB, params.Next);; var q0B = BodyDynamicQuat(biB, params.Next);; var ctA = ContactAPoint(ci);; var offA = ContactAOff(ci);; var ctB = ContactBPoint(ci);; var offB = ContactBOff(ci);; var ctAw = MulSpatialPoint(r0A, q0A, ctA);; var ctBw = MulSpatialPoint(r0B, q0B, ctB);; var thickA = Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(ci), u32(ContactAThick))];; var thickB = Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(ci), u32(ContactBThick))];; var thick = thickA + thickB;; var norm = ContactNorm(ci);; var nnorm = Negate3(norm);; var d = Dot3(norm, ctBw-(ctAw)) - thick;; if (d >= 0.0) { // now separated + return; +}; var comA = BodyCom(biA);; var mInvA = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(biA), u32(BodyInvMass))];; var iInvA = BodyInvInertia(biA);; var comB = BodyCom(biB);; var mInvB = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(biB), u32(BodyInvMass))];; var iInvB = BodyInvInertia(biB);; var w0A: vec3; +var w0B: vec3;; if (diA >= 0) { + w0A = DynamicAngDelta(diA, params.Next); +}; if (diB >= 0) { + w0B = DynamicAngDelta(diB, params.Next); +}; +var mu = 0.5 * (Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(biA), u32(BodyFriction))] + Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(biB), u32(BodyFriction))]);; var frTors = 0.5 * (Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(biA), u32(BodyFrictionTortion))] + Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(biB), u32(BodyFrictionTortion))]);; var frRoll = 0.5 * (Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(biA), u32(BodyFrictionRolling))] + Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(biB), u32(BodyFrictionRolling))]);; +var dA = ctAw-(MulSpatialPoint(r0A, q0A, comA));; var dB = ctBw-(MulSpatialPoint(r0B, q0B, comB));; var angA = Negate3(Cross3(dA, norm));; var angB = Cross3(dB, norm);; +var lambdaN = ContactConstraint(d, q0A, q0B, mInvA, mInvB, iInvA, iInvB, nnorm, norm, angA, angB, params.ContactRelax, params.Dt);; var linDeltaA = Negate3(norm)*(lambdaN);; var linDeltaB = norm*(lambdaN);; var angDeltaA = angA*(lambdaN);; var angDeltaB = angB*(lambdaN);; +if (mu > 0.0) { + ctAw = ctAw+(MulQuatVector(q0A, offA)); + ctBw = ctBw+(MulQuatVector(q0B, offB)); + var delta = ctBw-(ctAw); + var frDelta = delta-(norm*(Dot3(norm, delta))); + var perp = Normal3(frDelta); + angA = Negate3(Cross3(dA, perp)); + angB = Cross3(dB, perp); + var err = Length3(frDelta); + if (err > 0.0) { + var lambdaFr = ContactConstraint(err, q0A, q0B, mInvA, mInvB, iInvA, iInvB, Negate3(perp), perp, angA, angB, params.ContactRelax, params.Dt); + lambdaFr = max(lambdaFr, -lambdaN*mu); + linDeltaA = linDeltaA-(perp*(lambdaFr)); + linDeltaB = linDeltaB+(perp*(lambdaFr)); + angDeltaA = angDeltaA+(angA*(lambdaFr)); + angDeltaB = angDeltaB+(angB*(lambdaFr)); + } +}; var deltaW = w0B-(w0A);; if (frTors > 0.0) { + var err = Dot3(deltaW, norm) * params.Dt; + if (abs(err) > 0.0) { + var lin = vec3(0, 0, 0); + var lambdaTors = ContactConstraint(err, q0A, q0B, mInvA, mInvB, iInvA, iInvB, lin, lin, nnorm, norm, params.ContactRelax, params.Dt); + lambdaTors = clamp(lambdaTors, -lambdaN*frTors, lambdaN*frTors); + angDeltaA = angDeltaA-(norm*(lambdaTors)); + angDeltaB = angDeltaB+(norm*(lambdaTors)); + } +}; if (frRoll > 0.0) { + deltaW = deltaW-(norm*(Dot3(norm, deltaW))); + var err = Length3(deltaW) * params.Dt; + if (err > 0.0) { + var lin = vec3(0, 0, 0); + var rollN = Normal3(deltaW); + var lambdaRoll = ContactConstraint(err, q0A, q0B, mInvA, mInvB, iInvA, iInvB, lin, lin, Negate3(rollN), rollN, params.ContactRelax, params.Dt); + lambdaRoll = max(lambdaRoll, -lambdaN*frRoll); + angDeltaA = angDeltaA-(rollN*(lambdaRoll)); + angDeltaB = angDeltaB+(rollN*(lambdaRoll)); + } +}; SetContactADelta(ci, linDeltaA);; SetContactBDelta(ci, linDeltaB);; SetContactAAngDelta(ci, angDeltaA);; SetContactBAngDelta(ci, angDeltaB); } +fn ContactConstraint(err: f32, q0A: vec4,q0B: vec4, mInvA: f32,mInvB: f32, iInvA: mat3x3f,iInvB: mat3x3f, linA: vec3,linB: vec3,angA: vec3,angB: vec3, relaxation: f32,dt: f32) -> f32 { + var denom = f32(0.0); + denom += LengthSquared3(linA) * mInvA; + denom += LengthSquared3(linB) * mInvB; + var rotAngA = MulQuatVectorInverse(q0A, angA); + var rotAngB = MulQuatVectorInverse(q0B, angB); + denom += Dot3(rotAngA, iInvA*(rotAngA)); + denom += Dot3(rotAngB, iInvB*(rotAngB)); + var lambda = -err; + if (denom > 0.0) { + lambda /= dt * denom; + }return lambda * relaxation; +} + +//////// import: "control.go" +alias JointControlVars = i32; //enums:enum +const JointControlForce: JointControlVars = 0; +const JointTargetPos: JointControlVars = 1; +const JointTargetVel: JointControlVars = 2; + +//////// import: "dynamics.go" +alias DynamicVars = i32; //enums:enum +const DynBody: DynamicVars = 0; +const DynPosX: DynamicVars = 1; +const DynPosY: DynamicVars = 2; +const DynPosZ: DynamicVars = 3; +const DynQuatX: DynamicVars = 4; +const DynQuatY: DynamicVars = 5; +const DynQuatZ: DynamicVars = 6; +const DynQuatW: DynamicVars = 7; +const DynVelX: DynamicVars = 8; +const DynVelY: DynamicVars = 9; +const DynVelZ: DynamicVars = 10; +const DynAngVelX: DynamicVars = 11; +const DynAngVelY: DynamicVars = 12; +const DynAngVelZ: DynamicVars = 13; +const DynAccX: DynamicVars = 14; +const DynAccY: DynamicVars = 15; +const DynAccZ: DynamicVars = 16; +const DynAngAccX: DynamicVars = 17; +const DynAngAccY: DynamicVars = 18; +const DynAngAccZ: DynamicVars = 19; +const DynForceX: DynamicVars = 20; +const DynForceY: DynamicVars = 21; +const DynForceZ: DynamicVars = 22; +const DynTorqueX: DynamicVars = 23; +const DynTorqueY: DynamicVars = 24; +const DynTorqueZ: DynamicVars = 25; +const DynDeltaX: DynamicVars = 26; +const DynDeltaY: DynamicVars = 27; +const DynDeltaZ: DynamicVars = 28; +const DynAngDeltaX: DynamicVars = 29; +const DynAngDeltaY: DynamicVars = 30; +const DynAngDeltaZ: DynamicVars = 31; +fn DynamicPos(idx: i32,cni: i32) -> vec3 { + return vec3(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosZ))]); +} +fn DynamicQuat(idx: i32,cni: i32) -> vec4 { + return vec4(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatZ))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatW))]); +} +fn DynamicAngDelta(idx: i32,cni: i32) -> vec3 { + return vec3(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaZ))]); +} + +//////// import: "enumgen.go" +const BodyVarsN: BodyVars = 43; +const ContactVarsN: ContactVars = 32; +const JointControlVarsN: JointControlVars = 3; +const DynamicVarsN: DynamicVars = 32; +const GPUVarsN: GPUVars = 12; +const JointTypesN: JointTypes = 7; +const JointVarsN: JointVars = 50; +const JointDoFVarsN: JointDoFVars = 7; +const ShapesN: Shapes = 6; + +//////// import: "joint.go" +const JointLimitUnlimited = 1e10; +alias JointTypes = i32; //enums:enum +const Prismatic: JointTypes = 0; +const Revolute: JointTypes = 1; +const Ball: JointTypes = 2; +const Fixed: JointTypes = 3; +const Free: JointTypes = 4; +const Distance: JointTypes = 5; +const D6: JointTypes = 6; +alias JointVars = i32; //enums:enum +const JointType: JointVars = 0; +const JointEnabled: JointVars = 1; +const JointParent: JointVars = 2; +const JointChild: JointVars = 3; +const JointPPosX: JointVars = 4; +const JointPPosY: JointVars = 5; +const JointPPosZ: JointVars = 6; +const JointPQuatX: JointVars = 7; +const JointPQuatY: JointVars = 8; +const JointPQuatZ: JointVars = 9; +const JointPQuatW: JointVars = 10; +const JointCPosX: JointVars = 11; +const JointCPosY: JointVars = 12; +const JointCPosZ: JointVars = 13; +const JointCQuatX: JointVars = 14; +const JointCQuatY: JointVars = 15; +const JointCQuatZ: JointVars = 16; +const JointCQuatW: JointVars = 17; +const JointLinearDoFN: JointVars = 18; +const JointAngularDoFN: JointVars = 19; +const JointDoF1: JointVars = 20; +const JointDoF2: JointVars = 21; +const JointDoF3: JointVars = 22; +const JointDoF4: JointVars = 23; +const JointDoF5: JointVars = 24; +const JointDoF6: JointVars = 25; +const JointPForceX: JointVars = 26; +const JointPForceY: JointVars = 27; +const JointPForceZ: JointVars = 28; +const JointPTorqueX: JointVars = 29; +const JointPTorqueY: JointVars = 30; +const JointPTorqueZ: JointVars = 31; +const JointCForceX: JointVars = 32; +const JointCForceY: JointVars = 33; +const JointCForceZ: JointVars = 34; +const JointCTorqueX: JointVars = 35; +const JointCTorqueY: JointVars = 36; +const JointCTorqueZ: JointVars = 37; +const JointPDeltaX: JointVars = 38; +const JointPDeltaY: JointVars = 39; +const JointPDeltaZ: JointVars = 40; +const JointPAngDeltaX: JointVars = 41; +const JointPAngDeltaY: JointVars = 42; +const JointPAngDeltaZ: JointVars = 43; +const JointCDeltaX: JointVars = 44; +const JointCDeltaY: JointVars = 45; +const JointCDeltaZ: JointVars = 46; +const JointCAngDeltaX: JointVars = 47; +const JointCAngDeltaY: JointVars = 48; +const JointCAngDeltaZ: JointVars = 49; +alias JointDoFVars = i32; //enums:enum +const JointAxisX: JointDoFVars = 0; +const JointAxisY: JointDoFVars = 1; +const JointAxisZ: JointDoFVars = 2; +const JointLimitLower: JointDoFVars = 3; +const JointLimitUpper: JointDoFVars = 4; +const JointStiff: JointDoFVars = 5; +const JointDamp: JointDoFVars = 6; + +//////// import: "params.go" +struct PhysParams { + Iterations: i32, + Dt: f32, + SoftRelax: f32, + JointLinearRelax: f32, + JointAngularRelax: f32, + JointLinearComply: f32, + JointAngularComply: f32, + ContactRelax: f32, + AngularDamping: f32, + ContactWeighting: i32, + Restitution: i32, + MaxGeomIter: i32, + ContactMargin: f32, + ContactsMax: i32, + Cur: i32, + Next: i32, + BodiesN: i32, + DynamicsN: i32, + JointsN: i32, + JointDoFsN: i32, + BodyJointsMax: i32, + BodyCollidePairsN: i32, + pad: i32, + pad1: i32, + Gravity: vec4, +} + +//////// import: "shapecollide.go" +struct GeomData { + BodyIdx: i32, + Shape: Shapes, + MinSize: f32, + Thick: f32, + Radius: f32, + Size: vec3, + WbR: vec3, + WbQ: vec4, + BwR: vec3, + BwQ: vec4, +} + +//////// import: "shapegeom.go" + +//////// import: "shapes.go" +alias Shapes = i32; //enums:enum +const Plane: Shapes = 0; +const Sphere: Shapes = 1; +const Capsule: Shapes = 2; +const Cylinder: Shapes = 3; +const Box: Shapes = 4; +const Cone: Shapes = 5; + +//////// import: "slmath-matrix3.go" + +//////// import: "slmath-quaternion.go" +fn MulQuatVector(q: vec4, v: vec3) -> vec3 { + var xyz = vec3(q.x, q.y, q.z); + var t = Cross3(xyz, v)*(2);return v+(t*(q.w))+(Cross3(xyz, t)); +} +fn MulQuatVectorInverse(q: vec4, v: vec3) -> vec3 { + var xyz = vec3(q.x, q.y, q.z); + var t = Cross3(xyz, v)*(2);return v-(t*(q.w))+(Cross3(xyz, t)); +} +fn MulSpatialPoint(xP: vec3, xQ: vec4, p: vec3) -> vec3 { + var dp = MulQuatVector(xQ, p);return dp+(xP); +} + +//////// import: "slmath-vector2.go" + +//////// import: "slmath-vector3.go" +fn Negate3(v: vec3) -> vec3 { + return vec3(-v.x, -v.y, -v.z); +} +fn Length3(v: vec3) -> f32 { + return sqrt(v.x*v.x + v.y*v.y + v.z*v.z); +} +fn LengthSquared3(v: vec3) -> f32 { + return v.x*v.x + v.y*v.y + v.z*v.z; +} +fn Dot3(v: vec3,o: vec3) -> f32 { + return v.x*o.x + v.y*o.y + v.z*o.z; +} +fn Normal3(v: vec3) -> vec3 { + return v/(Length3(v)); +} +fn Cross3(v: vec3,o: vec3) -> vec3 { + return vec3(v.y*o.z-v.z*o.y, v.z*o.x-v.x*o.z, v.x*o.y-v.y*o.x); +} + +//////// import: "step.go" + +//////// import: "step_body.go" + +//////// import: "step_joint.go" \ No newline at end of file diff --git a/physics/shaders/StepBodyDeltas.wgsl b/physics/shaders/StepBodyDeltas.wgsl index 6d4e0620..5656ea3f 100644 --- a/physics/shaders/StepBodyDeltas.wgsl +++ b/physics/shaders/StepBodyDeltas.wgsl @@ -47,35 +47,37 @@ const BodyMass: BodyVars = 8; const BodyInvMass: BodyVars = 9; const BodyBounce: BodyVars = 10; const BodyFriction: BodyVars = 11; -const BodyPosX: BodyVars = 12; -const BodyPosY: BodyVars = 13; -const BodyPosZ: BodyVars = 14; -const BodyQuatX: BodyVars = 15; -const BodyQuatY: BodyVars = 16; -const BodyQuatZ: BodyVars = 17; -const BodyQuatW: BodyVars = 18; -const BodyComX: BodyVars = 19; -const BodyComY: BodyVars = 20; -const BodyComZ: BodyVars = 21; -const BodyInertiaXX: BodyVars = 22; -const BodyInertiaYX: BodyVars = 23; -const BodyInertiaZX: BodyVars = 24; -const BodyInertiaXY: BodyVars = 25; -const BodyInertiaYY: BodyVars = 26; -const BodyInertiaZY: BodyVars = 27; -const BodyInertiaXZ: BodyVars = 28; -const BodyInertiaYZ: BodyVars = 29; -const BodyInertiaZZ: BodyVars = 30; -const BodyInvInertiaXX: BodyVars = 31; -const BodyInvInertiaYX: BodyVars = 32; -const BodyInvInertiaZX: BodyVars = 33; -const BodyInvInertiaXY: BodyVars = 34; -const BodyInvInertiaYY: BodyVars = 35; -const BodyInvInertiaZY: BodyVars = 36; -const BodyInvInertiaXZ: BodyVars = 37; -const BodyInvInertiaYZ: BodyVars = 38; -const BodyInvInertiaZZ: BodyVars = 39; -const BodyRadius: BodyVars = 40; +const BodyFrictionTortion: BodyVars = 12; +const BodyFrictionRolling: BodyVars = 13; +const BodyPosX: BodyVars = 14; +const BodyPosY: BodyVars = 15; +const BodyPosZ: BodyVars = 16; +const BodyQuatX: BodyVars = 17; +const BodyQuatY: BodyVars = 18; +const BodyQuatZ: BodyVars = 19; +const BodyQuatW: BodyVars = 20; +const BodyComX: BodyVars = 21; +const BodyComY: BodyVars = 22; +const BodyComZ: BodyVars = 23; +const BodyInertiaXX: BodyVars = 24; +const BodyInertiaYX: BodyVars = 25; +const BodyInertiaZX: BodyVars = 26; +const BodyInertiaXY: BodyVars = 27; +const BodyInertiaYY: BodyVars = 28; +const BodyInertiaZY: BodyVars = 29; +const BodyInertiaXZ: BodyVars = 30; +const BodyInertiaYZ: BodyVars = 31; +const BodyInertiaZZ: BodyVars = 32; +const BodyInvInertiaXX: BodyVars = 33; +const BodyInvInertiaYX: BodyVars = 34; +const BodyInvInertiaZX: BodyVars = 35; +const BodyInvInertiaXY: BodyVars = 36; +const BodyInvInertiaYY: BodyVars = 37; +const BodyInvInertiaZY: BodyVars = 38; +const BodyInvInertiaXZ: BodyVars = 39; +const BodyInvInertiaYZ: BodyVars = 40; +const BodyInvInertiaZZ: BodyVars = 41; +const BodyRadius: BodyVars = 42; fn BodyCom(idx: i32) -> vec3 { return vec3(Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComX))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComY))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComZ))]); } @@ -112,9 +114,18 @@ const ContactBThick: ContactVars = 16; const ContactNormX: ContactVars = 17; const ContactNormY: ContactVars = 18; const ContactNormZ: ContactVars = 19; -const ContactForceX: ContactVars = 20; -const ContactForceY: ContactVars = 21; -const ContactForceZ: ContactVars = 22; +const ContactADeltaX: ContactVars = 20; +const ContactADeltaY: ContactVars = 21; +const ContactADeltaZ: ContactVars = 22; +const ContactAAngDeltaX: ContactVars = 23; +const ContactAAngDeltaY: ContactVars = 24; +const ContactAAngDeltaZ: ContactVars = 25; +const ContactBDeltaX: ContactVars = 26; +const ContactBDeltaY: ContactVars = 27; +const ContactBDeltaZ: ContactVars = 28; +const ContactBAngDeltaX: ContactVars = 29; +const ContactBAngDeltaY: ContactVars = 30; +const ContactBAngDeltaZ: ContactVars = 31; const BroadContactVarsN = ContactAPointX; //////// import: "control.go" @@ -195,8 +206,8 @@ fn SetDynamicAngDelta(idx: i32,cni: i32, angDelta: vec3) { } //////// import: "enumgen.go" -const BodyVarsN: BodyVars = 41; -const ContactVarsN: ContactVars = 23; +const BodyVarsN: BodyVars = 43; +const ContactVarsN: ContactVars = 32; const JointControlVarsN: JointControlVars = 3; const DynamicVarsN: DynamicVars = 32; const GPUVarsN: GPUVars = 12; diff --git a/physics/shaders/StepIntegrateBodies.wgsl b/physics/shaders/StepIntegrateBodies.wgsl index 100b4074..e53e4998 100644 --- a/physics/shaders/StepIntegrateBodies.wgsl +++ b/physics/shaders/StepIntegrateBodies.wgsl @@ -47,35 +47,37 @@ const BodyMass: BodyVars = 8; const BodyInvMass: BodyVars = 9; const BodyBounce: BodyVars = 10; const BodyFriction: BodyVars = 11; -const BodyPosX: BodyVars = 12; -const BodyPosY: BodyVars = 13; -const BodyPosZ: BodyVars = 14; -const BodyQuatX: BodyVars = 15; -const BodyQuatY: BodyVars = 16; -const BodyQuatZ: BodyVars = 17; -const BodyQuatW: BodyVars = 18; -const BodyComX: BodyVars = 19; -const BodyComY: BodyVars = 20; -const BodyComZ: BodyVars = 21; -const BodyInertiaXX: BodyVars = 22; -const BodyInertiaYX: BodyVars = 23; -const BodyInertiaZX: BodyVars = 24; -const BodyInertiaXY: BodyVars = 25; -const BodyInertiaYY: BodyVars = 26; -const BodyInertiaZY: BodyVars = 27; -const BodyInertiaXZ: BodyVars = 28; -const BodyInertiaYZ: BodyVars = 29; -const BodyInertiaZZ: BodyVars = 30; -const BodyInvInertiaXX: BodyVars = 31; -const BodyInvInertiaYX: BodyVars = 32; -const BodyInvInertiaZX: BodyVars = 33; -const BodyInvInertiaXY: BodyVars = 34; -const BodyInvInertiaYY: BodyVars = 35; -const BodyInvInertiaZY: BodyVars = 36; -const BodyInvInertiaXZ: BodyVars = 37; -const BodyInvInertiaYZ: BodyVars = 38; -const BodyInvInertiaZZ: BodyVars = 39; -const BodyRadius: BodyVars = 40; +const BodyFrictionTortion: BodyVars = 12; +const BodyFrictionRolling: BodyVars = 13; +const BodyPosX: BodyVars = 14; +const BodyPosY: BodyVars = 15; +const BodyPosZ: BodyVars = 16; +const BodyQuatX: BodyVars = 17; +const BodyQuatY: BodyVars = 18; +const BodyQuatZ: BodyVars = 19; +const BodyQuatW: BodyVars = 20; +const BodyComX: BodyVars = 21; +const BodyComY: BodyVars = 22; +const BodyComZ: BodyVars = 23; +const BodyInertiaXX: BodyVars = 24; +const BodyInertiaYX: BodyVars = 25; +const BodyInertiaZX: BodyVars = 26; +const BodyInertiaXY: BodyVars = 27; +const BodyInertiaYY: BodyVars = 28; +const BodyInertiaZY: BodyVars = 29; +const BodyInertiaXZ: BodyVars = 30; +const BodyInertiaYZ: BodyVars = 31; +const BodyInertiaZZ: BodyVars = 32; +const BodyInvInertiaXX: BodyVars = 33; +const BodyInvInertiaYX: BodyVars = 34; +const BodyInvInertiaZX: BodyVars = 35; +const BodyInvInertiaXY: BodyVars = 36; +const BodyInvInertiaYY: BodyVars = 37; +const BodyInvInertiaZY: BodyVars = 38; +const BodyInvInertiaXZ: BodyVars = 39; +const BodyInvInertiaYZ: BodyVars = 40; +const BodyInvInertiaZZ: BodyVars = 41; +const BodyRadius: BodyVars = 42; fn BodyCom(idx: i32) -> vec3 { return vec3(Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComX))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComY))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComZ))]); } @@ -112,9 +114,18 @@ const ContactBThick: ContactVars = 16; const ContactNormX: ContactVars = 17; const ContactNormY: ContactVars = 18; const ContactNormZ: ContactVars = 19; -const ContactForceX: ContactVars = 20; -const ContactForceY: ContactVars = 21; -const ContactForceZ: ContactVars = 22; +const ContactADeltaX: ContactVars = 20; +const ContactADeltaY: ContactVars = 21; +const ContactADeltaZ: ContactVars = 22; +const ContactAAngDeltaX: ContactVars = 23; +const ContactAAngDeltaY: ContactVars = 24; +const ContactAAngDeltaZ: ContactVars = 25; +const ContactBDeltaX: ContactVars = 26; +const ContactBDeltaY: ContactVars = 27; +const ContactBDeltaZ: ContactVars = 28; +const ContactBAngDeltaX: ContactVars = 29; +const ContactBAngDeltaY: ContactVars = 30; +const ContactBAngDeltaZ: ContactVars = 31; const BroadContactVarsN = ContactAPointX; //////// import: "control.go" @@ -201,8 +212,8 @@ fn SetDynamicAngDelta(idx: i32,cni: i32, angDelta: vec3) { } //////// import: "enumgen.go" -const BodyVarsN: BodyVars = 41; -const ContactVarsN: ContactVars = 23; +const BodyVarsN: BodyVars = 43; +const ContactVarsN: ContactVars = 32; const JointControlVarsN: JointControlVars = 3; const DynamicVarsN: DynamicVars = 32; const GPUVarsN: GPUVars = 12; diff --git a/physics/shaders/StepJointForces.wgsl b/physics/shaders/StepJointForces.wgsl index d78f33b2..424b948b 100644 --- a/physics/shaders/StepJointForces.wgsl +++ b/physics/shaders/StepJointForces.wgsl @@ -53,35 +53,37 @@ const BodyMass: BodyVars = 8; const BodyInvMass: BodyVars = 9; const BodyBounce: BodyVars = 10; const BodyFriction: BodyVars = 11; -const BodyPosX: BodyVars = 12; -const BodyPosY: BodyVars = 13; -const BodyPosZ: BodyVars = 14; -const BodyQuatX: BodyVars = 15; -const BodyQuatY: BodyVars = 16; -const BodyQuatZ: BodyVars = 17; -const BodyQuatW: BodyVars = 18; -const BodyComX: BodyVars = 19; -const BodyComY: BodyVars = 20; -const BodyComZ: BodyVars = 21; -const BodyInertiaXX: BodyVars = 22; -const BodyInertiaYX: BodyVars = 23; -const BodyInertiaZX: BodyVars = 24; -const BodyInertiaXY: BodyVars = 25; -const BodyInertiaYY: BodyVars = 26; -const BodyInertiaZY: BodyVars = 27; -const BodyInertiaXZ: BodyVars = 28; -const BodyInertiaYZ: BodyVars = 29; -const BodyInertiaZZ: BodyVars = 30; -const BodyInvInertiaXX: BodyVars = 31; -const BodyInvInertiaYX: BodyVars = 32; -const BodyInvInertiaZX: BodyVars = 33; -const BodyInvInertiaXY: BodyVars = 34; -const BodyInvInertiaYY: BodyVars = 35; -const BodyInvInertiaZY: BodyVars = 36; -const BodyInvInertiaXZ: BodyVars = 37; -const BodyInvInertiaYZ: BodyVars = 38; -const BodyInvInertiaZZ: BodyVars = 39; -const BodyRadius: BodyVars = 40; +const BodyFrictionTortion: BodyVars = 12; +const BodyFrictionRolling: BodyVars = 13; +const BodyPosX: BodyVars = 14; +const BodyPosY: BodyVars = 15; +const BodyPosZ: BodyVars = 16; +const BodyQuatX: BodyVars = 17; +const BodyQuatY: BodyVars = 18; +const BodyQuatZ: BodyVars = 19; +const BodyQuatW: BodyVars = 20; +const BodyComX: BodyVars = 21; +const BodyComY: BodyVars = 22; +const BodyComZ: BodyVars = 23; +const BodyInertiaXX: BodyVars = 24; +const BodyInertiaYX: BodyVars = 25; +const BodyInertiaZX: BodyVars = 26; +const BodyInertiaXY: BodyVars = 27; +const BodyInertiaYY: BodyVars = 28; +const BodyInertiaZY: BodyVars = 29; +const BodyInertiaXZ: BodyVars = 30; +const BodyInertiaYZ: BodyVars = 31; +const BodyInertiaZZ: BodyVars = 32; +const BodyInvInertiaXX: BodyVars = 33; +const BodyInvInertiaYX: BodyVars = 34; +const BodyInvInertiaZX: BodyVars = 35; +const BodyInvInertiaXY: BodyVars = 36; +const BodyInvInertiaYY: BodyVars = 37; +const BodyInvInertiaZY: BodyVars = 38; +const BodyInvInertiaXZ: BodyVars = 39; +const BodyInvInertiaYZ: BodyVars = 40; +const BodyInvInertiaZZ: BodyVars = 41; +const BodyRadius: BodyVars = 42; fn BodyCom(idx: i32) -> vec3 { return vec3(Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComX))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComY))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComZ))]); } @@ -108,9 +110,18 @@ const ContactBThick: ContactVars = 16; const ContactNormX: ContactVars = 17; const ContactNormY: ContactVars = 18; const ContactNormZ: ContactVars = 19; -const ContactForceX: ContactVars = 20; -const ContactForceY: ContactVars = 21; -const ContactForceZ: ContactVars = 22; +const ContactADeltaX: ContactVars = 20; +const ContactADeltaY: ContactVars = 21; +const ContactADeltaZ: ContactVars = 22; +const ContactAAngDeltaX: ContactVars = 23; +const ContactAAngDeltaY: ContactVars = 24; +const ContactAAngDeltaZ: ContactVars = 25; +const ContactBDeltaX: ContactVars = 26; +const ContactBDeltaY: ContactVars = 27; +const ContactBDeltaZ: ContactVars = 28; +const ContactBAngDeltaX: ContactVars = 29; +const ContactBAngDeltaY: ContactVars = 30; +const ContactBAngDeltaZ: ContactVars = 31; const BroadContactVarsN = ContactAPointX; //////// import: "control.go" @@ -167,8 +178,8 @@ fn DynamicQuat(idx: i32,cni: i32) -> vec4 { } //////// import: "enumgen.go" -const BodyVarsN: BodyVars = 41; -const ContactVarsN: ContactVars = 23; +const BodyVarsN: BodyVars = 43; +const ContactVarsN: ContactVars = 32; const JointControlVarsN: JointControlVars = 3; const DynamicVarsN: DynamicVars = 32; const GPUVarsN: GPUVars = 12; diff --git a/physics/shaders/StepSolveJoints.wgsl b/physics/shaders/StepSolveJoints.wgsl index 783c5dbb..6ea22792 100644 --- a/physics/shaders/StepSolveJoints.wgsl +++ b/physics/shaders/StepSolveJoints.wgsl @@ -53,35 +53,37 @@ const BodyMass: BodyVars = 8; const BodyInvMass: BodyVars = 9; const BodyBounce: BodyVars = 10; const BodyFriction: BodyVars = 11; -const BodyPosX: BodyVars = 12; -const BodyPosY: BodyVars = 13; -const BodyPosZ: BodyVars = 14; -const BodyQuatX: BodyVars = 15; -const BodyQuatY: BodyVars = 16; -const BodyQuatZ: BodyVars = 17; -const BodyQuatW: BodyVars = 18; -const BodyComX: BodyVars = 19; -const BodyComY: BodyVars = 20; -const BodyComZ: BodyVars = 21; -const BodyInertiaXX: BodyVars = 22; -const BodyInertiaYX: BodyVars = 23; -const BodyInertiaZX: BodyVars = 24; -const BodyInertiaXY: BodyVars = 25; -const BodyInertiaYY: BodyVars = 26; -const BodyInertiaZY: BodyVars = 27; -const BodyInertiaXZ: BodyVars = 28; -const BodyInertiaYZ: BodyVars = 29; -const BodyInertiaZZ: BodyVars = 30; -const BodyInvInertiaXX: BodyVars = 31; -const BodyInvInertiaYX: BodyVars = 32; -const BodyInvInertiaZX: BodyVars = 33; -const BodyInvInertiaXY: BodyVars = 34; -const BodyInvInertiaYY: BodyVars = 35; -const BodyInvInertiaZY: BodyVars = 36; -const BodyInvInertiaXZ: BodyVars = 37; -const BodyInvInertiaYZ: BodyVars = 38; -const BodyInvInertiaZZ: BodyVars = 39; -const BodyRadius: BodyVars = 40; +const BodyFrictionTortion: BodyVars = 12; +const BodyFrictionRolling: BodyVars = 13; +const BodyPosX: BodyVars = 14; +const BodyPosY: BodyVars = 15; +const BodyPosZ: BodyVars = 16; +const BodyQuatX: BodyVars = 17; +const BodyQuatY: BodyVars = 18; +const BodyQuatZ: BodyVars = 19; +const BodyQuatW: BodyVars = 20; +const BodyComX: BodyVars = 21; +const BodyComY: BodyVars = 22; +const BodyComZ: BodyVars = 23; +const BodyInertiaXX: BodyVars = 24; +const BodyInertiaYX: BodyVars = 25; +const BodyInertiaZX: BodyVars = 26; +const BodyInertiaXY: BodyVars = 27; +const BodyInertiaYY: BodyVars = 28; +const BodyInertiaZY: BodyVars = 29; +const BodyInertiaXZ: BodyVars = 30; +const BodyInertiaYZ: BodyVars = 31; +const BodyInertiaZZ: BodyVars = 32; +const BodyInvInertiaXX: BodyVars = 33; +const BodyInvInertiaYX: BodyVars = 34; +const BodyInvInertiaZX: BodyVars = 35; +const BodyInvInertiaXY: BodyVars = 36; +const BodyInvInertiaYY: BodyVars = 37; +const BodyInvInertiaZY: BodyVars = 38; +const BodyInvInertiaXZ: BodyVars = 39; +const BodyInvInertiaYZ: BodyVars = 40; +const BodyInvInertiaZZ: BodyVars = 41; +const BodyRadius: BodyVars = 42; fn BodyCom(idx: i32) -> vec3 { return vec3(Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComX))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComY))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComZ))]); } @@ -113,9 +115,18 @@ const ContactBThick: ContactVars = 16; const ContactNormX: ContactVars = 17; const ContactNormY: ContactVars = 18; const ContactNormZ: ContactVars = 19; -const ContactForceX: ContactVars = 20; -const ContactForceY: ContactVars = 21; -const ContactForceZ: ContactVars = 22; +const ContactADeltaX: ContactVars = 20; +const ContactADeltaY: ContactVars = 21; +const ContactADeltaZ: ContactVars = 22; +const ContactAAngDeltaX: ContactVars = 23; +const ContactAAngDeltaY: ContactVars = 24; +const ContactAAngDeltaZ: ContactVars = 25; +const ContactBDeltaX: ContactVars = 26; +const ContactBDeltaY: ContactVars = 27; +const ContactBDeltaZ: ContactVars = 28; +const ContactBAngDeltaX: ContactVars = 29; +const ContactBAngDeltaY: ContactVars = 30; +const ContactBAngDeltaZ: ContactVars = 31; const BroadContactVarsN = ContactAPointX; //////// import: "control.go" @@ -178,8 +189,8 @@ fn DynamicAngDelta(idx: i32,cni: i32) -> vec3 { } //////// import: "enumgen.go" -const BodyVarsN: BodyVars = 41; -const ContactVarsN: ContactVars = 23; +const BodyVarsN: BodyVars = 43; +const ContactVarsN: ContactVars = 32; const JointControlVarsN: JointControlVars = 3; const DynamicVarsN: DynamicVars = 32; const GPUVarsN: GPUVars = 12; @@ -803,20 +814,20 @@ fn JointAxisTarget(axis: vec3, targ: f32,weight: f32, axisTargets: ptr,tfbQ: vec4, mInva: f32,mInvb: f32, iInva: mat3x3f,iInvb: mat3x3f, lineara: vec3,linearb: vec3,angulara: vec3,angularb: vec3, lambdaIn: f32,compliance: f32,damping: f32,dt: f32) -> f32 { +fn PositionalCorrection(err: f32,derr: f32, tfaQ: vec4,tfbQ: vec4, mInvA: f32,mInvB: f32, iInvA: mat3x3f,iInvB: mat3x3f, linA: vec3,linB: vec3,angA: vec3,angB: vec3, lambdaIn: f32,compliance: f32,damping: f32,dt: f32) -> f32 { var denom = f32(0.0); - denom += LengthSquared3(lineara) * mInva; - denom += LengthSquared3(linearb) * mInvb; - var rotAngulara = MulQuatVectorInverse(tfaQ, angulara); - var rotAngularb = MulQuatVectorInverse(tfbQ, angularb); - denom += Dot3(rotAngulara, iInva*(rotAngulara)); - denom += Dot3(rotAngularb, iInvb*(rotAngularb)); + denom += LengthSquared3(linA) * mInvA; + denom += LengthSquared3(linB) * mInvB; + var rotAngA = MulQuatVectorInverse(tfaQ, angA); + var rotAngB = MulQuatVectorInverse(tfbQ, angB); + denom += Dot3(rotAngA, iInvA*(rotAngA)); + denom += Dot3(rotAngB, iInvB*(rotAngB)); var alpha = compliance; var gamma = compliance * damping; - var deltaLambda = -(err + alpha*lambdaIn + gamma*derr); + var lambda = -(err + alpha*lambdaIn + gamma*derr); if (denom+alpha > 0.0) { - deltaLambda /= (dt+gamma)*denom + alpha/dt; - }return deltaLambda; + lambda /= (dt+gamma)*denom + alpha/dt; + }return lambda; } fn AngularCorrection(err: f32,derr: f32, tfaQ: vec4,tfbQ: vec4, iInva: mat3x3f,iInvb: mat3x3f, angulara: vec3,angularb: vec3, lambdaIn: f32,compliance: f32,damping: f32,dt: f32) -> f32 { var rotAngulara = MulQuatVectorInverse(tfaQ, angulara); diff --git a/physics/shapegeom_test.go b/physics/shapegeom_test.go index 39047d48..28cfaaaa 100644 --- a/physics/shapegeom_test.go +++ b/physics/shapegeom_test.go @@ -47,7 +47,7 @@ func TestSphereSphere(t *testing.T) { InitGeomData(0, &gdB) var ptA, ptB, norm math32.Vector3 - dist := ColSphereSphere(0, &gdA, &gdB, &ptA, &ptB, &norm) + dist := ColSphereSphere(0, 10, &gdA, &gdB, &ptA, &ptB, &norm) margin := float32(0.01) var ctA, ctB, offA, offB math32.Vector3 diff --git a/physics/step.go b/physics/step.go index e7a68e01..a1c57cbc 100644 --- a/physics/step.go +++ b/physics/step.go @@ -21,6 +21,7 @@ func OneIfNonzero(f float32) float32 { // step does the following: // if self.compute_body_velocity_from_position_delta or self.enable_restitution: +// // save initial state: // body_q_init = wp.clone(state_in.body_q) // body_qd_init = wp.clone(state_in.body_qd) // body_deltas = wp.empty_like(state_out.body_qd) @@ -57,15 +58,26 @@ func (wl *World) Step() { params.Next = 1 } ToGPU(JointControlsVar) + wl.StepCollision() wl.StepJointForces() wl.StepIntegrateBodies() for range params.Iterations { wl.StepSolveJoints() + wl.StepBodyContacts() } RunDone(DynamicsVar) } +func (wl *World) StepCollision() { + params := GetParams(0) + RunCollisionInit(1) // could also just copy up + RunCollisionBroad(int(params.BodyCollidePairsN)) + // note: time getting BroadContactsN back down and using that vs. running full + RunCollisionNarrow(int(params.ContactsMax)) + RunDone(ContactsNVar) // we do multiple iterations so useful to have this +} + func (wl *World) StepJointForces() { params := GetParams(0) RunStepJointForces(int(params.JointsN)) @@ -82,3 +94,10 @@ func (wl *World) StepSolveJoints() { RunStepSolveJoints(int(params.JointsN)) RunDeltasFromJoints(int(params.DynamicsN)) } + +func (wl *World) StepBodyContacts() { + params := GetParams(0) + cmax := int(ContactsN.Values[0]) + RunStepBodyContacts(cmax) + RunDeltasFromContacts(int(params.DynamicsN)) +} diff --git a/physics/step.goal b/physics/step.goal index f2ef8de6..4973ad2c 100644 --- a/physics/step.goal +++ b/physics/step.goal @@ -19,6 +19,7 @@ func OneIfNonzero(f float32) float32 { // step does the following: // if self.compute_body_velocity_from_position_delta or self.enable_restitution: +// // save initial state: // body_q_init = wp.clone(state_in.body_q) // body_qd_init = wp.clone(state_in.body_qd) // body_deltas = wp.empty_like(state_out.body_qd) @@ -55,15 +56,26 @@ func (wl *World) Step() { params.Next = 1 } ToGPU(JointControlsVar) + wl.StepCollision() wl.StepJointForces() wl.StepIntegrateBodies() for range params.Iterations { wl.StepSolveJoints() + wl.StepBodyContacts() } RunDone(DynamicsVar) } +func (wl *World) StepCollision() { + params := GetParams(0) + RunCollisionInit(1) // could also just copy up + RunCollisionBroad(int(params.BodyCollidePairsN)) + // note: time getting BroadContactsN back down and using that vs. running full + RunCollisionNarrow(int(params.ContactsMax)) + RunDone(ContactsNVar) // we do multiple iterations so useful to have this +} + func (wl *World) StepJointForces() { params := GetParams(0) RunStepJointForces(int(params.JointsN)) @@ -80,3 +92,10 @@ func (wl *World) StepSolveJoints() { RunStepSolveJoints(int(params.JointsN)) RunDeltasFromJoints(int(params.DynamicsN)) } + +func (wl *World) StepBodyContacts() { + params := GetParams(0) + cmax := int(ContactsN.Values[0]) + RunStepBodyContacts(cmax) + RunDeltasFromContacts(int(params.DynamicsN)) +} diff --git a/physics/step_joint.go b/physics/step_joint.go index 2f6ecbfd..e3197704 100644 --- a/physics/step_joint.go +++ b/physics/step_joint.go @@ -461,27 +461,27 @@ func JointAxisTarget(axis math32.Vector3, targ, weight float32, axisTargets, axi *axisWeights = (*axisWeights).Add(slmath.Abs3(weightedAxis)) } -func PositionalCorrection(err, derr float32, tfaQ, tfbQ math32.Quat, mInva, mInvb float32, iInva, iInvb math32.Matrix3, lineara, linearb, angulara, angularb math32.Vector3, lambdaIn, compliance, damping, dt float32) float32 { +func PositionalCorrection(err, derr float32, tfaQ, tfbQ math32.Quat, mInvA, mInvB float32, iInvA, iInvB math32.Matrix3, linA, linB, angA, angB math32.Vector3, lambdaIn, compliance, damping, dt float32) float32 { denom := float32(0.0) - denom += slmath.LengthSquared3(lineara) * mInva - denom += slmath.LengthSquared3(linearb) * mInvb + denom += slmath.LengthSquared3(linA) * mInvA + denom += slmath.LengthSquared3(linB) * mInvB // # Eq. 2-3 (make sure to project into the frame of the body) - rotAngulara := slmath.MulQuatVectorInverse(tfaQ, angulara) - rotAngularb := slmath.MulQuatVectorInverse(tfbQ, angularb) + rotAngA := slmath.MulQuatVectorInverse(tfaQ, angA) + rotAngB := slmath.MulQuatVectorInverse(tfbQ, angB) - denom += slmath.Dot3(rotAngulara, iInva.MulVector3(rotAngulara)) - denom += slmath.Dot3(rotAngularb, iInvb.MulVector3(rotAngularb)) + denom += slmath.Dot3(rotAngA, iInvA.MulVector3(rotAngA)) + denom += slmath.Dot3(rotAngB, iInvB.MulVector3(rotAngB)) alpha := compliance gamma := compliance * damping - deltaLambda := -(err + alpha*lambdaIn + gamma*derr) + lambda := -(err + alpha*lambdaIn + gamma*derr) if denom+alpha > 0.0 { - deltaLambda /= (dt+gamma)*denom + alpha/dt + lambda /= (dt+gamma)*denom + alpha/dt } - return deltaLambda + return lambda } func AngularCorrection(err, derr float32, tfaQ, tfbQ math32.Quat, iInva, iInvb math32.Matrix3, angulara, angularb math32.Vector3, lambdaIn, compliance, damping, dt float32) float32 { diff --git a/physics/step_joint.goal b/physics/step_joint.goal index bc7b2996..f042735c 100644 --- a/physics/step_joint.goal +++ b/physics/step_joint.goal @@ -459,27 +459,27 @@ func JointAxisTarget(axis math32.Vector3, targ, weight float32, axisTargets, axi *axisWeights = (*axisWeights).Add(slmath.Abs3(weightedAxis)) } -func PositionalCorrection(err, derr float32, tfaQ, tfbQ math32.Quat, mInva, mInvb float32, iInva, iInvb math32.Matrix3, lineara, linearb, angulara, angularb math32.Vector3, lambdaIn, compliance, damping, dt float32) float32 { +func PositionalCorrection(err, derr float32, tfaQ, tfbQ math32.Quat, mInvA, mInvB float32, iInvA, iInvB math32.Matrix3, linA, linB, angA, angB math32.Vector3, lambdaIn, compliance, damping, dt float32) float32 { denom := float32(0.0) - denom += slmath.LengthSquared3(lineara) * mInva - denom += slmath.LengthSquared3(linearb) * mInvb + denom += slmath.LengthSquared3(linA) * mInvA + denom += slmath.LengthSquared3(linB) * mInvB // # Eq. 2-3 (make sure to project into the frame of the body) - rotAngulara := slmath.MulQuatVectorInverse(tfaQ, angulara) - rotAngularb := slmath.MulQuatVectorInverse(tfbQ, angularb) + rotAngA := slmath.MulQuatVectorInverse(tfaQ, angA) + rotAngB := slmath.MulQuatVectorInverse(tfbQ, angB) - denom += slmath.Dot3(rotAngulara, iInva.MulVector3(rotAngulara)) - denom += slmath.Dot3(rotAngularb, iInvb.MulVector3(rotAngularb)) + denom += slmath.Dot3(rotAngA, iInvA.MulVector3(rotAngA)) + denom += slmath.Dot3(rotAngB, iInvB.MulVector3(rotAngB)) alpha := compliance gamma := compliance * damping - deltaLambda := -(err + alpha*lambdaIn + gamma*derr) + lambda := -(err + alpha*lambdaIn + gamma*derr) if denom+alpha > 0.0 { - deltaLambda /= (dt+gamma)*denom + alpha/dt + lambda /= (dt+gamma)*denom + alpha/dt } - return deltaLambda + return lambda } func AngularCorrection(err, derr float32, tfaQ, tfbQ math32.Quat, iInva, iInvb math32.Matrix3, angulara, angularb math32.Vector3, lambdaIn, compliance, damping, dt float32) float32 { diff --git a/physics/typegen.go b/physics/typegen.go index 094f6490..b3d329d9 100644 --- a/physics/typegen.go +++ b/physics/typegen.go @@ -24,7 +24,7 @@ var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.JointVars", var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.JointDoFVars", IDName: "joint-do-f-vars", Doc: "JointDoFVars are joint DoF state variables stored in tensor.Float32,\none for each DoF."}) -var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.PhysParams", IDName: "phys-params", Doc: "PhysParams are the physics parameters", Directives: []types.Directive{{Tool: "gosl", Directive: "start"}}, Fields: []types.Field{{Name: "Iterations", Doc: "Iterations is the number of iterations to perform."}, {Name: "Dt", Doc: "Dt is the integration stepsize."}, {Name: "SoftRelax", Doc: "SoftRelax is soft-body relaxation constant."}, {Name: "JointLinearRelax", Doc: "JointLinearRelax is joint linear relaxation constant."}, {Name: "JointAngularRelax", Doc: "JointAngularRelax is joint angular relaxation constant."}, {Name: "JointLinearComply", Doc: "JointLinearComply is joint linear compliance constant."}, {Name: "JointAngularComply", Doc: "JointAngularComply is joint angular compliance constant."}, {Name: "ContactRelax", Doc: "ContactRelax is rigid contact relaxation constant."}, {Name: "AngularDamping", Doc: "AngularDamping is damping of angular motion."}, {Name: "ContactWeighting", Doc: "Contact weighting"}, {Name: "Restitution", Doc: "Restitution"}, {Name: "MaxGeomIter", Doc: "MaxGeomIter is number of iterations to perform in shape-based\ngeometry collision computations"}, {Name: "ContactMargin", Doc: "Contact margin is the extra distance for broadphase collision\naround rigid bodies."}, {Name: "ContactsMax", Doc: "Maximum number of contacts to process at any given point."}, {Name: "Cur", Doc: "Index for the current state (0 or 1, alternates with Next)."}, {Name: "Next", Doc: "Index for the next state (1 or 0, alternates with Cur)."}, {Name: "BodiesN", Doc: "BodiesN is number of rigid bodies."}, {Name: "DynamicsN", Doc: "DynamicsN is number of dynamics bodies."}, {Name: "JointsN", Doc: "JointsN is number of joints."}, {Name: "JointDoFsN", Doc: "JointDoFsN is number of joint DoFs."}, {Name: "BodyJointsMax", Doc: "BodyJointsMax is max number of joints per body + 1 for actual n."}, {Name: "BodyCollidePairsN", Doc: "BodyCollidePairsN is the total number of pre-compiled collision pairs\nto examine."}, {Name: "pad"}, {Name: "pad1"}, {Name: "Gravity", Doc: "Gravity is the gravity acceleration function"}}}) +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.PhysParams", IDName: "phys-params", Doc: "PhysParams are the physics parameters", Directives: []types.Directive{{Tool: "gosl", Directive: "start"}}, Fields: []types.Field{{Name: "Iterations", Doc: "Iterations is the number of iterations to perform."}, {Name: "Dt", Doc: "Dt is the integration stepsize."}, {Name: "SoftRelax", Doc: "SoftRelax is soft-body relaxation constant."}, {Name: "JointLinearRelax", Doc: "JointLinearRelax is joint linear relaxation constant."}, {Name: "JointAngularRelax", Doc: "JointAngularRelax is joint angular relaxation constant."}, {Name: "JointLinearComply", Doc: "JointLinearComply is joint linear compliance constant."}, {Name: "JointAngularComply", Doc: "JointAngularComply is joint angular compliance constant."}, {Name: "ContactRelax", Doc: "ContactRelax is rigid contact relaxation constant."}, {Name: "AngularDamping", Doc: "AngularDamping is damping of angular motion."}, {Name: "ContactWeighting", Doc: "Contact weighting: todo: requires atomic add"}, {Name: "Restitution", Doc: "Restitution"}, {Name: "MaxGeomIter", Doc: "MaxGeomIter is number of iterations to perform in shape-based\ngeometry collision computations"}, {Name: "ContactMargin", Doc: "Contact margin is the extra distance for broadphase collision\naround rigid bodies."}, {Name: "ContactsMax", Doc: "Maximum number of contacts to process at any given point."}, {Name: "Cur", Doc: "Index for the current state (0 or 1, alternates with Next)."}, {Name: "Next", Doc: "Index for the next state (1 or 0, alternates with Cur)."}, {Name: "BodiesN", Doc: "BodiesN is number of rigid bodies."}, {Name: "DynamicsN", Doc: "DynamicsN is number of dynamics bodies."}, {Name: "JointsN", Doc: "JointsN is number of joints."}, {Name: "JointDoFsN", Doc: "JointDoFsN is number of joint DoFs."}, {Name: "BodyJointsMax", Doc: "BodyJointsMax is max number of joints per body + 1 for actual n."}, {Name: "BodyCollidePairsN", Doc: "BodyCollidePairsN is the total number of pre-compiled collision pairs\nto examine."}, {Name: "pad"}, {Name: "pad1"}, {Name: "Gravity", Doc: "Gravity is the gravity acceleration function"}}}) var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.GeomData", IDName: "geom-data", Doc: "GeomData contains all geometric data for narrow-phase collision.", Directives: []types.Directive{{Tool: "gosl", Directive: "start"}}, Fields: []types.Field{{Name: "BodyIdx"}, {Name: "Shape"}, {Name: "MinSize", Doc: "MinSize is the min of the Size dimensions."}, {Name: "Thick", Doc: "Thickness of shape."}, {Name: "Radius", Doc: "Radius is the effective radius for sphere-like elements (Sphere, Capsule, Cone)"}, {Name: "Size"}, {Name: "WbR", Doc: "World-to-Body transform\nPosition (R) (i.e., BodyPos)"}, {Name: "WbQ", Doc: "Quaternion (Q) (i.e., BodyQuat)"}, {Name: "BwR", Doc: "Body-to-World transform (inverse)\nPosition (R)"}, {Name: "BwQ", Doc: "Quaternion (Q)"}}}) diff --git a/physics/vars.go b/physics/vars.go index 19041502..390d0d9a 100644 --- a/physics/vars.go +++ b/physics/vars.go @@ -46,9 +46,7 @@ var ( // BodyCollidePairs are pairs of Body indexes that could potentially collide // based on precomputed collision logic, using World, Group, and Joint indexes. - // The last entry is updated to contain the actual number of contacts generated - // in the CollisionBroad routine on each collision iteration. - // [BodyCollidePairsN+1][2] + // [BodyCollidePairsN][2] //gosl:dims 2 BodyCollidePairs *tensor.Int32 diff --git a/physics/world.go b/physics/world.go index ce21dc1d..419b383e 100644 --- a/physics/world.go +++ b/physics/world.go @@ -89,6 +89,7 @@ func (wl *World) Init() { wl.Joints = tensor.NewFloat32(0, int(JointVarsN)) wl.JointDoFs = tensor.NewFloat32(0, int(JointDoFVarsN)) wl.BodyJoints = tensor.NewInt32(0, 2, 2) + wl.BodyCollidePairs = tensor.NewInt32(0, 2) wl.Dynamics = tensor.NewFloat32(0, 2, int(DynamicVarsN)) wl.BroadContactsN = tensor.NewInt32(1) wl.BroadContacts = tensor.NewFloat32(0, int(ContactVarsN)) @@ -151,6 +152,7 @@ func (wl *World) SetAsCurrentVars() { Bodies = wl.Bodies Joints = wl.Joints BodyJoints = wl.BodyJoints + BodyCollidePairs = wl.BodyCollidePairs JointDoFs = wl.JointDoFs Dynamics = wl.Dynamics BroadContactsN = wl.BroadContactsN @@ -172,5 +174,5 @@ func (wl *World) GPUInit() { // the GPU. This is done in GPUInit, and if current switched. func (wl *World) ToGPUInfra() { ToGPUTensorStrides() - ToGPU(ParamsVar, BodiesVar, JointsVar, BodyJointsVar, JointDoFsVar, DynamicsVar, BroadContactsNVar, BroadContactsVar, ContactsNVar, ContactsVar) + ToGPU(ParamsVar, BodiesVar, JointsVar, BodyJointsVar, BodyCollidePairsVar, JointDoFsVar, DynamicsVar, BroadContactsNVar, BroadContactsVar, ContactsNVar, ContactsVar) } diff --git a/physics/world.goal b/physics/world.goal index 88af9733..ff32efe8 100644 --- a/physics/world.goal +++ b/physics/world.goal @@ -87,6 +87,7 @@ func (wl *World) Init() { wl.Joints = tensor.NewFloat32(0, int(JointVarsN)) wl.JointDoFs = tensor.NewFloat32(0, int(JointDoFVarsN)) wl.BodyJoints = tensor.NewInt32(0, 2, 2) + wl.BodyCollidePairs = tensor.NewInt32(0, 2) wl.Dynamics = tensor.NewFloat32(0, 2, int(DynamicVarsN)) wl.BroadContactsN = tensor.NewInt32(1) wl.BroadContacts = tensor.NewFloat32(0, int(ContactVarsN)) @@ -149,6 +150,7 @@ func (wl *World) SetAsCurrentVars() { Bodies = wl.Bodies Joints = wl.Joints BodyJoints = wl.BodyJoints + BodyCollidePairs = wl.BodyCollidePairs JointDoFs = wl.JointDoFs Dynamics = wl.Dynamics BroadContactsN = wl.BroadContactsN @@ -170,5 +172,5 @@ func (wl *World) GPUInit() { // the GPU. This is done in GPUInit, and if current switched. func (wl *World) ToGPUInfra() { ToGPUTensorStrides() - ToGPU(ParamsVar, BodiesVar, JointsVar, BodyJointsVar, JointDoFsVar, DynamicsVar, BroadContactsNVar, BroadContactsVar, ContactsNVar, ContactsVar) + ToGPU(ParamsVar, BodiesVar, JointsVar, BodyJointsVar, BodyCollidePairsVar, JointDoFsVar, DynamicsVar, BroadContactsNVar, BroadContactsVar, ContactsNVar, ContactsVar) } diff --git a/physics/world/view.go b/physics/world/view.go index 8c6cb557..d3bbe78f 100644 --- a/physics/world/view.go +++ b/physics/world/view.go @@ -141,7 +141,7 @@ func (vw *View) BoxInit(sld *xyz.Solid) { } sld.SetMeshName(mnm) sld.Pose.Scale = vw.Size.MulScalar(2) - vw.UpdateColor(vw.Color, s6ld) + vw.UpdateColor(vw.Color, sld) sld.Updater(func() { vw.UpdatePose(sld) }) @@ -157,7 +157,7 @@ func (vw *View) PlaneInit(sld *xyz.Solid) { } sld.SetMeshName(mnm) if vw.Size.X == 0 { - inf := 1e6 + inf := float32(1e6) sld.Pose.Scale = math32.Vec3(inf, inf, 1) } else { sld.Pose.Scale = vw.Size.MulScalar(2) From b160a62d9f46d7bd86b5723ec276c5ccaae06fbe Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Sun, 21 Dec 2025 22:52:46 +0100 Subject: [PATCH 36/97] physics: not obviously working correctly.. --- physics/examples/test1/test1.go | 20 ++++++++++++-------- physics/joint.go | 24 ++++++++++++++++++++++++ physics/joint.goal | 24 ++++++++++++++++++++++++ 3 files changed, 60 insertions(+), 8 deletions(-) diff --git a/physics/examples/test1/test1.go b/physics/examples/test1/test1.go index 2ecd65b8..0726e266 100644 --- a/physics/examples/test1/test1.go +++ b/physics/examples/test1/test1.go @@ -48,22 +48,26 @@ func main() { split.SetSplits(0.2, 0.8) rot := math32.NewQuat(0, 0, 0, 1) - thick := float32(0.1) - wr.NewBody(wl, "floor", physics.Box, "grey", math32.Vec3(10, thick, 10), - math32.Vec3(0, -thick, 0), rot) + // thick := float32(0.1) + wr.NewBody(wl, "floor", physics.Plane, "grey", math32.Vec3(10, 0, 10), + math32.Vec3(0, 0, 0), rot) height := float32(.5) width := height * .4 depth := height * .15 - b1 := wr.NewDynamic(wl, "body", physics.Box, "purple", 1.0, math32.Vec3(width, height, depth), - math32.Vec3(0, height, 0), rot) + _ = width + b1 := wr.NewDynamic(wl, "body", physics.Sphere, "purple", 1.0, math32.Vec3(height, height, depth), + math32.Vec3(0, height+2, 0), rot) + _ = b1 - bj := wl.NewJointRevolute(-1, b1.DynamicIndex, math32.Vec3(0, 0, 0), math32.Vec3(0, -height/2, 0), math32.Vec3(0, 1, 0)) - // bj := wl.NewJointPrismatic(-1, b1.DynamicIndex, math32.Vec3(0, 0, 0), math32.Vec3(0, -height/2, 0), math32.Vec3(1, 0, 0)) + // bj := wl.NewJointRevolute(-1, b1.DynamicIndex, math32.Vec3(0, 0, 0), math32.Vec3(0, -height, 0), math32.Vec3(0, 1, 0)) + // bj := wl.NewJointPrismatic(-1, b1.DynamicIndex, math32.Vec3(0, 0, 0), math32.Vec3(0, -height, 0), math32.Vec3(1, 0, 0)) // physics.SetJointControlForce(bj, 0, 5) - physics.SetJointTargetPos(bj, 0, 1) + // physics.SetJointTargetPos(bj, 0, 1) // physics.SetJointDoF(bj, 0, physics.JointDamp, 0.01) // physics.SetJointDoF(bj, 0, physics.JointStiff, 1) // this makes a big difference + bj := wl.NewJointFree(-1, b1.DynamicIndex, math32.Vec3(0, 0, 0), math32.Vec3(0, -height, 0)) + _ = bj wr.Init(wl) diff --git a/physics/joint.go b/physics/joint.go index 9b9b9069..9693032d 100644 --- a/physics/joint.go +++ b/physics/joint.go @@ -458,6 +458,30 @@ func (wl *World) NewJointDistance(parent, child int32, ppos, cpos math32.Vector3 return idx } +// NewJointFree adds a new Free joint +// between parent and child dynamic object indexes, +// with distance constrained only on the first linear X axis. +// Use -1 for parent to add a world-anchored joint. +// ppos, cpos are the relative positions from the parent, child. +// Sets relative rotation matricies to identity by default. +// Use [SetJointDoF] to set the remaining DoF parameters. +func (wl *World) NewJointFree(parent, child int32, ppos, cpos math32.Vector3) int32 { + idx := wl.newJoint(Distance, parent, child, ppos, cpos) + SetJointLinearDoFN(idx, 0) + SetJointAngularDoFN(idx, 0) + for d := range math32.W { + axis := math32.Vector3{} + axis.SetDim(d, 1) + wl.newJointDoF(idx, int32(d), axis) + } + for d := range math32.W { + axis := math32.Vector3{} + axis.SetDim(d, 1) + wl.newJointDoF(idx, int32(d), axis) + } + return idx +} + // newJoint adds a new joint between parent and child // dynamic object indexes. // Use -1 for parent to add a world-anchored joint. diff --git a/physics/joint.goal b/physics/joint.goal index 10888f09..7d111e50 100644 --- a/physics/joint.goal +++ b/physics/joint.goal @@ -458,6 +458,30 @@ func (wl *World) NewJointDistance(parent, child int32, ppos, cpos math32.Vector3 return idx } +// NewJointFree adds a new Free joint +// between parent and child dynamic object indexes, +// with distance constrained only on the first linear X axis. +// Use -1 for parent to add a world-anchored joint. +// ppos, cpos are the relative positions from the parent, child. +// Sets relative rotation matricies to identity by default. +// Use [SetJointDoF] to set the remaining DoF parameters. +func (wl *World) NewJointFree(parent, child int32, ppos, cpos math32.Vector3) int32 { + idx := wl.newJoint(Distance, parent, child, ppos, cpos) + SetJointLinearDoFN(idx, 0) + SetJointAngularDoFN(idx, 0) + for d := range math32.W { + axis := math32.Vector3{} + axis.SetDim(d, 1) + wl.newJointDoF(idx, int32(d), axis) + } + for d := range math32.W { + axis := math32.Vector3{} + axis.SetDim(d, 1) + wl.newJointDoF(idx, int32(d), axis) + } + return idx +} + // newJoint adds a new joint between parent and child // dynamic object indexes. // Use -1 for parent to add a world-anchored joint. From f747fde951533b740b2a1685f1a3068edbfe23b0 Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Mon, 22 Dec 2025 10:38:00 +0100 Subject: [PATCH 37/97] physics: bouncing ball working-ish, normals sorted, contacts working; needs restitution --- gosl/gotosl/nodes.go | 1 + physics/config.go | 14 ++++ physics/config.goal | 14 ++++ physics/contact.go | 42 +++++++--- physics/contact.goal | 44 +++++++--- physics/dynamics.go | 4 + physics/dynamics.goal | 4 + physics/enumgen.go | 20 ++--- physics/examples/test1/test1.go | 54 ++++++++---- physics/shaders/CollisionBroad.wgsl | 42 ++++++---- physics/shaders/CollisionInit.wgsl | 30 +++---- physics/shaders/CollisionNarrow.wgsl | 93 +++++++++++++-------- physics/shaders/DeltasFromContacts.wgsl | 42 ++++++---- physics/shaders/DeltasFromJoints.wgsl | 36 ++++---- physics/shaders/DynamicsCurToNext.wgsl | 33 ++++---- physics/shaders/ForcesFromJoints.wgsl | 36 ++++---- physics/shaders/InitDynamics.wgsl | 37 ++++---- physics/shaders/StepBodyContacts.wgsl | 102 +++++++++++++++++------ physics/shaders/StepBodyDeltas.wgsl | 68 +++++++-------- physics/shaders/StepIntegrateBodies.wgsl | 95 ++++++++------------- physics/shaders/StepJointForces.wgsl | 54 ++++++------ physics/shaders/StepSolveJoints.wgsl | 63 +++++++------- physics/shapecollide.go | 41 ++++----- physics/shapecollide.goal | 41 ++++----- physics/shapegeom.go | 4 +- physics/step.go | 2 +- physics/step.goal | 2 +- physics/step_body.go | 21 +++-- physics/step_body.goal | 23 +++-- physics/world.go | 3 +- physics/world.goal | 3 +- 31 files changed, 627 insertions(+), 441 deletions(-) diff --git a/gosl/gotosl/nodes.go b/gosl/gotosl/nodes.go index fd4dfa6f..df35c0c8 100644 --- a/gosl/gotosl/nodes.go +++ b/gosl/gotosl/nodes.go @@ -2396,6 +2396,7 @@ func (p *printer) stmt(stmt ast.Stmt, nextIsRBrace bool, nosemi bool) { if !nosemi { p.print(token.SEMICOLON) } + p.print(newline) case *ast.GoStmt: p.print(token.GO, blank) diff --git a/physics/config.go b/physics/config.go index c06d09c6..2890bc7d 100644 --- a/physics/config.go +++ b/physics/config.go @@ -15,6 +15,7 @@ func (wl *World) Config() { wl.ConfigBodyCollidePairs() wl.SetMaxContacts() wl.SetAsCurrent() + wl.ConfigBodies() wl.GPUInit() wl.InitState() } @@ -59,6 +60,19 @@ func (wl *World) ConfigJoints() { } } +// ConfigBodies updates computed body values from current values. +// Call if body params (mass, size) change. +func (wl *World) ConfigBodies() { + params := &wl.Params[0] + nb := params.BodiesN + for bi := range nb { + shape := GetBodyShape(bi) + size := BodyHSize(bi) + mass := Bodies.Value(int(bi), int(BodyMass)) + wl.SetMass(bi, shape, size, mass) + } +} + // InitState initializes the simulation state. func (wl *World) InitState() { params := GetParams(0) diff --git a/physics/config.goal b/physics/config.goal index c48361c2..fb2aaaac 100644 --- a/physics/config.goal +++ b/physics/config.goal @@ -13,6 +13,7 @@ func (wl *World) Config() { wl.ConfigBodyCollidePairs() wl.SetMaxContacts() wl.SetAsCurrent() + wl.ConfigBodies() wl.GPUInit() wl.InitState() } @@ -57,6 +58,19 @@ func (wl *World) ConfigJoints() { } } +// ConfigBodies updates computed body values from current values. +// Call if body params (mass, size) change. +func (wl *World) ConfigBodies() { + params := &wl.Params[0] + nb := params.BodiesN + for bi := range nb { + shape := GetBodyShape(bi) + size := BodyHSize(bi) + mass := Bodies[bi, BodyMass] + wl.SetMass(bi, shape, size, mass) + } +} + // InitState initializes the simulation state. func (wl *World) InitState() { params := GetParams(0) diff --git a/physics/contact.go b/physics/contact.go index 86382fbb..1bbe444b 100644 --- a/physics/contact.go +++ b/physics/contact.go @@ -62,6 +62,10 @@ const ( ContactNormY ContactNormZ + // contact weighting -- 1 if contact made; for restitution + // use this to filter contacts when updating body. + ContactWeight + // computed contact deltas, A ContactADeltaX ContactADeltaY @@ -295,6 +299,7 @@ func CollisionBroad(i uint32) { //gosl:kernel if d > rb+margin { return } + // fmt.Println("broad ct plane:", queryB, szA, closest, d, rb, margin) } else { d := slmath.Length3(xwAR.Sub(xwBR)) ra := Bodies.Value(int(biA), int(BodyRadius)) @@ -302,9 +307,6 @@ func CollisionBroad(i uint32) { //gosl:kernel return } } - // pair_index_ab = shape_a * num_shapes + shape_b - // pair_index_ba = shape_b * num_shapes + shape_a - var ncB int32 ncA := ShapePairContacts(sA, sB, infPlane, &ncB) @@ -452,6 +454,7 @@ func StepBodyContacts(i uint32) { //gosl:kernel if ci >= cmax { return } + biA := GetContactA(ci) biB := GetContactB(ci) diA := GetBodyDynamic(biA) @@ -473,11 +476,17 @@ func StepBodyContacts(i uint32) { //gosl:kernel thickA := Contacts.Value(int(ci), int(ContactAThick)) thickB := Contacts.Value(int(ci), int(ContactBThick)) thick := thickA + thickB - norm := ContactNorm(ci) - nnorm := slmath.Negate3(norm) + nnorm := ContactNorm(ci) + norm := slmath.Negate3(nnorm) d := slmath.Dot3(norm, ctBw.Sub(ctAw)) - thick if d >= 0.0 { // now separated + Contacts.Set(0.0, int(ci), int(ContactWeight)) + z := math32.Vec3(0, 0, 0) + SetContactADelta(ci, z) + SetContactBDelta(ci, z) + SetContactAAngDelta(ci, z) + SetContactBAngDelta(ci, z) return } comA := BodyCom(biA) @@ -508,8 +517,6 @@ func StepBodyContacts(i uint32) { //gosl:kernel angA := slmath.Negate3(slmath.Cross3(dA, norm)) angB := slmath.Cross3(dB, norm) - // todo: contact_inv_weight -- requires atomic - lambdaN := ContactConstraint(d, q0A, q0B, mInvA, mInvB, iInvA, iInvB, nnorm, norm, angA, angB, params.ContactRelax, params.Dt) linDeltaA := slmath.Negate3(norm).MulScalar(lambdaN) @@ -580,6 +587,7 @@ func StepBodyContacts(i uint32) { //gosl:kernel } } + Contacts.Set(1.0, int(ci), int(ContactWeight)) SetContactADelta(ci, linDeltaA) SetContactBDelta(ci, linDeltaB) SetContactAAngDelta(ci, angDeltaA) @@ -597,26 +605,38 @@ func DeltasFromContacts(i uint32) { //gosl:kernel bi := DynamicBody(di) - td := DynamicDelta(di, params.Next) - ta := DynamicAngDelta(di, params.Next) + td := math32.Vec3(0, 0, 0) + ta := math32.Vec3(0, 0, 0) + tw := float32(0) for ci := range cmax { + wt := Contacts.Value(int(ci), int(ContactWeight)) + if wt == 0 { + continue + } biA := GetContactA(ci) biB := GetContactB(ci) if biA == bi { + tw += wt d := ContactADelta(ci) td = td.Add(d) a := ContactAAngDelta(ci) ta = ta.Add(a) } if biB == bi { + tw += wt d := ContactBDelta(ci) td = td.Add(d) a := ContactBAngDelta(ci) ta = ta.Add(a) } } - SetDynamicDelta(di, params.Next, td) - SetDynamicAngDelta(di, params.Next, ta) + v0 := DynamicDelta(di, params.Next) + w0 := DynamicAngDelta(di, params.Next) + // fmt.Println(params.Next, "contact:", v0, td) + + SetDynamicDelta(di, params.Next, td.Add(v0)) + SetDynamicAngDelta(di, params.Next, ta.Add(w0)) + Dynamics.Set(tw, int(di), int(params.Next), int(DynContactWeight)) } func ContactConstraint(err float32, q0A, q0B math32.Quat, mInvA, mInvB float32, iInvA, iInvB math32.Matrix3, linA, linB, angA, angB math32.Vector3, relaxation, dt float32) float32 { diff --git a/physics/contact.goal b/physics/contact.goal index feaa5ad0..78e27203 100644 --- a/physics/contact.goal +++ b/physics/contact.goal @@ -60,6 +60,10 @@ const ( ContactNormY ContactNormZ + // contact weighting -- 1 if contact made; for restitution + // use this to filter contacts when updating body. + ContactWeight + // computed contact deltas, A ContactADeltaX ContactADeltaY @@ -293,6 +297,7 @@ func CollisionBroad(i uint32) { //gosl:kernel if d > rb+margin { return } + // fmt.Println("broad ct plane:", queryB, szA, closest, d, rb, margin) } else { d := slmath.Length3(xwAR.Sub(xwBR)) ra := Bodies[biA, BodyRadius] @@ -300,9 +305,6 @@ func CollisionBroad(i uint32) { //gosl:kernel return } } - // pair_index_ab = shape_a * num_shapes + shape_b - // pair_index_ba = shape_b * num_shapes + shape_a - var ncB int32 ncA := ShapePairContacts(sA, sB, infPlane, &ncB) @@ -450,6 +452,7 @@ func StepBodyContacts(i uint32) { //gosl:kernel if ci >= cmax { return } + biA := GetContactA(ci) biB := GetContactB(ci) diA := GetBodyDynamic(biA) @@ -471,11 +474,17 @@ func StepBodyContacts(i uint32) { //gosl:kernel thickA := Contacts[ci, ContactAThick] thickB := Contacts[ci, ContactBThick] thick := thickA + thickB - norm := ContactNorm(ci) - nnorm := slmath.Negate3(norm) + nnorm := ContactNorm(ci) + norm := slmath.Negate3(nnorm) d := slmath.Dot3(norm, ctBw.Sub(ctAw)) - thick if d >= 0.0 { // now separated + Contacts[ci, ContactWeight] = 0.0 + z := math32.Vec3(0,0,0) + SetContactADelta(ci, z) + SetContactBDelta(ci, z) + SetContactAAngDelta(ci, z) + SetContactBAngDelta(ci, z) return } comA := BodyCom(biA) @@ -506,8 +515,6 @@ func StepBodyContacts(i uint32) { //gosl:kernel angA := slmath.Negate3(slmath.Cross3(dA, norm)) angB := slmath.Cross3(dB, norm) - // todo: contact_inv_weight -- requires atomic - lambdaN := ContactConstraint(d, q0A, q0B, mInvA, mInvB, iInvA, iInvB, nnorm, norm, angA, angB, params.ContactRelax, params.Dt) linDeltaA := slmath.Negate3(norm).MulScalar(lambdaN) @@ -577,7 +584,8 @@ func StepBodyContacts(i uint32) { //gosl:kernel angDeltaB = angDeltaB.Add(rollN.MulScalar(lambdaRoll)) } } - + + Contacts[ci, ContactWeight] = 1.0 SetContactADelta(ci, linDeltaA) SetContactBDelta(ci, linDeltaB) SetContactAAngDelta(ci, angDeltaA) @@ -595,26 +603,38 @@ func DeltasFromContacts(i uint32) { //gosl:kernel bi := DynamicBody(di) - td := DynamicDelta(di, params.Next) - ta := DynamicAngDelta(di, params.Next) + td := math32.Vec3(0,0,0) + ta := math32.Vec3(0,0,0) + tw := float32(0) for ci := range cmax { + wt := Contacts[ci, ContactWeight] + if wt == 0 { + continue + } biA := GetContactA(ci) biB := GetContactB(ci) if biA == bi { + tw += wt d := ContactADelta(ci) td = td.Add(d) a := ContactAAngDelta(ci) ta = ta.Add(a) } if biB == bi { + tw += wt d := ContactBDelta(ci) td = td.Add(d) a := ContactBAngDelta(ci) ta = ta.Add(a) } } - SetDynamicDelta(di, params.Next, td) - SetDynamicAngDelta(di, params.Next, ta) + v0 := DynamicDelta(di, params.Next) + w0 := DynamicAngDelta(di, params.Next) + // fmt.Println(params.Next, "contact:", v0, td) + + SetDynamicDelta(di, params.Next, td.Add(v0)) + SetDynamicAngDelta(di, params.Next, ta.Add(w0)) + Dynamics[di, params.Next, DynContactWeight] = tw } func ContactConstraint(err float32, q0A, q0B math32.Quat, mInvA, mInvB float32, iInvA, iInvB math32.Matrix3, linA, linB, angA, angB math32.Vector3, relaxation, dt float32) float32 { diff --git a/physics/dynamics.go b/physics/dynamics.go index 7620c019..5abfe9a9 100644 --- a/physics/dynamics.go +++ b/physics/dynamics.go @@ -71,6 +71,9 @@ const ( DynAngDeltaX DynAngDeltaY DynAngDeltaZ + + // integrated weight of all contacts + DynContactWeight ) // cni = current / next index @@ -191,6 +194,7 @@ func SetDynamicAngDelta(idx, cni int32, angDelta math32.Vector3) { // SetMass sets the mass of given body object (only relevant for dynamics), // including a default inertia tensor based on solid shape of given size. func (wl *World) SetMass(idx int32, shape Shapes, size math32.Vector3, mass float32) { + Bodies.Set(shape.Radius(size), int(idx), int(BodyRadius)) Bodies.Set(mass, int(idx), int(BodyMass)) invm := mass if mass > 0 { diff --git a/physics/dynamics.goal b/physics/dynamics.goal index 22694d90..4c4823f3 100644 --- a/physics/dynamics.goal +++ b/physics/dynamics.goal @@ -69,6 +69,9 @@ const ( DynAngDeltaX DynAngDeltaY DynAngDeltaZ + + // integrated weight of all contacts + DynContactWeight ) // cni = current / next index @@ -189,6 +192,7 @@ func SetDynamicAngDelta(idx, cni int32, angDelta math32.Vector3) { // SetMass sets the mass of given body object (only relevant for dynamics), // including a default inertia tensor based on solid shape of given size. func (wl *World) SetMass(idx int32, shape Shapes, size math32.Vector3, mass float32) { + Bodies[idx, BodyRadius] = shape.Radius(size) Bodies[idx, BodyMass] = mass invm := mass if mass > 0 { diff --git a/physics/enumgen.go b/physics/enumgen.go index fe25ab53..8e733163 100644 --- a/physics/enumgen.go +++ b/physics/enumgen.go @@ -51,20 +51,20 @@ func (i BodyVars) MarshalText() ([]byte, error) { return []byte(i.String()), nil // UnmarshalText implements the [encoding.TextUnmarshaler] interface. func (i *BodyVars) UnmarshalText(text []byte) error { return enums.UnmarshalText(i, text, "BodyVars") } -var _ContactVarsValues = []ContactVars{0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31} +var _ContactVarsValues = []ContactVars{0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32} // ContactVarsN is the highest valid value for type ContactVars, plus one. // //gosl:start -const ContactVarsN ContactVars = 32 +const ContactVarsN ContactVars = 33 //gosl:end -var _ContactVarsValueMap = map[string]ContactVars{`ContactA`: 0, `ContactB`: 1, `ContactPointIdx`: 2, `ContactAPointX`: 3, `ContactAPointY`: 4, `ContactAPointZ`: 5, `ContactBPointX`: 6, `ContactBPointY`: 7, `ContactBPointZ`: 8, `ContactAOffX`: 9, `ContactAOffY`: 10, `ContactAOffZ`: 11, `ContactBOffX`: 12, `ContactBOffY`: 13, `ContactBOffZ`: 14, `ContactAThick`: 15, `ContactBThick`: 16, `ContactNormX`: 17, `ContactNormY`: 18, `ContactNormZ`: 19, `ContactADeltaX`: 20, `ContactADeltaY`: 21, `ContactADeltaZ`: 22, `ContactAAngDeltaX`: 23, `ContactAAngDeltaY`: 24, `ContactAAngDeltaZ`: 25, `ContactBDeltaX`: 26, `ContactBDeltaY`: 27, `ContactBDeltaZ`: 28, `ContactBAngDeltaX`: 29, `ContactBAngDeltaY`: 30, `ContactBAngDeltaZ`: 31} +var _ContactVarsValueMap = map[string]ContactVars{`ContactA`: 0, `ContactB`: 1, `ContactPointIdx`: 2, `ContactAPointX`: 3, `ContactAPointY`: 4, `ContactAPointZ`: 5, `ContactBPointX`: 6, `ContactBPointY`: 7, `ContactBPointZ`: 8, `ContactAOffX`: 9, `ContactAOffY`: 10, `ContactAOffZ`: 11, `ContactBOffX`: 12, `ContactBOffY`: 13, `ContactBOffZ`: 14, `ContactAThick`: 15, `ContactBThick`: 16, `ContactNormX`: 17, `ContactNormY`: 18, `ContactNormZ`: 19, `ContactWeight`: 20, `ContactADeltaX`: 21, `ContactADeltaY`: 22, `ContactADeltaZ`: 23, `ContactAAngDeltaX`: 24, `ContactAAngDeltaY`: 25, `ContactAAngDeltaZ`: 26, `ContactBDeltaX`: 27, `ContactBDeltaY`: 28, `ContactBDeltaZ`: 29, `ContactBAngDeltaX`: 30, `ContactBAngDeltaY`: 31, `ContactBAngDeltaZ`: 32} -var _ContactVarsDescMap = map[ContactVars]string{0: `first body index`, 1: `the other body index`, 2: `contact point index for A-B pair`, 3: `contact point on body A`, 4: ``, 5: ``, 6: `contact point on body B`, 7: ``, 8: ``, 9: `contact offset on body A`, 10: ``, 11: ``, 12: `contact offset on body B`, 13: ``, 14: ``, 15: `Contact thickness`, 16: ``, 17: `normal pointing from center of B to center of A`, 18: ``, 19: ``, 20: `computed contact deltas, A`, 21: ``, 22: ``, 23: ``, 24: ``, 25: ``, 26: `computed contact deltas, B`, 27: ``, 28: ``, 29: ``, 30: ``, 31: ``} +var _ContactVarsDescMap = map[ContactVars]string{0: `first body index`, 1: `the other body index`, 2: `contact point index for A-B pair`, 3: `contact point on body A`, 4: ``, 5: ``, 6: `contact point on body B`, 7: ``, 8: ``, 9: `contact offset on body A`, 10: ``, 11: ``, 12: `contact offset on body B`, 13: ``, 14: ``, 15: `Contact thickness`, 16: ``, 17: `normal pointing from center of B to center of A`, 18: ``, 19: ``, 20: `contact weighting -- 1 if contact made; for restitution use this to filter contacts when updating body.`, 21: `computed contact deltas, A`, 22: ``, 23: ``, 24: ``, 25: ``, 26: ``, 27: `computed contact deltas, B`, 28: ``, 29: ``, 30: ``, 31: ``, 32: ``} -var _ContactVarsMap = map[ContactVars]string{0: `ContactA`, 1: `ContactB`, 2: `ContactPointIdx`, 3: `ContactAPointX`, 4: `ContactAPointY`, 5: `ContactAPointZ`, 6: `ContactBPointX`, 7: `ContactBPointY`, 8: `ContactBPointZ`, 9: `ContactAOffX`, 10: `ContactAOffY`, 11: `ContactAOffZ`, 12: `ContactBOffX`, 13: `ContactBOffY`, 14: `ContactBOffZ`, 15: `ContactAThick`, 16: `ContactBThick`, 17: `ContactNormX`, 18: `ContactNormY`, 19: `ContactNormZ`, 20: `ContactADeltaX`, 21: `ContactADeltaY`, 22: `ContactADeltaZ`, 23: `ContactAAngDeltaX`, 24: `ContactAAngDeltaY`, 25: `ContactAAngDeltaZ`, 26: `ContactBDeltaX`, 27: `ContactBDeltaY`, 28: `ContactBDeltaZ`, 29: `ContactBAngDeltaX`, 30: `ContactBAngDeltaY`, 31: `ContactBAngDeltaZ`} +var _ContactVarsMap = map[ContactVars]string{0: `ContactA`, 1: `ContactB`, 2: `ContactPointIdx`, 3: `ContactAPointX`, 4: `ContactAPointY`, 5: `ContactAPointZ`, 6: `ContactBPointX`, 7: `ContactBPointY`, 8: `ContactBPointZ`, 9: `ContactAOffX`, 10: `ContactAOffY`, 11: `ContactAOffZ`, 12: `ContactBOffX`, 13: `ContactBOffY`, 14: `ContactBOffZ`, 15: `ContactAThick`, 16: `ContactBThick`, 17: `ContactNormX`, 18: `ContactNormY`, 19: `ContactNormZ`, 20: `ContactWeight`, 21: `ContactADeltaX`, 22: `ContactADeltaY`, 23: `ContactADeltaZ`, 24: `ContactAAngDeltaX`, 25: `ContactAAngDeltaY`, 26: `ContactAAngDeltaZ`, 27: `ContactBDeltaX`, 28: `ContactBDeltaY`, 29: `ContactBDeltaZ`, 30: `ContactBAngDeltaX`, 31: `ContactBAngDeltaY`, 32: `ContactBAngDeltaZ`} // String returns the string representation of this ContactVars value. func (i ContactVars) String() string { return enums.String(i, _ContactVarsMap) } @@ -145,20 +145,20 @@ func (i *JointControlVars) UnmarshalText(text []byte) error { return enums.UnmarshalText(i, text, "JointControlVars") } -var _DynamicVarsValues = []DynamicVars{0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31} +var _DynamicVarsValues = []DynamicVars{0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32} // DynamicVarsN is the highest valid value for type DynamicVars, plus one. // //gosl:start -const DynamicVarsN DynamicVars = 32 +const DynamicVarsN DynamicVars = 33 //gosl:end -var _DynamicVarsValueMap = map[string]DynamicVars{`DynBody`: 0, `DynPosX`: 1, `DynPosY`: 2, `DynPosZ`: 3, `DynQuatX`: 4, `DynQuatY`: 5, `DynQuatZ`: 6, `DynQuatW`: 7, `DynVelX`: 8, `DynVelY`: 9, `DynVelZ`: 10, `DynAngVelX`: 11, `DynAngVelY`: 12, `DynAngVelZ`: 13, `DynAccX`: 14, `DynAccY`: 15, `DynAccZ`: 16, `DynAngAccX`: 17, `DynAngAccY`: 18, `DynAngAccZ`: 19, `DynForceX`: 20, `DynForceY`: 21, `DynForceZ`: 22, `DynTorqueX`: 23, `DynTorqueY`: 24, `DynTorqueZ`: 25, `DynDeltaX`: 26, `DynDeltaY`: 27, `DynDeltaZ`: 28, `DynAngDeltaX`: 29, `DynAngDeltaY`: 30, `DynAngDeltaZ`: 31} +var _DynamicVarsValueMap = map[string]DynamicVars{`DynBody`: 0, `DynPosX`: 1, `DynPosY`: 2, `DynPosZ`: 3, `DynQuatX`: 4, `DynQuatY`: 5, `DynQuatZ`: 6, `DynQuatW`: 7, `DynVelX`: 8, `DynVelY`: 9, `DynVelZ`: 10, `DynAngVelX`: 11, `DynAngVelY`: 12, `DynAngVelZ`: 13, `DynAccX`: 14, `DynAccY`: 15, `DynAccZ`: 16, `DynAngAccX`: 17, `DynAngAccY`: 18, `DynAngAccZ`: 19, `DynForceX`: 20, `DynForceY`: 21, `DynForceZ`: 22, `DynTorqueX`: 23, `DynTorqueY`: 24, `DynTorqueZ`: 25, `DynDeltaX`: 26, `DynDeltaY`: 27, `DynDeltaZ`: 28, `DynAngDeltaX`: 29, `DynAngDeltaY`: 30, `DynAngDeltaZ`: 31, `DynContactWeight`: 32} -var _DynamicVarsDescMap = map[DynamicVars]string{0: `Index of body in list of bodies.`, 1: `3D position of structural center.`, 2: ``, 3: ``, 4: `Quaternion rotation.`, 5: ``, 6: ``, 7: ``, 8: `Linear velocity.`, 9: ``, 10: ``, 11: `Angular velocity.`, 12: ``, 13: ``, 14: `Linear acceleration.`, 15: ``, 16: ``, 17: `Angular acceleration due to applied torques.`, 18: ``, 19: ``, 20: `Linear force driving linear acceleration (from joints, etc).`, 21: ``, 22: ``, 23: `Torque driving angular acceleration (from joints, etc).`, 24: ``, 25: ``, 26: `Linear deltas.`, 27: ``, 28: ``, 29: `Angular deltas.`, 30: ``, 31: ``} +var _DynamicVarsDescMap = map[DynamicVars]string{0: `Index of body in list of bodies.`, 1: `3D position of structural center.`, 2: ``, 3: ``, 4: `Quaternion rotation.`, 5: ``, 6: ``, 7: ``, 8: `Linear velocity.`, 9: ``, 10: ``, 11: `Angular velocity.`, 12: ``, 13: ``, 14: `Linear acceleration.`, 15: ``, 16: ``, 17: `Angular acceleration due to applied torques.`, 18: ``, 19: ``, 20: `Linear force driving linear acceleration (from joints, etc).`, 21: ``, 22: ``, 23: `Torque driving angular acceleration (from joints, etc).`, 24: ``, 25: ``, 26: `Linear deltas.`, 27: ``, 28: ``, 29: `Angular deltas.`, 30: ``, 31: ``, 32: `integrated weight of all contacts`} -var _DynamicVarsMap = map[DynamicVars]string{0: `DynBody`, 1: `DynPosX`, 2: `DynPosY`, 3: `DynPosZ`, 4: `DynQuatX`, 5: `DynQuatY`, 6: `DynQuatZ`, 7: `DynQuatW`, 8: `DynVelX`, 9: `DynVelY`, 10: `DynVelZ`, 11: `DynAngVelX`, 12: `DynAngVelY`, 13: `DynAngVelZ`, 14: `DynAccX`, 15: `DynAccY`, 16: `DynAccZ`, 17: `DynAngAccX`, 18: `DynAngAccY`, 19: `DynAngAccZ`, 20: `DynForceX`, 21: `DynForceY`, 22: `DynForceZ`, 23: `DynTorqueX`, 24: `DynTorqueY`, 25: `DynTorqueZ`, 26: `DynDeltaX`, 27: `DynDeltaY`, 28: `DynDeltaZ`, 29: `DynAngDeltaX`, 30: `DynAngDeltaY`, 31: `DynAngDeltaZ`} +var _DynamicVarsMap = map[DynamicVars]string{0: `DynBody`, 1: `DynPosX`, 2: `DynPosY`, 3: `DynPosZ`, 4: `DynQuatX`, 5: `DynQuatY`, 6: `DynQuatZ`, 7: `DynQuatW`, 8: `DynVelX`, 9: `DynVelY`, 10: `DynVelZ`, 11: `DynAngVelX`, 12: `DynAngVelY`, 13: `DynAngVelZ`, 14: `DynAccX`, 15: `DynAccY`, 16: `DynAccZ`, 17: `DynAngAccX`, 18: `DynAngAccY`, 19: `DynAngAccZ`, 20: `DynForceX`, 21: `DynForceY`, 22: `DynForceZ`, 23: `DynTorqueX`, 24: `DynTorqueY`, 25: `DynTorqueZ`, 26: `DynDeltaX`, 27: `DynDeltaY`, 28: `DynDeltaZ`, 29: `DynAngDeltaX`, 30: `DynAngDeltaY`, 31: `DynAngDeltaZ`, 32: `DynContactWeight`} // String returns the string representation of this DynamicVars value. func (i DynamicVars) String() string { return enums.String(i, _DynamicVarsMap) } diff --git a/physics/examples/test1/test1.go b/physics/examples/test1/test1.go index 0726e266..d0d71107 100644 --- a/physics/examples/test1/test1.go +++ b/physics/examples/test1/test1.go @@ -7,6 +7,9 @@ package main //go:generate core generate import ( + "fmt" + "time" + "cogentcore.org/core/colors" "cogentcore.org/core/core" "cogentcore.org/core/events" @@ -66,14 +69,15 @@ func main() { // physics.SetJointTargetPos(bj, 0, 1) // physics.SetJointDoF(bj, 0, physics.JointDamp, 0.01) // physics.SetJointDoF(bj, 0, physics.JointStiff, 1) // this makes a big difference - bj := wl.NewJointFree(-1, b1.DynamicIndex, math32.Vec3(0, 0, 0), math32.Vec3(0, -height, 0)) - _ = bj + // bj := wl.NewJointFree(-1, b1.DynamicIndex, math32.Vec3(0, 0, 0), math32.Vec3(0, -height, 0)) + // _ = bj wr.Init(wl) params := physics.GetParams(0) - params.Dt = 0.05 - params.Gravity.Y = 0 + params.Dt = 0.01 + // params.Gravity.Y = 0 + fmt.Println(params.ContactRelax) wr.Update() @@ -90,6 +94,31 @@ func main() { sc.SaveCamera("1") sc.SaveCamera("default") + stepNButton := func(p *tree.Plan, n int) { + nm := fmt.Sprintf("Step %d", n) + tree.AddAt(p, nm, func(w *core.Button) { + w.SetText(nm).SetIcon(icons.PlayArrow). + SetTooltip(fmt.Sprintf("Step state %d times", n)). + OnClick(func(e events.Event) { + go func() { + for range n { + wl.Step() + wr.Update() + if se.IsVisible() { + se.AsyncLock() + se.NeedsRender() + se.AsyncUnlock() + time.Sleep(10 * time.Millisecond) + } + } + }() + }) + w.Styler(func(s *styles.Style) { + s.SetAbilities(true, abilities.RepeatClickable) + }) + }) + } + b.AddTopBar(func(bar *core.Frame) { core.NewToolbar(bar).Maker(func(p *tree.Plan) { tree.Add(p, func(w *core.Button) { @@ -103,20 +132,9 @@ func main() { } }) }) - tree.Add(p, func(w *core.Button) { - w.SetText("Step").SetIcon(icons.PlayArrow). - SetTooltip("Step state"). - OnClick(func(e events.Event) { - wl.Step() - wr.Update() - if se.IsVisible() { - se.NeedsRender() - } - }) - w.Styler(func(s *styles.Style) { - s.SetAbilities(true, abilities.RepeatClickable) - }) - }) + stepNButton(p, 1) + stepNButton(p, 10) + stepNButton(p, 100) }) }) b.RunMainWindow() diff --git a/physics/shaders/CollisionBroad.wgsl b/physics/shaders/CollisionBroad.wgsl index a39bbe92..d7691cfe 100644 --- a/physics/shaders/CollisionBroad.wgsl +++ b/physics/shaders/CollisionBroad.wgsl @@ -138,18 +138,19 @@ const ContactBThick: ContactVars = 16; const ContactNormX: ContactVars = 17; const ContactNormY: ContactVars = 18; const ContactNormZ: ContactVars = 19; -const ContactADeltaX: ContactVars = 20; -const ContactADeltaY: ContactVars = 21; -const ContactADeltaZ: ContactVars = 22; -const ContactAAngDeltaX: ContactVars = 23; -const ContactAAngDeltaY: ContactVars = 24; -const ContactAAngDeltaZ: ContactVars = 25; -const ContactBDeltaX: ContactVars = 26; -const ContactBDeltaY: ContactVars = 27; -const ContactBDeltaZ: ContactVars = 28; -const ContactBAngDeltaX: ContactVars = 29; -const ContactBAngDeltaY: ContactVars = 30; -const ContactBAngDeltaZ: ContactVars = 31; +const ContactWeight: ContactVars = 20; +const ContactADeltaX: ContactVars = 21; +const ContactADeltaY: ContactVars = 22; +const ContactADeltaZ: ContactVars = 23; +const ContactAAngDeltaX: ContactVars = 24; +const ContactAAngDeltaY: ContactVars = 25; +const ContactAAngDeltaZ: ContactVars = 26; +const ContactBDeltaX: ContactVars = 27; +const ContactBDeltaY: ContactVars = 28; +const ContactBDeltaZ: ContactVars = 29; +const ContactBAngDeltaX: ContactVars = 30; +const ContactBAngDeltaY: ContactVars = 31; +const ContactBAngDeltaZ: ContactVars = 32; const BroadContactVarsN = ContactAPointX; fn SetBroadContactA(idx: i32,bodIdx: i32) { BroadContacts[Index2D(TensorStrides[70], TensorStrides[71], u32(idx), u32(ContactA))] = bitcast(u32(bodIdx)); @@ -258,6 +259,7 @@ const DynDeltaZ: DynamicVars = 28; const DynAngDeltaX: DynamicVars = 29; const DynAngDeltaY: DynamicVars = 30; const DynAngDeltaZ: DynamicVars = 31; +const DynContactWeight: DynamicVars = 32; fn DynamicPos(idx: i32,cni: i32) -> vec3 { return vec3(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosZ))]); } @@ -267,9 +269,9 @@ fn DynamicQuat(idx: i32,cni: i32) -> vec4 { //////// import: "enumgen.go" const BodyVarsN: BodyVars = 43; -const ContactVarsN: ContactVars = 32; +const ContactVarsN: ContactVars = 33; const JointControlVarsN: JointControlVars = 3; -const DynamicVarsN: DynamicVars = 32; +const DynamicVarsN: DynamicVars = 33; const GPUVarsN: GPUVars = 12; const JointTypesN: JointTypes = 7; const JointVarsN: JointVars = 50; @@ -393,10 +395,12 @@ struct GeomData { fn ClosestPointPlane(width: f32,length: f32, pt: vec3) -> vec3 { var cp = pt; if (width == 0.0) { - cp.y = f32(0);return cp; + cp.y = f32(0); + return cp; } cp.x = clamp(pt.x, -width, width); - cp.z = clamp(pt.z, -length, length);return cp; + cp.z = clamp(pt.z, -length, length); +return cp; } //////// import: "shapes.go" @@ -475,10 +479,12 @@ fn ShapePairContacts(a: Shapes,b: Shapes, infPlane: bool, ba: ptr) //////// import: "slmath-quaternion.go" fn MulQuatVector(q: vec4, v: vec3) -> vec3 { var xyz = vec3(q.x, q.y, q.z); - var t = Cross3(xyz, v)*(2);return v+(t*(q.w))+(Cross3(xyz, t)); + var t = Cross3(xyz, v)*(2); +return v+(t*(q.w))+(Cross3(xyz, t)); } fn MulSpatialPoint(xP: vec3, xQ: vec4, p: vec3) -> vec3 { - var dp = MulQuatVector(xQ, p);return dp+(xP); + var dp = MulQuatVector(xQ, p); +return dp+(xP); } //////// import: "slmath-vector2.go" diff --git a/physics/shaders/CollisionInit.wgsl b/physics/shaders/CollisionInit.wgsl index 55dd83d8..e5453a00 100644 --- a/physics/shaders/CollisionInit.wgsl +++ b/physics/shaders/CollisionInit.wgsl @@ -95,18 +95,19 @@ const ContactBThick: ContactVars = 16; const ContactNormX: ContactVars = 17; const ContactNormY: ContactVars = 18; const ContactNormZ: ContactVars = 19; -const ContactADeltaX: ContactVars = 20; -const ContactADeltaY: ContactVars = 21; -const ContactADeltaZ: ContactVars = 22; -const ContactAAngDeltaX: ContactVars = 23; -const ContactAAngDeltaY: ContactVars = 24; -const ContactAAngDeltaZ: ContactVars = 25; -const ContactBDeltaX: ContactVars = 26; -const ContactBDeltaY: ContactVars = 27; -const ContactBDeltaZ: ContactVars = 28; -const ContactBAngDeltaX: ContactVars = 29; -const ContactBAngDeltaY: ContactVars = 30; -const ContactBAngDeltaZ: ContactVars = 31; +const ContactWeight: ContactVars = 20; +const ContactADeltaX: ContactVars = 21; +const ContactADeltaY: ContactVars = 22; +const ContactADeltaZ: ContactVars = 23; +const ContactAAngDeltaX: ContactVars = 24; +const ContactAAngDeltaY: ContactVars = 25; +const ContactAAngDeltaZ: ContactVars = 26; +const ContactBDeltaX: ContactVars = 27; +const ContactBDeltaY: ContactVars = 28; +const ContactBDeltaZ: ContactVars = 29; +const ContactBAngDeltaX: ContactVars = 30; +const ContactBAngDeltaY: ContactVars = 31; +const ContactBAngDeltaZ: ContactVars = 32; const BroadContactVarsN = ContactAPointX; fn CollisionInit(i: u32) { //gosl:kernel if (i > 0) { @@ -156,12 +157,13 @@ const DynDeltaZ: DynamicVars = 28; const DynAngDeltaX: DynamicVars = 29; const DynAngDeltaY: DynamicVars = 30; const DynAngDeltaZ: DynamicVars = 31; +const DynContactWeight: DynamicVars = 32; //////// import: "enumgen.go" const BodyVarsN: BodyVars = 43; -const ContactVarsN: ContactVars = 32; +const ContactVarsN: ContactVars = 33; const JointControlVarsN: JointControlVars = 3; -const DynamicVarsN: DynamicVars = 32; +const DynamicVarsN: DynamicVars = 33; const GPUVarsN: GPUVars = 12; const JointTypesN: JointTypes = 7; const JointVarsN: JointVars = 50; diff --git a/physics/shaders/CollisionNarrow.wgsl b/physics/shaders/CollisionNarrow.wgsl index 1c70a718..9e62c01c 100644 --- a/physics/shaders/CollisionNarrow.wgsl +++ b/physics/shaders/CollisionNarrow.wgsl @@ -140,18 +140,19 @@ const ContactBThick: ContactVars = 16; const ContactNormX: ContactVars = 17; const ContactNormY: ContactVars = 18; const ContactNormZ: ContactVars = 19; -const ContactADeltaX: ContactVars = 20; -const ContactADeltaY: ContactVars = 21; -const ContactADeltaZ: ContactVars = 22; -const ContactAAngDeltaX: ContactVars = 23; -const ContactAAngDeltaY: ContactVars = 24; -const ContactAAngDeltaZ: ContactVars = 25; -const ContactBDeltaX: ContactVars = 26; -const ContactBDeltaY: ContactVars = 27; -const ContactBDeltaZ: ContactVars = 28; -const ContactBAngDeltaX: ContactVars = 29; -const ContactBAngDeltaY: ContactVars = 30; -const ContactBAngDeltaZ: ContactVars = 31; +const ContactWeight: ContactVars = 20; +const ContactADeltaX: ContactVars = 21; +const ContactADeltaY: ContactVars = 22; +const ContactADeltaZ: ContactVars = 23; +const ContactAAngDeltaX: ContactVars = 24; +const ContactAAngDeltaY: ContactVars = 25; +const ContactAAngDeltaZ: ContactVars = 26; +const ContactBDeltaX: ContactVars = 27; +const ContactBDeltaY: ContactVars = 28; +const ContactBDeltaZ: ContactVars = 29; +const ContactBAngDeltaX: ContactVars = 30; +const ContactBAngDeltaY: ContactVars = 31; +const ContactBAngDeltaZ: ContactVars = 32; const BroadContactVarsN = ContactAPointX; fn GetBroadContactA(idx: i32) -> i32 { return i32(bitcast(BroadContacts[Index2D(TensorStrides[70], TensorStrides[71], u32(idx), u32(ContactA))])); @@ -347,6 +348,7 @@ const DynDeltaZ: DynamicVars = 28; const DynAngDeltaX: DynamicVars = 29; const DynAngDeltaY: DynamicVars = 30; const DynAngDeltaZ: DynamicVars = 31; +const DynContactWeight: DynamicVars = 32; fn DynamicPos(idx: i32,cni: i32) -> vec3 { return vec3(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosZ))]); } @@ -356,9 +358,9 @@ fn DynamicQuat(idx: i32,cni: i32) -> vec4 { //////// import: "enumgen.go" const BodyVarsN: BodyVars = 43; -const ContactVarsN: ContactVars = 32; +const ContactVarsN: ContactVars = 33; const JointControlVarsN: JointControlVars = 3; -const DynamicVarsN: DynamicVars = 32; +const DynamicVarsN: DynamicVars = 33; const GPUVarsN: GPUVars = 12; const JointTypesN: JointTypes = 7; const JointVarsN: JointVars = 50; @@ -512,7 +514,8 @@ fn ContactPoints(dist: f32,margin: f32, gdA: ptr, gdB: ptr, gdB: ptr, pA: ptr>,pB: ptr>,norm: ptr>) -> f32 { var pAw = (*gdA).WbR; @@ -520,7 +523,8 @@ fn ColSphereSphere(cpi: i32,maxIter: i32, gdA: ptr, gdB: ptr< var diff = pAw-(pBw); *pA = pAw; *pB = pBw; - *norm = Normal3(diff);return Dot3(diff, *norm); + *norm = Normal3(diff); +return Dot3(diff, *norm); } fn ColCapsulePlane(cpi: i32,maxIter: i32, gdA: ptr, gdB: ptr, pA: ptr>,pB: ptr>,norm: ptr>) -> f32 { var pAw: vec3; @@ -556,7 +560,8 @@ fn ColCapsulePlane(cpi: i32,maxIter: i32, gdA: ptr, gdB: ptr< *norm = MulQuatVector((*gdB).WbQ, vec3(0, 1, 0)); } *pA = pAw; - *pB = pBw;return Dot3(diff, *norm); + *pB = pBw; +return Dot3(diff, *norm); } fn ColCapsuleCapsule(cpi: i32,maxIter: i32, gdA: ptr, gdB: ptr, pA: ptr>,pB: ptr>,norm: ptr>) -> f32 { var hhA = (*gdA).Size.y; @@ -575,7 +580,8 @@ fn ColCapsuleCapsule(cpi: i32,maxIter: i32, gdA: ptr, gdB: pt var diff = pAw-(pBw); *norm = Normal3(diff); *pA = pAw; - *pB = pBw;return Dot3(diff, *norm); + *pB = pBw; +return Dot3(diff, *norm); } fn ColBoxBox(cpi: i32,maxIter: i32, gdA: ptr, gdB: ptr, pA: ptr>,pB: ptr>,norm: ptr>) -> f32 { var edge0: vec3; @@ -593,7 +599,8 @@ fn ColBoxBox(cpi: i32,maxIter: i32, gdA: ptr, gdB: ptr, gdB: ptr, pA: ptr>,pB: ptr>,norm: ptr>) -> f32 { var hhB = (*gdB).Size.y; @@ -611,7 +618,8 @@ fn ColBoxCapsule(cpi: i32,maxIter: i32, gdA: ptr, gdB: ptr, gdB: ptr, pA: ptr>,pB: ptr>,norm: ptr>) -> f32 { var width = (*gdB).Size.x; @@ -661,7 +669,8 @@ fn ColBoxPlane(cpi: i32,maxIter: i32, gdA: ptr, gdB: ptr, gdB: ptr, pA: ptr>,pB: ptr>,norm: ptr>) -> f32 { var pAw = (*gdA).WbR; @@ -671,7 +680,8 @@ fn ColSphereBox(cpi: i32,maxIter: i32, gdA: ptr, gdB: ptr, gdB: ptr, pA: ptr>,pB: ptr>,norm: ptr>) -> f32 { var pAw = (*gdA).WbR; @@ -682,7 +692,8 @@ fn ColSphereCapsule(cpi: i32,maxIter: i32, gdA: ptr, gdB: ptr var diff = pAw-(pBw); *norm = Normal3(diff); *pA = pAw; - *pB = pBw;return Dot3(diff, *norm); + *pB = pBw; +return Dot3(diff, *norm); } fn ColSpherePlane(cpi: i32,maxIter: i32, gdA: ptr, gdB: ptr, pA: ptr>,pB: ptr>,norm: ptr>) -> f32 { var pAw = (*gdA).WbR; @@ -691,7 +702,8 @@ fn ColSpherePlane(cpi: i32,maxIter: i32, gdA: ptr, gdB: ptr(0.0, 0.0, 1.0)); *pA = pAw; - *pB = pBw;return Dot3(diff, *norm); + *pB = pBw; +return Dot3(diff, *norm); } fn ColCylinderPlane(cpi: i32,maxIter: i32, gdA: ptr, gdB: ptr, pA: ptr>,pB: ptr>,norm: ptr>) -> f32 { var plNorm = MulQuatVector((*gdB).WbQ, vec3(0.0, 1.0, 0.0)); @@ -738,7 +750,8 @@ fn ColCylinderPlane(cpi: i32,maxIter: i32, gdA: ptr, gdB: ptr } } *pA = pos+(n*(dist * 0.5)); - *pB = pos-(n*(dist * 0.5));return dist; + *pB = pos-(n*(dist * 0.5)); +return dist; } //////// import: "shapegeom.go" @@ -746,7 +759,8 @@ fn BoxSDF(upper: vec3,p: vec3) -> f32 { var qx = abs(p.x) - upper.x; var qy = abs(p.y) - upper.y; var qz = abs(p.z) - upper.z; - var e = vec3(max(qx, 0.0), max(qy, 0.0), max(qz, 0.0));return Length3(e) + min(max(qx, max(qy, qz)), 0.0); + var e = vec3(max(qx, 0.0), max(qy, 0.0), max(qz, 0.0)); +return Length3(e) + min(max(qx, max(qy, qz)), 0.0); } fn BoxSDFGrad(upper: vec3,p: vec3) -> vec3 { var qx = abs(p.x) - upper.x; @@ -755,7 +769,8 @@ fn BoxSDFGrad(upper: vec3,p: vec3) -> vec3 { if (qx > 0.0 || qy > 0.0 || qz > 0.0) { var x = clamp(p.x, -upper.x, upper.x); var y = clamp(p.y, -upper.y, upper.y); - var z = clamp(p.z, -upper.z, upper.z);return Normal3(p-(vec3(x, y, z))); + var z = clamp(p.z, -upper.z, upper.z); + return Normal3(p-(vec3(x, y, z))); } var sx = sign(p.x); var sy = sign(p.y); @@ -770,21 +785,25 @@ fn BoxSDFGrad(upper: vec3,p: vec3) -> vec3 { } fn CylinderSDF(radius: f32,hh: f32, p: vec3) -> f32 { var dx = Length3(vec3(p.x, 0.0, p.z)) - radius; - var dy = abs(p.y) - hh;return min(max(dx, dy), 0.0) + Length2(vec2(max(dx, 0.0), max(dy, 0.0))); + var dy = abs(p.y) - hh; +return min(max(dx, dy), 0.0) + Length2(vec2(max(dx, 0.0), max(dy, 0.0))); } fn ClosestPointPlane(width: f32,length: f32, pt: vec3) -> vec3 { var cp = pt; if (width == 0.0) { - cp.y = f32(0);return cp; + cp.y = f32(0); + return cp; } cp.x = clamp(pt.x, -width, width); - cp.z = clamp(pt.z, -length, length);return cp; + cp.z = clamp(pt.z, -length, length); +return cp; } fn ClosestPointLineSegment(a: vec3,b: vec3,pt: vec3) -> vec3 { var ab = b-(a); var ap = pt-(a); var t = Dot3(ap, ab) / Dot3(ab, ab); - t = clamp(t, 0.0, 1.0);return a+(ab*(t)); + t = clamp(t, 0.0, 1.0); +return a+(ab*(t)); } fn ClosestPointBox(upper: vec3,pt: vec3) -> vec3 { var x = clamp(pt.x, -upper.x, upper.x); @@ -806,7 +825,8 @@ fn ClosestPointBox(upper: vec3,pt: vec3) -> vec3 { fn BoxVertex(ptId: i32, upper: vec3) -> vec3 { var sign_x = f32(ptId%2)*2.0 - 1.0; var sign_y = f32((ptId/2)%2)*2.0 - 1.0; - var sign_z = f32((ptId/4)%2)*2.0 - 1.0;return vec3(sign_x*upper.x, sign_y*upper.y, sign_z*upper.z); + var sign_z = f32((ptId/4)%2)*2.0 - 1.0; +return vec3(sign_x*upper.x, sign_y*upper.y, sign_z*upper.z); } fn BoxEdge(edgeId: i32, upper: vec3, edge0: ptr>,edge1: ptr>) { var eid = edgeId; @@ -947,10 +967,12 @@ fn QuatNormalize(q: vec4) -> vec4 { } fn MulQuatVector(q: vec4, v: vec3) -> vec3 { var xyz = vec3(q.x, q.y, q.z); - var t = Cross3(xyz, v)*(2);return v+(t*(q.w))+(Cross3(xyz, t)); + var t = Cross3(xyz, v)*(2); +return v+(t*(q.w))+(Cross3(xyz, t)); } fn MulSpatialPoint(xP: vec3, xQ: vec4, p: vec3) -> vec3 { - var dp = MulQuatVector(xQ, p);return dp+(xP); + var dp = MulQuatVector(xQ, p); +return dp+(xP); } fn SpatialTransformInverse(p: vec3, q: vec4, oP: ptr>, oQ: ptr>) { var qi = QuatInverse(q); @@ -961,7 +983,8 @@ fn QuatInverse(q: vec4) -> vec4 { var nq = q; nq.x *= f32(-1); nq.y *= f32(-1); - nq.z *= f32(-1);return QuatNormalize(nq); + nq.z *= f32(-1); +return QuatNormalize(nq); } //////// import: "slmath-vector2.go" diff --git a/physics/shaders/DeltasFromContacts.wgsl b/physics/shaders/DeltasFromContacts.wgsl index 7581c18b..0d008198 100644 --- a/physics/shaders/DeltasFromContacts.wgsl +++ b/physics/shaders/DeltasFromContacts.wgsl @@ -107,18 +107,19 @@ const ContactBThick: ContactVars = 16; const ContactNormX: ContactVars = 17; const ContactNormY: ContactVars = 18; const ContactNormZ: ContactVars = 19; -const ContactADeltaX: ContactVars = 20; -const ContactADeltaY: ContactVars = 21; -const ContactADeltaZ: ContactVars = 22; -const ContactAAngDeltaX: ContactVars = 23; -const ContactAAngDeltaY: ContactVars = 24; -const ContactAAngDeltaZ: ContactVars = 25; -const ContactBDeltaX: ContactVars = 26; -const ContactBDeltaY: ContactVars = 27; -const ContactBDeltaZ: ContactVars = 28; -const ContactBAngDeltaX: ContactVars = 29; -const ContactBAngDeltaY: ContactVars = 30; -const ContactBAngDeltaZ: ContactVars = 31; +const ContactWeight: ContactVars = 20; +const ContactADeltaX: ContactVars = 21; +const ContactADeltaY: ContactVars = 22; +const ContactADeltaZ: ContactVars = 23; +const ContactAAngDeltaX: ContactVars = 24; +const ContactAAngDeltaY: ContactVars = 25; +const ContactAAngDeltaZ: ContactVars = 26; +const ContactBDeltaX: ContactVars = 27; +const ContactBDeltaY: ContactVars = 28; +const ContactBDeltaZ: ContactVars = 29; +const ContactBAngDeltaX: ContactVars = 30; +const ContactBAngDeltaY: ContactVars = 31; +const ContactBAngDeltaZ: ContactVars = 32; const BroadContactVarsN = ContactAPointX; fn GetContactA(idx: i32) -> i32 { return i32(bitcast(Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactA))])); } fn GetContactB(idx: i32) -> i32 { return i32(bitcast(Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactB))])); } @@ -144,16 +145,23 @@ fn DeltasFromContacts(i: u32) { //gosl:kernel var bi = DynamicBody(di); var td = DynamicDelta(di, params.Next); var ta = DynamicAngDelta(di, params.Next); + var tw = f32(0); for (var ci=0; ci i32 { - return i32(bitcast(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(0), u32(DynBody))])); -} +const DynContactWeight: DynamicVars = 32; +fn DynamicBody(idx: i32) -> i32 { return i32(bitcast(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(0), u32(DynBody))])); } fn DynamicDelta(idx: i32,cni: i32) -> vec3 { return vec3(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaZ))]); } @@ -226,9 +234,9 @@ fn SetDynamicAngDelta(idx: i32,cni: i32, angDelta: vec3) { //////// import: "enumgen.go" const BodyVarsN: BodyVars = 43; -const ContactVarsN: ContactVars = 32; +const ContactVarsN: ContactVars = 33; const JointControlVarsN: JointControlVars = 3; -const DynamicVarsN: DynamicVars = 32; +const DynamicVarsN: DynamicVars = 33; const GPUVarsN: GPUVars = 12; const JointTypesN: JointTypes = 7; const JointVarsN: JointVars = 50; diff --git a/physics/shaders/DeltasFromJoints.wgsl b/physics/shaders/DeltasFromJoints.wgsl index 31e7a5b8..26e8e0cc 100644 --- a/physics/shaders/DeltasFromJoints.wgsl +++ b/physics/shaders/DeltasFromJoints.wgsl @@ -103,18 +103,19 @@ const ContactBThick: ContactVars = 16; const ContactNormX: ContactVars = 17; const ContactNormY: ContactVars = 18; const ContactNormZ: ContactVars = 19; -const ContactADeltaX: ContactVars = 20; -const ContactADeltaY: ContactVars = 21; -const ContactADeltaZ: ContactVars = 22; -const ContactAAngDeltaX: ContactVars = 23; -const ContactAAngDeltaY: ContactVars = 24; -const ContactAAngDeltaZ: ContactVars = 25; -const ContactBDeltaX: ContactVars = 26; -const ContactBDeltaY: ContactVars = 27; -const ContactBDeltaZ: ContactVars = 28; -const ContactBAngDeltaX: ContactVars = 29; -const ContactBAngDeltaY: ContactVars = 30; -const ContactBAngDeltaZ: ContactVars = 31; +const ContactWeight: ContactVars = 20; +const ContactADeltaX: ContactVars = 21; +const ContactADeltaY: ContactVars = 22; +const ContactADeltaZ: ContactVars = 23; +const ContactAAngDeltaX: ContactVars = 24; +const ContactAAngDeltaY: ContactVars = 25; +const ContactAAngDeltaZ: ContactVars = 26; +const ContactBDeltaX: ContactVars = 27; +const ContactBDeltaY: ContactVars = 28; +const ContactBDeltaZ: ContactVars = 29; +const ContactBAngDeltaX: ContactVars = 30; +const ContactBAngDeltaY: ContactVars = 31; +const ContactBAngDeltaZ: ContactVars = 32; const BroadContactVarsN = ContactAPointX; //////// import: "control.go" @@ -157,6 +158,7 @@ const DynDeltaZ: DynamicVars = 28; const DynAngDeltaX: DynamicVars = 29; const DynAngDeltaY: DynamicVars = 30; const DynAngDeltaZ: DynamicVars = 31; +const DynContactWeight: DynamicVars = 32; fn SetDynamicDelta(idx: i32,cni: i32, delta: vec3) { Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaX))] = delta.x; Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaY))] = delta.y; @@ -170,9 +172,9 @@ fn SetDynamicAngDelta(idx: i32,cni: i32, angDelta: vec3) { //////// import: "enumgen.go" const BodyVarsN: BodyVars = 43; -const ContactVarsN: ContactVars = 32; +const ContactVarsN: ContactVars = 33; const JointControlVarsN: JointControlVars = 3; -const DynamicVarsN: DynamicVars = 32; +const DynamicVarsN: DynamicVars = 33; const GPUVarsN: GPUVars = 12; const JointTypesN: JointTypes = 7; const JointVarsN: JointVars = 50; @@ -336,14 +338,16 @@ fn DeltasFromJoints(i: u32) { //gosl:kernel var nc = BodyJoints[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(di), u32(1), u32(0))]; var td = vec3(0, 0, 0); var ta = vec3(0, 0, 0); - for (var i = i32(1); i <= np; i++) { + for (var i = i32(1); + i <= np; i++) { var ji = BodyJoints[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(di), u32(0), u32(i))]; var d = JointPDelta(ji); td = td+(d); var a = JointPAngDelta(ji); ta = ta+(a); } - for (var i = i32(1); i <= nc; i++) { + for (var i = i32(1); + i <= nc; i++) { var ji = BodyJoints[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(di), u32(1), u32(i))]; var d = JointCDelta(ji); td = td+(d); diff --git a/physics/shaders/DynamicsCurToNext.wgsl b/physics/shaders/DynamicsCurToNext.wgsl index 3f828a06..261e27e2 100644 --- a/physics/shaders/DynamicsCurToNext.wgsl +++ b/physics/shaders/DynamicsCurToNext.wgsl @@ -95,18 +95,19 @@ const ContactBThick: ContactVars = 16; const ContactNormX: ContactVars = 17; const ContactNormY: ContactVars = 18; const ContactNormZ: ContactVars = 19; -const ContactADeltaX: ContactVars = 20; -const ContactADeltaY: ContactVars = 21; -const ContactADeltaZ: ContactVars = 22; -const ContactAAngDeltaX: ContactVars = 23; -const ContactAAngDeltaY: ContactVars = 24; -const ContactAAngDeltaZ: ContactVars = 25; -const ContactBDeltaX: ContactVars = 26; -const ContactBDeltaY: ContactVars = 27; -const ContactBDeltaZ: ContactVars = 28; -const ContactBAngDeltaX: ContactVars = 29; -const ContactBAngDeltaY: ContactVars = 30; -const ContactBAngDeltaZ: ContactVars = 31; +const ContactWeight: ContactVars = 20; +const ContactADeltaX: ContactVars = 21; +const ContactADeltaY: ContactVars = 22; +const ContactADeltaZ: ContactVars = 23; +const ContactAAngDeltaX: ContactVars = 24; +const ContactAAngDeltaY: ContactVars = 25; +const ContactAAngDeltaZ: ContactVars = 26; +const ContactBDeltaX: ContactVars = 27; +const ContactBDeltaY: ContactVars = 28; +const ContactBDeltaZ: ContactVars = 29; +const ContactBAngDeltaX: ContactVars = 30; +const ContactBAngDeltaY: ContactVars = 31; +const ContactBAngDeltaZ: ContactVars = 32; const BroadContactVarsN = ContactAPointX; //////// import: "control.go" @@ -149,12 +150,13 @@ const DynDeltaZ: DynamicVars = 28; const DynAngDeltaX: DynamicVars = 29; const DynAngDeltaY: DynamicVars = 30; const DynAngDeltaZ: DynamicVars = 31; +const DynContactWeight: DynamicVars = 32; //////// import: "enumgen.go" const BodyVarsN: BodyVars = 43; -const ContactVarsN: ContactVars = 32; +const ContactVarsN: ContactVars = 33; const JointControlVarsN: JointControlVars = 3; -const DynamicVarsN: DynamicVars = 32; +const DynamicVarsN: DynamicVars = 33; const GPUVarsN: GPUVars = 12; const JointTypesN: JointTypes = 7; const JointVarsN: JointVars = 50; @@ -302,7 +304,8 @@ fn DynamicsCurToNext(i: u32) { //gosl:kernel if (ii >= params.DynamicsN) { return; } - for (var di = DynBody; di < DynamicVarsN; di++) { + for (var di = DynBody; + di < DynamicVarsN; di++) { Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(ii), u32(params.Next), u32(di))] = Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(ii), u32(params.Cur), u32(di))]; } } diff --git a/physics/shaders/ForcesFromJoints.wgsl b/physics/shaders/ForcesFromJoints.wgsl index 948bd12a..33029c24 100644 --- a/physics/shaders/ForcesFromJoints.wgsl +++ b/physics/shaders/ForcesFromJoints.wgsl @@ -103,18 +103,19 @@ const ContactBThick: ContactVars = 16; const ContactNormX: ContactVars = 17; const ContactNormY: ContactVars = 18; const ContactNormZ: ContactVars = 19; -const ContactADeltaX: ContactVars = 20; -const ContactADeltaY: ContactVars = 21; -const ContactADeltaZ: ContactVars = 22; -const ContactAAngDeltaX: ContactVars = 23; -const ContactAAngDeltaY: ContactVars = 24; -const ContactAAngDeltaZ: ContactVars = 25; -const ContactBDeltaX: ContactVars = 26; -const ContactBDeltaY: ContactVars = 27; -const ContactBDeltaZ: ContactVars = 28; -const ContactBAngDeltaX: ContactVars = 29; -const ContactBAngDeltaY: ContactVars = 30; -const ContactBAngDeltaZ: ContactVars = 31; +const ContactWeight: ContactVars = 20; +const ContactADeltaX: ContactVars = 21; +const ContactADeltaY: ContactVars = 22; +const ContactADeltaZ: ContactVars = 23; +const ContactAAngDeltaX: ContactVars = 24; +const ContactAAngDeltaY: ContactVars = 25; +const ContactAAngDeltaZ: ContactVars = 26; +const ContactBDeltaX: ContactVars = 27; +const ContactBDeltaY: ContactVars = 28; +const ContactBDeltaZ: ContactVars = 29; +const ContactBAngDeltaX: ContactVars = 30; +const ContactBAngDeltaY: ContactVars = 31; +const ContactBAngDeltaZ: ContactVars = 32; const BroadContactVarsN = ContactAPointX; //////// import: "control.go" @@ -157,6 +158,7 @@ const DynDeltaZ: DynamicVars = 28; const DynAngDeltaX: DynamicVars = 29; const DynAngDeltaY: DynamicVars = 30; const DynAngDeltaZ: DynamicVars = 31; +const DynContactWeight: DynamicVars = 32; fn SetDynamicForce(idx: i32,cni: i32, force: vec3) { Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynForceX))] = force.x; Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynForceY))] = force.y; @@ -170,9 +172,9 @@ fn SetDynamicTorque(idx: i32,cni: i32, torque: vec3) { //////// import: "enumgen.go" const BodyVarsN: BodyVars = 43; -const ContactVarsN: ContactVars = 32; +const ContactVarsN: ContactVars = 33; const JointControlVarsN: JointControlVars = 3; -const DynamicVarsN: DynamicVars = 32; +const DynamicVarsN: DynamicVars = 33; const GPUVarsN: GPUVars = 12; const JointTypesN: JointTypes = 7; const JointVarsN: JointVars = 50; @@ -336,14 +338,16 @@ fn ForcesFromJoints(i: u32) { //gosl:kernel var nc = BodyJoints[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(di), u32(1), u32(0))]; var tf = vec3(0, 0, 0); var tt = vec3(0, 0, 0); - for (var i = i32(1); i <= np; i++) { + for (var i = i32(1); + i <= np; i++) { var ji = BodyJoints[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(di), u32(0), u32(i))]; var f = JointPForce(ji); tf = tf+(f); var t = JointPTorque(ji); tt = tt+(t); } - for (var i = i32(1); i <= nc; i++) { + for (var i = i32(1); + i <= nc; i++) { var ji = BodyJoints[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(di), u32(1), u32(i))]; var f = JointCForce(ji); tf = tf+(f); diff --git a/physics/shaders/InitDynamics.wgsl b/physics/shaders/InitDynamics.wgsl index d349e3f6..118c578b 100644 --- a/physics/shaders/InitDynamics.wgsl +++ b/physics/shaders/InitDynamics.wgsl @@ -101,18 +101,19 @@ const ContactBThick: ContactVars = 16; const ContactNormX: ContactVars = 17; const ContactNormY: ContactVars = 18; const ContactNormZ: ContactVars = 19; -const ContactADeltaX: ContactVars = 20; -const ContactADeltaY: ContactVars = 21; -const ContactADeltaZ: ContactVars = 22; -const ContactAAngDeltaX: ContactVars = 23; -const ContactAAngDeltaY: ContactVars = 24; -const ContactAAngDeltaZ: ContactVars = 25; -const ContactBDeltaX: ContactVars = 26; -const ContactBDeltaY: ContactVars = 27; -const ContactBDeltaZ: ContactVars = 28; -const ContactBAngDeltaX: ContactVars = 29; -const ContactBAngDeltaY: ContactVars = 30; -const ContactBAngDeltaZ: ContactVars = 31; +const ContactWeight: ContactVars = 20; +const ContactADeltaX: ContactVars = 21; +const ContactADeltaY: ContactVars = 22; +const ContactADeltaZ: ContactVars = 23; +const ContactAAngDeltaX: ContactVars = 24; +const ContactAAngDeltaY: ContactVars = 25; +const ContactAAngDeltaZ: ContactVars = 26; +const ContactBDeltaX: ContactVars = 27; +const ContactBDeltaY: ContactVars = 28; +const ContactBDeltaZ: ContactVars = 29; +const ContactBAngDeltaX: ContactVars = 30; +const ContactBAngDeltaY: ContactVars = 31; +const ContactBAngDeltaZ: ContactVars = 32; const BroadContactVarsN = ContactAPointX; //////// import: "control.go" @@ -155,15 +156,14 @@ const DynDeltaZ: DynamicVars = 28; const DynAngDeltaX: DynamicVars = 29; const DynAngDeltaY: DynamicVars = 30; const DynAngDeltaZ: DynamicVars = 31; -fn DynamicBody(idx: i32) -> i32 { - return i32(bitcast(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(0), u32(DynBody))])); -} +const DynContactWeight: DynamicVars = 32; +fn DynamicBody(idx: i32) -> i32 { return i32(bitcast(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(0), u32(DynBody))])); } //////// import: "enumgen.go" const BodyVarsN: BodyVars = 43; -const ContactVarsN: ContactVars = 32; +const ContactVarsN: ContactVars = 33; const JointControlVarsN: JointControlVars = 3; -const DynamicVarsN: DynamicVars = 32; +const DynamicVarsN: DynamicVars = 33; const GPUVarsN: GPUVars = 12; const JointTypesN: JointTypes = 7; const JointVarsN: JointVars = 50; @@ -320,7 +320,8 @@ fn InitDynamics(i: u32) { //gosl:kernel Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(ii), u32(cni), u32(DynQuatY))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyQuatY))]; Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(ii), u32(cni), u32(DynQuatZ))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyQuatZ))]; Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(ii), u32(cni), u32(DynQuatW))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyQuatW))]; - for (var v = DynVelX; v < DynamicVarsN; v++) { + for (var v = DynVelX; + v < DynamicVarsN; v++) { Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(ii), u32(cni), u32(v))] = 0.0; } diff --git a/physics/shaders/StepBodyContacts.wgsl b/physics/shaders/StepBodyContacts.wgsl index 6fa08b2e..aaa9e6bb 100644 --- a/physics/shaders/StepBodyContacts.wgsl +++ b/physics/shaders/StepBodyContacts.wgsl @@ -138,18 +138,19 @@ const ContactBThick: ContactVars = 16; const ContactNormX: ContactVars = 17; const ContactNormY: ContactVars = 18; const ContactNormZ: ContactVars = 19; -const ContactADeltaX: ContactVars = 20; -const ContactADeltaY: ContactVars = 21; -const ContactADeltaZ: ContactVars = 22; -const ContactAAngDeltaX: ContactVars = 23; -const ContactAAngDeltaY: ContactVars = 24; -const ContactAAngDeltaZ: ContactVars = 25; -const ContactBDeltaX: ContactVars = 26; -const ContactBDeltaY: ContactVars = 27; -const ContactBDeltaZ: ContactVars = 28; -const ContactBAngDeltaX: ContactVars = 29; -const ContactBAngDeltaY: ContactVars = 30; -const ContactBAngDeltaZ: ContactVars = 31; +const ContactWeight: ContactVars = 20; +const ContactADeltaX: ContactVars = 21; +const ContactADeltaY: ContactVars = 22; +const ContactADeltaZ: ContactVars = 23; +const ContactAAngDeltaX: ContactVars = 24; +const ContactAAngDeltaY: ContactVars = 25; +const ContactAAngDeltaZ: ContactVars = 26; +const ContactBDeltaX: ContactVars = 27; +const ContactBDeltaY: ContactVars = 28; +const ContactBDeltaZ: ContactVars = 29; +const ContactBAngDeltaX: ContactVars = 30; +const ContactBAngDeltaY: ContactVars = 31; +const ContactBAngDeltaZ: ContactVars = 32; const BroadContactVarsN = ContactAPointX; fn GetContactA(idx: i32) -> i32 { return i32(bitcast(Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactA))])); } fn GetContactB(idx: i32) -> i32 { return i32(bitcast(Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactB))])); } @@ -163,19 +164,63 @@ fn SetContactAAngDelta(idx: i32, pos: vec3) { Contacts[Index2D(TensorStride fn SetContactBDelta(idx: i32, pos: vec3) { Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactBDeltaX))] = pos.x;; Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactBDeltaY))] = pos.y;; Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactBDeltaZ))] = pos.z; } fn SetContactBAngDelta(idx: i32, pos: vec3) { Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactBAngDeltaX))] = pos.x;; Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactBAngDeltaY))] = pos.y;; Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactBAngDeltaZ))] = pos.z; } fn StepBodyContacts(i: u32) { //gosl:kernel -let params = Params[0];; var ci = i32(i);; var cmax = ContactsN[0];; if (ci >= cmax) { +let params = Params[0];; var ci = i32(i); +; var cmax = ContactsN[0]; +; if (ci >= cmax) { return; -}; var biA = GetContactA(ci);; var biB = GetContactB(ci);; var diA = GetBodyDynamic(biA);; var diB = GetBodyDynamic(biB);; var r0A = BodyDynamicPos(biA, params.Next);; var q0A = BodyDynamicQuat(biA, params.Next);; var r0B = BodyDynamicPos(biB, params.Next);; var q0B = BodyDynamicQuat(biB, params.Next);; var ctA = ContactAPoint(ci);; var offA = ContactAOff(ci);; var ctB = ContactBPoint(ci);; var offB = ContactBOff(ci);; var ctAw = MulSpatialPoint(r0A, q0A, ctA);; var ctBw = MulSpatialPoint(r0B, q0B, ctB);; var thickA = Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(ci), u32(ContactAThick))];; var thickB = Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(ci), u32(ContactBThick))];; var thick = thickA + thickB;; var norm = ContactNorm(ci);; var nnorm = Negate3(norm);; var d = Dot3(norm, ctBw-(ctAw)) - thick;; if (d >= 0.0) { // now separated - return; -}; var comA = BodyCom(biA);; var mInvA = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(biA), u32(BodyInvMass))];; var iInvA = BodyInvInertia(biA);; var comB = BodyCom(biB);; var mInvB = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(biB), u32(BodyInvMass))];; var iInvB = BodyInvInertia(biB);; var w0A: vec3; +}; var biA = GetContactA(ci); +; var biB = GetContactB(ci); +; var diA = GetBodyDynamic(biA); +; var diB = GetBodyDynamic(biB); +; var r0A = BodyDynamicPos(biA, params.Next); +; var q0A = BodyDynamicQuat(biA, params.Next); +; var r0B = BodyDynamicPos(biB, params.Next); +; var q0B = BodyDynamicQuat(biB, params.Next); +; var ctA = ContactAPoint(ci); +; var offA = ContactAOff(ci); +; var ctB = ContactBPoint(ci); +; var offB = ContactBOff(ci); +; var ctAw = MulSpatialPoint(r0A, q0A, ctA); +; var ctBw = MulSpatialPoint(r0B, q0B, ctB); +; var thickA = Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(ci), u32(ContactAThick))]; +; var thickB = Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(ci), u32(ContactBThick))]; +; var thick = thickA + thickB; +; var norm = ContactNorm(ci); +; var nnorm = Negate3(norm); +; var d = Dot3(norm, ctBw-(ctAw)) - thick; +; if (d >= 0.0) { // now separated + Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(ci), u32(ContactWeight))] = 0.0; + var z = vec3(0, 0, 0); + SetContactADelta(ci, z); + SetContactBDelta(ci, z); + SetContactAAngDelta(ci, z); + SetContactBAngDelta(ci, z);return; +}; var comA = BodyCom(biA); +; var mInvA = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(biA), u32(BodyInvMass))]; +; var iInvA = BodyInvInertia(biA); +; var comB = BodyCom(biB); +; var mInvB = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(biB), u32(BodyInvMass))]; +; var iInvB = BodyInvInertia(biB); +; var w0A: vec3; var w0B: vec3;; if (diA >= 0) { w0A = DynamicAngDelta(diA, params.Next); }; if (diB >= 0) { w0B = DynamicAngDelta(diB, params.Next); }; -var mu = 0.5 * (Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(biA), u32(BodyFriction))] + Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(biB), u32(BodyFriction))]);; var frTors = 0.5 * (Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(biA), u32(BodyFrictionTortion))] + Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(biB), u32(BodyFrictionTortion))]);; var frRoll = 0.5 * (Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(biA), u32(BodyFrictionRolling))] + Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(biB), u32(BodyFrictionRolling))]);; -var dA = ctAw-(MulSpatialPoint(r0A, q0A, comA));; var dB = ctBw-(MulSpatialPoint(r0B, q0B, comB));; var angA = Negate3(Cross3(dA, norm));; var angB = Cross3(dB, norm);; -var lambdaN = ContactConstraint(d, q0A, q0B, mInvA, mInvB, iInvA, iInvB, nnorm, norm, angA, angB, params.ContactRelax, params.Dt);; var linDeltaA = Negate3(norm)*(lambdaN);; var linDeltaB = norm*(lambdaN);; var angDeltaA = angA*(lambdaN);; var angDeltaB = angB*(lambdaN);; +var mu = 0.5 * (Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(biA), u32(BodyFriction))] + Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(biB), u32(BodyFriction))]); +; var frTors = 0.5 * (Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(biA), u32(BodyFrictionTortion))] + Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(biB), u32(BodyFrictionTortion))]); +; var frRoll = 0.5 * (Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(biA), u32(BodyFrictionRolling))] + Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(biB), u32(BodyFrictionRolling))]); +; +var dA = ctAw-(MulSpatialPoint(r0A, q0A, comA)); +; var dB = ctBw-(MulSpatialPoint(r0B, q0B, comB)); +; var angA = Negate3(Cross3(dA, norm)); +; var angB = Cross3(dB, norm); +; var lambdaN = ContactConstraint(d, q0A, q0B, mInvA, mInvB, iInvA, iInvB, nnorm, norm, angA, angB, params.ContactRelax, params.Dt); +; var linDeltaA = Negate3(norm)*(lambdaN); +; var linDeltaB = norm*(lambdaN); +; var angDeltaA = angA*(lambdaN); +; var angDeltaB = angB*(lambdaN); +; if (mu > 0.0) { ctAw = ctAw+(MulQuatVector(q0A, offA)); ctBw = ctBw+(MulQuatVector(q0B, offB)); @@ -193,7 +238,8 @@ if (mu > 0.0) { angDeltaA = angDeltaA+(angA*(lambdaFr)); angDeltaB = angDeltaB+(angB*(lambdaFr)); } -}; var deltaW = w0B-(w0A);; if (frTors > 0.0) { +}; var deltaW = w0B-(w0A); +; if (frTors > 0.0) { var err = Dot3(deltaW, norm) * params.Dt; if (abs(err) > 0.0) { var lin = vec3(0, 0, 0); @@ -213,7 +259,7 @@ if (mu > 0.0) { angDeltaA = angDeltaA-(rollN*(lambdaRoll)); angDeltaB = angDeltaB+(rollN*(lambdaRoll)); } -}; SetContactADelta(ci, linDeltaA);; SetContactBDelta(ci, linDeltaB);; SetContactAAngDelta(ci, angDeltaA);; SetContactBAngDelta(ci, angDeltaB); } +}; Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(ci), u32(ContactWeight))] = 1.0;; SetContactADelta(ci, linDeltaA);; SetContactBDelta(ci, linDeltaB);; SetContactAAngDelta(ci, angDeltaA);; SetContactBAngDelta(ci, angDeltaB); } fn ContactConstraint(err: f32, q0A: vec4,q0B: vec4, mInvA: f32,mInvB: f32, iInvA: mat3x3f,iInvB: mat3x3f, linA: vec3,linB: vec3,angA: vec3,angB: vec3, relaxation: f32,dt: f32) -> f32 { var denom = f32(0.0); denom += LengthSquared3(linA) * mInvA; @@ -268,6 +314,7 @@ const DynDeltaZ: DynamicVars = 28; const DynAngDeltaX: DynamicVars = 29; const DynAngDeltaY: DynamicVars = 30; const DynAngDeltaZ: DynamicVars = 31; +const DynContactWeight: DynamicVars = 32; fn DynamicPos(idx: i32,cni: i32) -> vec3 { return vec3(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosZ))]); } @@ -280,9 +327,9 @@ fn DynamicAngDelta(idx: i32,cni: i32) -> vec3 { //////// import: "enumgen.go" const BodyVarsN: BodyVars = 43; -const ContactVarsN: ContactVars = 32; +const ContactVarsN: ContactVars = 33; const JointControlVarsN: JointControlVars = 3; -const DynamicVarsN: DynamicVars = 32; +const DynamicVarsN: DynamicVars = 33; const GPUVarsN: GPUVars = 12; const JointTypesN: JointTypes = 7; const JointVarsN: JointVars = 50; @@ -418,14 +465,17 @@ const Cone: Shapes = 5; //////// import: "slmath-quaternion.go" fn MulQuatVector(q: vec4, v: vec3) -> vec3 { var xyz = vec3(q.x, q.y, q.z); - var t = Cross3(xyz, v)*(2);return v+(t*(q.w))+(Cross3(xyz, t)); + var t = Cross3(xyz, v)*(2); +return v+(t*(q.w))+(Cross3(xyz, t)); } fn MulQuatVectorInverse(q: vec4, v: vec3) -> vec3 { var xyz = vec3(q.x, q.y, q.z); - var t = Cross3(xyz, v)*(2);return v-(t*(q.w))+(Cross3(xyz, t)); + var t = Cross3(xyz, v)*(2); +return v-(t*(q.w))+(Cross3(xyz, t)); } fn MulSpatialPoint(xP: vec3, xQ: vec4, p: vec3) -> vec3 { - var dp = MulQuatVector(xQ, p);return dp+(xP); + var dp = MulQuatVector(xQ, p); +return dp+(xP); } //////// import: "slmath-vector2.go" diff --git a/physics/shaders/StepBodyDeltas.wgsl b/physics/shaders/StepBodyDeltas.wgsl index 5656ea3f..f7dfceee 100644 --- a/physics/shaders/StepBodyDeltas.wgsl +++ b/physics/shaders/StepBodyDeltas.wgsl @@ -114,18 +114,19 @@ const ContactBThick: ContactVars = 16; const ContactNormX: ContactVars = 17; const ContactNormY: ContactVars = 18; const ContactNormZ: ContactVars = 19; -const ContactADeltaX: ContactVars = 20; -const ContactADeltaY: ContactVars = 21; -const ContactADeltaZ: ContactVars = 22; -const ContactAAngDeltaX: ContactVars = 23; -const ContactAAngDeltaY: ContactVars = 24; -const ContactAAngDeltaZ: ContactVars = 25; -const ContactBDeltaX: ContactVars = 26; -const ContactBDeltaY: ContactVars = 27; -const ContactBDeltaZ: ContactVars = 28; -const ContactBAngDeltaX: ContactVars = 29; -const ContactBAngDeltaY: ContactVars = 30; -const ContactBAngDeltaZ: ContactVars = 31; +const ContactWeight: ContactVars = 20; +const ContactADeltaX: ContactVars = 21; +const ContactADeltaY: ContactVars = 22; +const ContactADeltaZ: ContactVars = 23; +const ContactAAngDeltaX: ContactVars = 24; +const ContactAAngDeltaY: ContactVars = 25; +const ContactAAngDeltaZ: ContactVars = 26; +const ContactBDeltaX: ContactVars = 27; +const ContactBDeltaY: ContactVars = 28; +const ContactBDeltaZ: ContactVars = 29; +const ContactBAngDeltaX: ContactVars = 30; +const ContactBAngDeltaY: ContactVars = 31; +const ContactBAngDeltaZ: ContactVars = 32; const BroadContactVarsN = ContactAPointX; //////// import: "control.go" @@ -168,26 +169,12 @@ const DynDeltaZ: DynamicVars = 28; const DynAngDeltaX: DynamicVars = 29; const DynAngDeltaY: DynamicVars = 30; const DynAngDeltaZ: DynamicVars = 31; -fn DynamicBody(idx: i32) -> i32 { - return i32(bitcast(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(0), u32(DynBody))])); -} -fn DynamicPos(idx: i32,cni: i32) -> vec3 { - return vec3(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosZ))]); -} -fn SetDynamicPos(idx: i32,cni: i32, pos: vec3) { - Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosX))] = pos.x; - Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosY))] = pos.y; - Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosZ))] = pos.z; -} -fn DynamicQuat(idx: i32,cni: i32) -> vec4 { - return vec4(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatZ))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatW))]); -} -fn SetDynamicQuat(idx: i32,cni: i32, rot: vec4) { - Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatX))] = rot.x; - Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatY))] = rot.y; - Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatZ))] = rot.z; - Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatW))] = rot.w; -} +const DynContactWeight: DynamicVars = 32; +fn DynamicBody(idx: i32) -> i32 { return i32(bitcast(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(0), u32(DynBody))])); } +fn DynamicPos(idx: i32,cni: i32) -> vec3 { return vec3(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosZ))]); } +fn SetDynamicPos(idx: i32,cni: i32, pos: vec3) { Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosX))] = pos.x;; Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosY))] = pos.y;; Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosZ))] = pos.z; } +fn DynamicQuat(idx: i32,cni: i32) -> vec4 { return vec4(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatZ))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatW))]); } +fn SetDynamicQuat(idx: i32,cni: i32, rot: vec4) { Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatX))] = rot.x;; Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatY))] = rot.y;; Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatZ))] = rot.z;; Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatW))] = rot.w; } fn DynamicDelta(idx: i32,cni: i32) -> vec3 { return vec3(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaZ))]); } @@ -207,9 +194,9 @@ fn SetDynamicAngDelta(idx: i32,cni: i32, angDelta: vec3) { //////// import: "enumgen.go" const BodyVarsN: BodyVars = 43; -const ContactVarsN: ContactVars = 32; +const ContactVarsN: ContactVars = 33; const JointControlVarsN: JointControlVars = 3; -const DynamicVarsN: DynamicVars = 32; +const DynamicVarsN: DynamicVars = 33; const GPUVarsN: GPUVars = 12; const JointTypesN: JointTypes = 7; const JointVarsN: JointVars = 50; @@ -364,18 +351,21 @@ fn QuatNormalize(q: vec4) -> vec4 { } fn MulQuatVector(q: vec4, v: vec3) -> vec3 { var xyz = vec3(q.x, q.y, q.z); - var t = Cross3(xyz, v)*(2);return v+(t*(q.w))+(Cross3(xyz, t)); + var t = Cross3(xyz, v)*(2); +return v+(t*(q.w))+(Cross3(xyz, t)); } fn MulQuatVectorInverse(q: vec4, v: vec3) -> vec3 { var xyz = vec3(q.x, q.y, q.z); - var t = Cross3(xyz, v)*(2);return v-(t*(q.w))+(Cross3(xyz, t)); + var t = Cross3(xyz, v)*(2); +return v-(t*(q.w))+(Cross3(xyz, t)); } fn MulQuats(a: vec4,b: vec4) -> vec4 { var q: vec4; q.x = a.x*b.w + a.w*b.x + a.y*b.z - a.z*b.y; q.y = a.y*b.w + a.w*b.y + a.z*b.x - a.x*b.z; q.z = a.z*b.w + a.w*b.z + a.x*b.y - a.y*b.x; - q.w = a.w*b.w - a.x*b.x - a.y*b.y - a.z*b.z;return q; + q.w = a.w*b.w - a.x*b.x - a.y*b.y - a.z*b.z; +return q; } //////// import: "slmath-vector2.go" @@ -409,6 +399,10 @@ fn StepBodyDeltas(i: u32) { //gosl:kernel var v0 = DynamicDelta(di, params.Next); var w0 = DynamicAngDelta(di, params.Next); var weight = f32(1.0); + var cWt = Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(di), u32(params.Next), u32(DynContactWeight))]; + if (cWt > 0) { + weight = 1.0 / cWt; + } var dp = v0*(invMass * weight); var dq = w0*(weight); var wb = MulQuatVectorInverse(q0, w0); diff --git a/physics/shaders/StepIntegrateBodies.wgsl b/physics/shaders/StepIntegrateBodies.wgsl index e53e4998..9c8829eb 100644 --- a/physics/shaders/StepIntegrateBodies.wgsl +++ b/physics/shaders/StepIntegrateBodies.wgsl @@ -114,18 +114,19 @@ const ContactBThick: ContactVars = 16; const ContactNormX: ContactVars = 17; const ContactNormY: ContactVars = 18; const ContactNormZ: ContactVars = 19; -const ContactADeltaX: ContactVars = 20; -const ContactADeltaY: ContactVars = 21; -const ContactADeltaZ: ContactVars = 22; -const ContactAAngDeltaX: ContactVars = 23; -const ContactAAngDeltaY: ContactVars = 24; -const ContactAAngDeltaZ: ContactVars = 25; -const ContactBDeltaX: ContactVars = 26; -const ContactBDeltaY: ContactVars = 27; -const ContactBDeltaZ: ContactVars = 28; -const ContactBAngDeltaX: ContactVars = 29; -const ContactBAngDeltaY: ContactVars = 30; -const ContactBAngDeltaZ: ContactVars = 31; +const ContactWeight: ContactVars = 20; +const ContactADeltaX: ContactVars = 21; +const ContactADeltaY: ContactVars = 22; +const ContactADeltaZ: ContactVars = 23; +const ContactAAngDeltaX: ContactVars = 24; +const ContactAAngDeltaY: ContactVars = 25; +const ContactAAngDeltaZ: ContactVars = 26; +const ContactBDeltaX: ContactVars = 27; +const ContactBDeltaY: ContactVars = 28; +const ContactBDeltaZ: ContactVars = 29; +const ContactBAngDeltaX: ContactVars = 30; +const ContactBAngDeltaY: ContactVars = 31; +const ContactBAngDeltaZ: ContactVars = 32; const BroadContactVarsN = ContactAPointX; //////// import: "control.go" @@ -168,54 +169,24 @@ const DynDeltaZ: DynamicVars = 28; const DynAngDeltaX: DynamicVars = 29; const DynAngDeltaY: DynamicVars = 30; const DynAngDeltaZ: DynamicVars = 31; -fn DynamicBody(idx: i32) -> i32 { - return i32(bitcast(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(0), u32(DynBody))])); -} -fn DynamicPos(idx: i32,cni: i32) -> vec3 { - return vec3(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosZ))]); -} -fn SetDynamicPos(idx: i32,cni: i32, pos: vec3) { - Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosX))] = pos.x; - Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosY))] = pos.y; - Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosZ))] = pos.z; -} -fn DynamicQuat(idx: i32,cni: i32) -> vec4 { - return vec4(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatZ))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatW))]); -} -fn SetDynamicQuat(idx: i32,cni: i32, rot: vec4) { - Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatX))] = rot.x; - Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatY))] = rot.y; - Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatZ))] = rot.z; - Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatW))] = rot.w; -} -fn DynamicForce(idx: i32,cni: i32) -> vec3 { - return vec3(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynForceX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynForceY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynForceZ))]); -} -fn DynamicTorque(idx: i32,cni: i32) -> vec3 { - return vec3(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynTorqueX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynTorqueY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynTorqueZ))]); -} -fn DynamicDelta(idx: i32,cni: i32) -> vec3 { - return vec3(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaZ))]); -} -fn SetDynamicDelta(idx: i32,cni: i32, delta: vec3) { - Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaX))] = delta.x; - Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaY))] = delta.y; - Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaZ))] = delta.z; -} -fn DynamicAngDelta(idx: i32,cni: i32) -> vec3 { - return vec3(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaZ))]); -} -fn SetDynamicAngDelta(idx: i32,cni: i32, angDelta: vec3) { - Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaX))] = angDelta.x; - Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaY))] = angDelta.y; - Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaZ))] = angDelta.z; -} +const DynContactWeight: DynamicVars = 32; +fn DynamicBody(idx: i32) -> i32 { return i32(bitcast(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(0), u32(DynBody))])); } +fn DynamicPos(idx: i32,cni: i32) -> vec3 { return vec3(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosZ))]); } +fn SetDynamicPos(idx: i32,cni: i32, pos: vec3) { Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosX))] = pos.x;; Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosY))] = pos.y;; Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosZ))] = pos.z; } +fn DynamicQuat(idx: i32,cni: i32) -> vec4 { return vec4(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatZ))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatW))]); } +fn SetDynamicQuat(idx: i32,cni: i32, rot: vec4) { Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatX))] = rot.x;; Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatY))] = rot.y;; Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatZ))] = rot.z;; Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatW))] = rot.w; } +fn DynamicForce(idx: i32,cni: i32) -> vec3 { return vec3(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynForceX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynForceY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynForceZ))]); } +fn DynamicTorque(idx: i32,cni: i32) -> vec3 { return vec3(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynTorqueX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynTorqueY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynTorqueZ))]); } +fn DynamicDelta(idx: i32,cni: i32) -> vec3 { return vec3(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaZ))]); } +fn SetDynamicDelta(idx: i32,cni: i32, delta: vec3) { Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaX))] = delta.x;; Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaY))] = delta.y;; Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaZ))] = delta.z; } +fn DynamicAngDelta(idx: i32,cni: i32) -> vec3 { return vec3(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaZ))]); } +fn SetDynamicAngDelta(idx: i32,cni: i32, angDelta: vec3) { Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaX))] = angDelta.x;; Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaY))] = angDelta.y;; Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaZ))] = angDelta.z; } //////// import: "enumgen.go" const BodyVarsN: BodyVars = 43; -const ContactVarsN: ContactVars = 32; +const ContactVarsN: ContactVars = 33; const JointControlVarsN: JointControlVars = 3; -const DynamicVarsN: DynamicVars = 32; +const DynamicVarsN: DynamicVars = 33; const GPUVarsN: GPUVars = 12; const JointTypesN: JointTypes = 7; const JointVarsN: JointVars = 50; @@ -370,25 +341,29 @@ fn QuatNormalize(q: vec4) -> vec4 { } fn MulQuatVector(q: vec4, v: vec3) -> vec3 { var xyz = vec3(q.x, q.y, q.z); - var t = Cross3(xyz, v)*(2);return v+(t*(q.w))+(Cross3(xyz, t)); + var t = Cross3(xyz, v)*(2); +return v+(t*(q.w))+(Cross3(xyz, t)); } fn MulQuatVectorInverse(q: vec4, v: vec3) -> vec3 { var xyz = vec3(q.x, q.y, q.z); - var t = Cross3(xyz, v)*(2);return v-(t*(q.w))+(Cross3(xyz, t)); + var t = Cross3(xyz, v)*(2); +return v-(t*(q.w))+(Cross3(xyz, t)); } fn MulQuats(a: vec4,b: vec4) -> vec4 { var q: vec4; q.x = a.x*b.w + a.w*b.x + a.y*b.z - a.z*b.y; q.y = a.y*b.w + a.w*b.y + a.z*b.x - a.x*b.z; q.z = a.z*b.w + a.w*b.z + a.x*b.y - a.y*b.x; - q.w = a.w*b.w - a.x*b.x - a.y*b.y - a.z*b.z;return q; + q.w = a.w*b.w - a.x*b.x - a.y*b.y - a.z*b.z; +return q; } fn QuatAdd(q: vec4, o: vec4) -> vec4 { var nq = q; nq.x += o.x; nq.y += o.y; nq.z += o.z; - nq.w += o.w;return nq; + nq.w += o.w; +return nq; } //////// import: "slmath-vector2.go" diff --git a/physics/shaders/StepJointForces.wgsl b/physics/shaders/StepJointForces.wgsl index 424b948b..004ae2ff 100644 --- a/physics/shaders/StepJointForces.wgsl +++ b/physics/shaders/StepJointForces.wgsl @@ -110,18 +110,19 @@ const ContactBThick: ContactVars = 16; const ContactNormX: ContactVars = 17; const ContactNormY: ContactVars = 18; const ContactNormZ: ContactVars = 19; -const ContactADeltaX: ContactVars = 20; -const ContactADeltaY: ContactVars = 21; -const ContactADeltaZ: ContactVars = 22; -const ContactAAngDeltaX: ContactVars = 23; -const ContactAAngDeltaY: ContactVars = 24; -const ContactAAngDeltaZ: ContactVars = 25; -const ContactBDeltaX: ContactVars = 26; -const ContactBDeltaY: ContactVars = 27; -const ContactBDeltaZ: ContactVars = 28; -const ContactBAngDeltaX: ContactVars = 29; -const ContactBAngDeltaY: ContactVars = 30; -const ContactBAngDeltaZ: ContactVars = 31; +const ContactWeight: ContactVars = 20; +const ContactADeltaX: ContactVars = 21; +const ContactADeltaY: ContactVars = 22; +const ContactADeltaZ: ContactVars = 23; +const ContactAAngDeltaX: ContactVars = 24; +const ContactAAngDeltaY: ContactVars = 25; +const ContactAAngDeltaZ: ContactVars = 26; +const ContactBDeltaX: ContactVars = 27; +const ContactBDeltaY: ContactVars = 28; +const ContactBDeltaZ: ContactVars = 29; +const ContactBAngDeltaX: ContactVars = 30; +const ContactBAngDeltaY: ContactVars = 31; +const ContactBAngDeltaZ: ContactVars = 32; const BroadContactVarsN = ContactAPointX; //////// import: "control.go" @@ -167,21 +168,16 @@ const DynDeltaZ: DynamicVars = 28; const DynAngDeltaX: DynamicVars = 29; const DynAngDeltaY: DynamicVars = 30; const DynAngDeltaZ: DynamicVars = 31; -fn DynamicBody(idx: i32) -> i32 { - return i32(bitcast(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(0), u32(DynBody))])); -} -fn DynamicPos(idx: i32,cni: i32) -> vec3 { - return vec3(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosZ))]); -} -fn DynamicQuat(idx: i32,cni: i32) -> vec4 { - return vec4(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatZ))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatW))]); -} +const DynContactWeight: DynamicVars = 32; +fn DynamicBody(idx: i32) -> i32 { return i32(bitcast(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(0), u32(DynBody))])); } +fn DynamicPos(idx: i32,cni: i32) -> vec3 { return vec3(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosZ))]); } +fn DynamicQuat(idx: i32,cni: i32) -> vec4 { return vec4(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatZ))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatW))]); } //////// import: "enumgen.go" const BodyVarsN: BodyVars = 43; -const ContactVarsN: ContactVars = 32; +const ContactVarsN: ContactVars = 33; const JointControlVarsN: JointControlVars = 3; -const DynamicVarsN: DynamicVars = 32; +const DynamicVarsN: DynamicVars = 33; const GPUVarsN: GPUVars = 12; const JointTypesN: JointTypes = 7; const JointVarsN: JointVars = 50; @@ -253,7 +249,8 @@ fn GetJointType(idx: i32) -> JointTypes { return JointTypes(bitcast(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointType))])); } fn GetJointEnabled(idx: i32) -> bool { - var je = bitcast(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointEnabled))]);return je != 0; + var je = bitcast(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointEnabled))]); +return je != 0; } fn JointParentIndex(idx: i32) -> i32 { return i32(bitcast(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointParent))])); @@ -370,21 +367,24 @@ const Cone: Shapes = 5; //////// import: "slmath-quaternion.go" fn MulQuatVector(q: vec4, v: vec3) -> vec3 { var xyz = vec3(q.x, q.y, q.z); - var t = Cross3(xyz, v)*(2);return v+(t*(q.w))+(Cross3(xyz, t)); + var t = Cross3(xyz, v)*(2); +return v+(t*(q.w))+(Cross3(xyz, t)); } fn MulQuats(a: vec4,b: vec4) -> vec4 { var q: vec4; q.x = a.x*b.w + a.w*b.x + a.y*b.z - a.z*b.y; q.y = a.y*b.w + a.w*b.y + a.z*b.x - a.x*b.z; q.z = a.z*b.w + a.w*b.z + a.x*b.y - a.y*b.x; - q.w = a.w*b.w - a.x*b.x - a.y*b.y - a.z*b.z;return q; + q.w = a.w*b.w - a.x*b.x - a.y*b.y - a.z*b.z; +return q; } fn MulSpatialTransforms(aP: vec3, aQ: vec4, bP: vec3, bQ: vec4, oP: ptr>, oQ: ptr>) { *oP = MulQuatVector(aQ, bP)+(aP); *oQ = MulQuats(aQ, bQ); } fn MulSpatialPoint(xP: vec3, xQ: vec4, p: vec3) -> vec3 { - var dp = MulQuatVector(xQ, p);return dp+(xP); + var dp = MulQuatVector(xQ, p); +return dp+(xP); } //////// import: "slmath-vector2.go" diff --git a/physics/shaders/StepSolveJoints.wgsl b/physics/shaders/StepSolveJoints.wgsl index 6ea22792..44f976d9 100644 --- a/physics/shaders/StepSolveJoints.wgsl +++ b/physics/shaders/StepSolveJoints.wgsl @@ -115,18 +115,19 @@ const ContactBThick: ContactVars = 16; const ContactNormX: ContactVars = 17; const ContactNormY: ContactVars = 18; const ContactNormZ: ContactVars = 19; -const ContactADeltaX: ContactVars = 20; -const ContactADeltaY: ContactVars = 21; -const ContactADeltaZ: ContactVars = 22; -const ContactAAngDeltaX: ContactVars = 23; -const ContactAAngDeltaY: ContactVars = 24; -const ContactAAngDeltaZ: ContactVars = 25; -const ContactBDeltaX: ContactVars = 26; -const ContactBDeltaY: ContactVars = 27; -const ContactBDeltaZ: ContactVars = 28; -const ContactBAngDeltaX: ContactVars = 29; -const ContactBAngDeltaY: ContactVars = 30; -const ContactBAngDeltaZ: ContactVars = 31; +const ContactWeight: ContactVars = 20; +const ContactADeltaX: ContactVars = 21; +const ContactADeltaY: ContactVars = 22; +const ContactADeltaZ: ContactVars = 23; +const ContactAAngDeltaX: ContactVars = 24; +const ContactAAngDeltaY: ContactVars = 25; +const ContactAAngDeltaZ: ContactVars = 26; +const ContactBDeltaX: ContactVars = 27; +const ContactBDeltaY: ContactVars = 28; +const ContactBDeltaZ: ContactVars = 29; +const ContactBAngDeltaX: ContactVars = 30; +const ContactBAngDeltaY: ContactVars = 31; +const ContactBAngDeltaZ: ContactVars = 32; const BroadContactVarsN = ContactAPointX; //////// import: "control.go" @@ -172,15 +173,10 @@ const DynDeltaZ: DynamicVars = 28; const DynAngDeltaX: DynamicVars = 29; const DynAngDeltaY: DynamicVars = 30; const DynAngDeltaZ: DynamicVars = 31; -fn DynamicBody(idx: i32) -> i32 { - return i32(bitcast(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(0), u32(DynBody))])); -} -fn DynamicPos(idx: i32,cni: i32) -> vec3 { - return vec3(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosZ))]); -} -fn DynamicQuat(idx: i32,cni: i32) -> vec4 { - return vec4(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatZ))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatW))]); -} +const DynContactWeight: DynamicVars = 32; +fn DynamicBody(idx: i32) -> i32 { return i32(bitcast(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(0), u32(DynBody))])); } +fn DynamicPos(idx: i32,cni: i32) -> vec3 { return vec3(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosZ))]); } +fn DynamicQuat(idx: i32,cni: i32) -> vec4 { return vec4(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatZ))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatW))]); } fn DynamicDelta(idx: i32,cni: i32) -> vec3 { return vec3(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaZ))]); } @@ -190,9 +186,9 @@ fn DynamicAngDelta(idx: i32,cni: i32) -> vec3 { //////// import: "enumgen.go" const BodyVarsN: BodyVars = 43; -const ContactVarsN: ContactVars = 32; +const ContactVarsN: ContactVars = 33; const JointControlVarsN: JointControlVars = 3; -const DynamicVarsN: DynamicVars = 32; +const DynamicVarsN: DynamicVars = 33; const GPUVarsN: GPUVars = 12; const JointTypesN: JointTypes = 7; const JointVarsN: JointVars = 50; @@ -264,7 +260,8 @@ fn GetJointType(idx: i32) -> JointTypes { return JointTypes(bitcast(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointType))])); } fn GetJointEnabled(idx: i32) -> bool { - var je = bitcast(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointEnabled))]);return je != 0; + var je = bitcast(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointEnabled))]); +return je != 0; } fn JointParentIndex(idx: i32) -> i32 { return i32(bitcast(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointParent))])); @@ -409,25 +406,29 @@ fn QuatNormalize(q: vec4) -> vec4 { } fn MulQuatVector(q: vec4, v: vec3) -> vec3 { var xyz = vec3(q.x, q.y, q.z); - var t = Cross3(xyz, v)*(2);return v+(t*(q.w))+(Cross3(xyz, t)); + var t = Cross3(xyz, v)*(2); +return v+(t*(q.w))+(Cross3(xyz, t)); } fn MulQuatVectorInverse(q: vec4, v: vec3) -> vec3 { var xyz = vec3(q.x, q.y, q.z); - var t = Cross3(xyz, v)*(2);return v-(t*(q.w))+(Cross3(xyz, t)); + var t = Cross3(xyz, v)*(2); +return v-(t*(q.w))+(Cross3(xyz, t)); } fn MulQuats(a: vec4,b: vec4) -> vec4 { var q: vec4; q.x = a.x*b.w + a.w*b.x + a.y*b.z - a.z*b.y; q.y = a.y*b.w + a.w*b.y + a.z*b.x - a.x*b.z; q.z = a.z*b.w + a.w*b.z + a.x*b.y - a.y*b.x; - q.w = a.w*b.w - a.x*b.x - a.y*b.y - a.z*b.z;return q; + q.w = a.w*b.w - a.x*b.x - a.y*b.y - a.z*b.z; +return q; } fn MulSpatialTransforms(aP: vec3, aQ: vec4, bP: vec3, bQ: vec4, oP: ptr>, oQ: ptr>) { *oP = MulQuatVector(aQ, bP)+(aP); *oQ = MulQuats(aQ, bQ); } fn MulSpatialPoint(xP: vec3, xQ: vec4, p: vec3) -> vec3 { - var dp = MulQuatVector(xQ, p);return dp+(xP); + var dp = MulQuatVector(xQ, p); +return dp+(xP); } fn SpatialTransformInverse(p: vec3, q: vec4, oP: ptr>, oQ: ptr>) { var qi = QuatInverse(q); @@ -438,7 +439,8 @@ fn QuatInverse(q: vec4) -> vec4 { var nq = q; nq.x *= f32(-1); nq.y *= f32(-1); - nq.z *= f32(-1);return QuatNormalize(nq); + nq.z *= f32(-1); +return QuatNormalize(nq); } fn QuatDot(q: vec4,o: vec4) -> f32 { return q.x*o.x + q.y*o.y + q.z*o.z + q.w*o.w; @@ -448,7 +450,8 @@ fn QuatMulScalar(q: vec4, s: f32) -> vec4 { nq.x *= s; nq.y *= s; nq.z *= s; - nq.w *= s;return nq; + nq.w *= s; +return nq; } //////// import: "slmath-vector2.go" diff --git a/physics/shapecollide.go b/physics/shapecollide.go index 3b871f05..64527e55 100644 --- a/physics/shapecollide.go +++ b/physics/shapecollide.go @@ -162,16 +162,16 @@ func ColCapsuleCapsule(cpi, maxIter int32, gdA *GeomData, gdB *GeomData, pA, pB, hhB := gdB.Size.Y // edge from capsule A // depending on point id, we query an edge from 0 to 0.5 or 0.5 to 1 - e0 := math32.Vec3(0.0, 0.0, hhA*float32(cpi%2)) - e1 := math32.Vec3(0.0, 0.0, -hhA*float32((cpi+1)%2)) + e0 := math32.Vec3(0, 0, hhA*float32(cpi%2)) + e1 := math32.Vec3(0, 0, -hhA*float32((cpi+1)%2)) edge0w := slmath.MulSpatialPoint(gdA.WbR, gdA.WbQ, e0) edge1w := slmath.MulSpatialPoint(gdA.WbR, gdA.WbQ, e1) edge0b := slmath.MulSpatialPoint(gdA.BwR, gdA.BwQ, edge0w) edge1b := slmath.MulSpatialPoint(gdA.BwR, gdA.BwQ, edge1w) u := ClosestEdgeCapsule(gdB.Size.X, gdB.Size.Y, edge0b, edge1b, maxIter) pAw := edge0w.MulScalar(1 - u).Add(edge1w.MulScalar(u)) - p0Bw := slmath.MulSpatialPoint(gdB.WbR, gdB.WbQ, math32.Vec3(0.0, 0.0, hhB)) - p1Bw := slmath.MulSpatialPoint(gdB.WbR, gdB.WbQ, math32.Vec3(0.0, 0.0, -hhB)) + p0Bw := slmath.MulSpatialPoint(gdB.WbR, gdB.WbQ, math32.Vec3(0, hhB, 0)) + p1Bw := slmath.MulSpatialPoint(gdB.WbR, gdB.WbQ, math32.Vec3(0, -hhB, 0)) pBw := ClosestPointLineSegment(p0Bw, p1Bw, pAw) diff := pAw.Sub(pBw) *norm = slmath.Normal3(diff) @@ -209,8 +209,8 @@ func ColBoxCapsule(cpi, maxIter int32, gdA *GeomData, gdB *GeomData, pA, pB, nor hhB := gdB.Size.Y // capsule B // depending on point id, we query an edge from 0 to 0.5 or 0.5 to 1 - e0 := math32.Vec3(0.0, 0.0, -hhB*float32(cpi%2)) - e1 := math32.Vec3(0.0, 0.0, hhB*float32((cpi+1)%2)) + e0 := math32.Vec3(0, -hhB*float32(cpi%2), 0) + e1 := math32.Vec3(0, hhB*float32((cpi+1)%2), 0) edge0w := slmath.MulSpatialPoint(gdB.WbR, gdB.WbQ, e0) edge1w := slmath.MulSpatialPoint(gdB.WbR, gdB.WbQ, e1) edge0a := slmath.MulSpatialPoint(gdA.BwR, gdA.BwQ, edge0w) @@ -243,15 +243,16 @@ func ColBoxPlane(cpi, maxIter int32, gdA *GeomData, gdB *GeomData, pA, pB, norm pBody := ClosestPointPlane(width, length, queryB) pBw = slmath.MulSpatialPoint(gdB.WbR, gdB.WbQ, pBody) diff = pAw.Sub(pBw) - *norm = slmath.MulQuatVector(gdB.WbQ, math32.Vec3(0.0, 0.0, 1.0)) - if width > 0.0 && length > 0.0 { + *norm = slmath.MulQuatVector(gdB.WbQ, math32.Vec3(0, 1, 0)) + if width > 0 && length > 0 { if math32.Abs(queryB.X) > width || math32.Abs(queryB.Z) > length { // skip, we will evaluate the plane edge contact with the box later - return 1.0e6 // invalid + return 1e6 // invalid } + // note: commented out in original: // check whether the COM is above the plane // sign = wp.sign(slmath.Dot3(wp.transform_get_translation(gdA.X_ws) - pBw, normal)) - // if sign < 0.0: + // if sign < 0: // // // the entire box is most likely below the plane // return @@ -275,7 +276,7 @@ func ColBoxPlane(cpi, maxIter int32, gdA *GeomData, gdB *GeomData, pA, pB, norm queryB := slmath.MulSpatialPoint(gdA.BwR, gdA.BwQ, pAw) if math32.Abs(queryB.X) > width || math32.Abs(queryB.Z) > length { // ensure that the closest point is actually inside the plane - return 1.0e6 // invalid + return 1e6 // invalid } diff = pAw.Sub(pBw) comA := gdA.WbR @@ -284,7 +285,7 @@ func ColBoxPlane(cpi, maxIter int32, gdA *GeomData, gdB *GeomData, pA, pB, norm // the COM is outside the plane *norm = slmath.Normal3(comA.Sub(pBw)) } else { - *norm = slmath.MulQuatVector(gdB.WbQ, math32.Vec3(0.0, 0.0, 1.0)) + *norm = slmath.MulQuatVector(gdB.WbQ, math32.Vec3(0, 0, 1)) } } *pA = pAw @@ -311,8 +312,8 @@ func ColSphereCapsule(cpi, maxIter int32, gdA *GeomData, gdB *GeomData, pA, pB, pAw := gdA.WbR hhB := gdB.Size.Y // capsule B - AB := slmath.MulSpatialPoint(gdB.WbR, gdB.WbQ, math32.Vec3(0.0, 0.0, hhB)) - BB := slmath.MulSpatialPoint(gdB.WbR, gdB.WbQ, math32.Vec3(0.0, 0.0, -hhB)) + AB := slmath.MulSpatialPoint(gdB.WbR, gdB.WbQ, math32.Vec3(0, hhB, 0)) + BB := slmath.MulSpatialPoint(gdB.WbR, gdB.WbQ, math32.Vec3(0, -hhB, 0)) pBw := ClosestPointLineSegment(AB, BB, pAw) diff := pAw.Sub(pBw) *norm = slmath.Normal3(diff) @@ -327,7 +328,7 @@ func ColSpherePlane(cpi, maxIter int32, gdA *GeomData, gdB *GeomData, pA, pB, no pBody := ClosestPointPlane(gdB.Size.X, gdB.Size.Z, slmath.MulSpatialPoint(gdB.BwR, gdB.BwQ, pAw)) pBw := slmath.MulSpatialPoint(gdB.WbR, gdB.WbQ, pBody) diff := pAw.Sub(pBw) - *norm = slmath.MulQuatVector(gdB.WbQ, math32.Vec3(0.0, 0.0, 1.0)) + *norm = slmath.MulQuatVector(gdB.WbQ, math32.Vec3(0, 1, 0)) *pA = pAw *pB = pBw return slmath.Dot3(diff, *norm) @@ -336,12 +337,12 @@ func ColSpherePlane(cpi, maxIter int32, gdA *GeomData, gdB *GeomData, pA, pB, no // Handle collision between a cylinder (geo_a) and an infinite plane (geo_b). func ColCylinderPlane(cpi, maxIter int32, gdA *GeomData, gdB *GeomData, pA, pB, norm *math32.Vector3) float32 { // World-space plane - plNorm := slmath.MulQuatVector(gdB.WbQ, math32.Vec3(0.0, 1.0, 0.0)) - plPos := slmath.MulSpatialPoint(gdB.WbR, gdB.WbQ, math32.Vec3(0.0, 0.0, 0.0)) + plNorm := slmath.MulQuatVector(gdB.WbQ, math32.Vec3(0, 1, 0)) + plPos := slmath.MulSpatialPoint(gdB.WbR, gdB.WbQ, math32.Vec3(0, 0, 0)) // World-space cylinder params - cylCtr := slmath.MulSpatialPoint(gdA.WbR, gdA.WbQ, math32.Vec3(0.0, 0.0, 0.0)) - cylAx := slmath.Normal3(slmath.MulQuatVector(gdA.WbQ, math32.Vec3(0.0, 0.0, 1.0))) + cylCtr := slmath.MulSpatialPoint(gdA.WbR, gdA.WbQ, math32.Vec3(0, 0, 0)) + cylAx := slmath.Normal3(slmath.MulQuatVector(gdA.WbQ, math32.Vec3(0, 1, 0))) cylRad := gdA.Size.X cylHh := gdA.Size.Y @@ -371,7 +372,7 @@ func ColCylinderPlane(cpi, maxIter int32, gdA *GeomData, gdB *GeomData, pA, pB, // vec = wp.where( // len_sqr >= 1e-12, // vec * safe_div(cylinder_radius, wp.sqrt(len_sqr)), - // math32.Vec3(1.0, 0.0, 0.0).MuScalar(cylRad), // Default x-axis when degenerate + // math32.Vec3(1, 0, 0).MuScalar(cylRad), // Default x-axis when degenerate // ) // Project scaled vector on normal diff --git a/physics/shapecollide.goal b/physics/shapecollide.goal index 114e49ee..9174c9ab 100644 --- a/physics/shapecollide.goal +++ b/physics/shapecollide.goal @@ -160,16 +160,16 @@ func ColCapsuleCapsule(cpi, maxIter int32, gdA *GeomData, gdB *GeomData, pA, pB, hhB := gdB.Size.Y // edge from capsule A // depending on point id, we query an edge from 0 to 0.5 or 0.5 to 1 - e0 := math32.Vec3(0.0, 0.0, hhA*float32(cpi%2)) - e1 := math32.Vec3(0.0, 0.0, -hhA*float32((cpi+1)%2)) + e0 := math32.Vec3(0, 0, hhA*float32(cpi%2)) + e1 := math32.Vec3(0, 0, -hhA*float32((cpi+1)%2)) edge0w := slmath.MulSpatialPoint(gdA.WbR, gdA.WbQ, e0) edge1w := slmath.MulSpatialPoint(gdA.WbR, gdA.WbQ, e1) edge0b := slmath.MulSpatialPoint(gdA.BwR, gdA.BwQ, edge0w) edge1b := slmath.MulSpatialPoint(gdA.BwR, gdA.BwQ, edge1w) u := ClosestEdgeCapsule(gdB.Size.X, gdB.Size.Y, edge0b, edge1b, maxIter) pAw := edge0w.MulScalar(1 - u).Add(edge1w.MulScalar(u)) - p0Bw := slmath.MulSpatialPoint(gdB.WbR, gdB.WbQ, math32.Vec3(0.0, 0.0, hhB)) - p1Bw := slmath.MulSpatialPoint(gdB.WbR, gdB.WbQ, math32.Vec3(0.0, 0.0, -hhB)) + p0Bw := slmath.MulSpatialPoint(gdB.WbR, gdB.WbQ, math32.Vec3(0, hhB, 0)) + p1Bw := slmath.MulSpatialPoint(gdB.WbR, gdB.WbQ, math32.Vec3(0, -hhB, 0)) pBw := ClosestPointLineSegment(p0Bw, p1Bw, pAw) diff := pAw.Sub(pBw) *norm = slmath.Normal3(diff) @@ -207,8 +207,8 @@ func ColBoxCapsule(cpi, maxIter int32, gdA *GeomData, gdB *GeomData, pA, pB, nor hhB := gdB.Size.Y // capsule B // depending on point id, we query an edge from 0 to 0.5 or 0.5 to 1 - e0 := math32.Vec3(0.0, 0.0, -hhB*float32(cpi%2)) - e1 := math32.Vec3(0.0, 0.0, hhB*float32((cpi+1)%2)) + e0 := math32.Vec3(0, -hhB*float32(cpi%2), 0) + e1 := math32.Vec3(0, hhB*float32((cpi+1)%2), 0) edge0w := slmath.MulSpatialPoint(gdB.WbR, gdB.WbQ, e0) edge1w := slmath.MulSpatialPoint(gdB.WbR, gdB.WbQ, e1) edge0a := slmath.MulSpatialPoint(gdA.BwR, gdA.BwQ, edge0w) @@ -241,15 +241,16 @@ func ColBoxPlane(cpi, maxIter int32, gdA *GeomData, gdB *GeomData, pA, pB, norm pBody := ClosestPointPlane(width, length, queryB) pBw = slmath.MulSpatialPoint(gdB.WbR, gdB.WbQ, pBody) diff = pAw.Sub(pBw) - *norm = slmath.MulQuatVector(gdB.WbQ, math32.Vec3(0.0, 0.0, 1.0)) - if width > 0.0 && length > 0.0 { + *norm = slmath.MulQuatVector(gdB.WbQ, math32.Vec3(0, 1, 0)) + if width > 0 && length > 0 { if math32.Abs(queryB.X) > width || math32.Abs(queryB.Z) > length { // skip, we will evaluate the plane edge contact with the box later - return 1.0e6 // invalid + return 1e6 // invalid } + // note: commented out in original: // check whether the COM is above the plane // sign = wp.sign(slmath.Dot3(wp.transform_get_translation(gdA.X_ws) - pBw, normal)) - // if sign < 0.0: + // if sign < 0: // // the entire box is most likely below the plane // return } @@ -272,7 +273,7 @@ func ColBoxPlane(cpi, maxIter int32, gdA *GeomData, gdB *GeomData, pA, pB, norm queryB := slmath.MulSpatialPoint(gdA.BwR, gdA.BwQ, pAw) if math32.Abs(queryB.X) > width || math32.Abs(queryB.Z) > length { // ensure that the closest point is actually inside the plane - return 1.0e6 // invalid + return 1e6 // invalid } diff = pAw.Sub(pBw) comA := gdA.WbR @@ -281,7 +282,7 @@ func ColBoxPlane(cpi, maxIter int32, gdA *GeomData, gdB *GeomData, pA, pB, norm // the COM is outside the plane *norm = slmath.Normal3(comA.Sub(pBw)) } else { - *norm = slmath.MulQuatVector(gdB.WbQ, math32.Vec3(0.0, 0.0, 1.0)) + *norm = slmath.MulQuatVector(gdB.WbQ, math32.Vec3(0, 0, 1)) } } *pA = pAw @@ -308,8 +309,8 @@ func ColSphereCapsule(cpi, maxIter int32, gdA *GeomData, gdB *GeomData, pA, pB, pAw := gdA.WbR hhB := gdB.Size.Y // capsule B - AB := slmath.MulSpatialPoint(gdB.WbR, gdB.WbQ, math32.Vec3(0.0, 0.0, hhB)) - BB := slmath.MulSpatialPoint(gdB.WbR, gdB.WbQ, math32.Vec3(0.0, 0.0, -hhB)) + AB := slmath.MulSpatialPoint(gdB.WbR, gdB.WbQ, math32.Vec3(0, hhB, 0)) + BB := slmath.MulSpatialPoint(gdB.WbR, gdB.WbQ, math32.Vec3(0, -hhB, 0)) pBw := ClosestPointLineSegment(AB, BB, pAw) diff := pAw.Sub(pBw) *norm = slmath.Normal3(diff) @@ -324,7 +325,7 @@ func ColSpherePlane(cpi, maxIter int32, gdA *GeomData, gdB *GeomData, pA, pB, no pBody := ClosestPointPlane(gdB.Size.X, gdB.Size.Z, slmath.MulSpatialPoint(gdB.BwR, gdB.BwQ, pAw)) pBw := slmath.MulSpatialPoint(gdB.WbR, gdB.WbQ, pBody) diff := pAw.Sub(pBw) - *norm = slmath.MulQuatVector(gdB.WbQ, math32.Vec3(0.0, 0.0, 1.0)) + *norm = slmath.MulQuatVector(gdB.WbQ, math32.Vec3(0, 1, 0)) *pA = pAw *pB = pBw return slmath.Dot3(diff, *norm) @@ -333,12 +334,12 @@ func ColSpherePlane(cpi, maxIter int32, gdA *GeomData, gdB *GeomData, pA, pB, no // Handle collision between a cylinder (geo_a) and an infinite plane (geo_b). func ColCylinderPlane(cpi, maxIter int32, gdA *GeomData, gdB *GeomData, pA, pB, norm *math32.Vector3) float32 { // World-space plane - plNorm := slmath.MulQuatVector(gdB.WbQ, math32.Vec3(0.0, 1.0, 0.0)) - plPos := slmath.MulSpatialPoint(gdB.WbR, gdB.WbQ, math32.Vec3(0.0, 0.0, 0.0)) + plNorm := slmath.MulQuatVector(gdB.WbQ, math32.Vec3(0, 1, 0)) + plPos := slmath.MulSpatialPoint(gdB.WbR, gdB.WbQ, math32.Vec3(0, 0, 0)) // World-space cylinder params - cylCtr := slmath.MulSpatialPoint(gdA.WbR, gdA.WbQ, math32.Vec3(0.0, 0.0, 0.0)) - cylAx := slmath.Normal3(slmath.MulQuatVector(gdA.WbQ, math32.Vec3(0.0, 0.0, 1.0))) + cylCtr := slmath.MulSpatialPoint(gdA.WbR, gdA.WbQ, math32.Vec3(0, 0, 0)) + cylAx := slmath.Normal3(slmath.MulQuatVector(gdA.WbQ, math32.Vec3(0, 1, 0))) cylRad := gdA.Size.X cylHh := gdA.Size.Y @@ -368,7 +369,7 @@ func ColCylinderPlane(cpi, maxIter int32, gdA *GeomData, gdB *GeomData, pA, pB, // vec = wp.where( // len_sqr >= 1e-12, // vec * safe_div(cylinder_radius, wp.sqrt(len_sqr)), - // math32.Vec3(1.0, 0.0, 0.0).MuScalar(cylRad), // Default x-axis when degenerate + // math32.Vec3(1, 0, 0).MuScalar(cylRad), // Default x-axis when degenerate // ) // Project scaled vector on normal diff --git a/physics/shapegeom.go b/physics/shapegeom.go index c24f4580..b3db709d 100644 --- a/physics/shapegeom.go +++ b/physics/shapegeom.go @@ -89,11 +89,11 @@ func PlaneSDF(width, length float32, p math32.Vector3) float32 { } // ClosestPointPlane projects the point onto the quad in -// the xz plane (if size > 0.0, otherwise infinite. +// the xz plane (if size > 0.0), otherwise infinite. func ClosestPointPlane(width, length float32, pt math32.Vector3) math32.Vector3 { cp := pt + cp.Y = 0 if width == 0.0 { - cp.Y = 0 return cp } cp.X = math32.Clamp(pt.X, -width, width) diff --git a/physics/step.go b/physics/step.go index a1c57cbc..1c1e26e0 100644 --- a/physics/step.go +++ b/physics/step.go @@ -86,7 +86,7 @@ func (wl *World) StepJointForces() { func (wl *World) StepIntegrateBodies() { params := GetParams(0) - RunStepIntegrateBodies(int(params.JointsN)) + RunStepIntegrateBodies(int(params.DynamicsN)) } func (wl *World) StepSolveJoints() { diff --git a/physics/step.goal b/physics/step.goal index 4973ad2c..d2083817 100644 --- a/physics/step.goal +++ b/physics/step.goal @@ -84,7 +84,7 @@ func (wl *World) StepJointForces() { func (wl *World) StepIntegrateBodies() { params := GetParams(0) - RunStepIntegrateBodies(int(params.JointsN)) + RunStepIntegrateBodies(int(params.DynamicsN)) } func (wl *World) StepSolveJoints() { diff --git a/physics/step_body.go b/physics/step_body.go index 534c6bcd..ae9d69ab 100644 --- a/physics/step_body.go +++ b/physics/step_body.go @@ -110,8 +110,12 @@ func DeltasFromJoints(i uint32) { //gosl:kernel a := JointCAngDelta(ji) ta = ta.Add(a) } - SetDynamicDelta(di, params.Next, td) - SetDynamicAngDelta(di, params.Next, ta) + v0 := DynamicDelta(di, params.Next) + w0 := DynamicAngDelta(di, params.Next) + // fmt.Println(params.Next, "joint v:", v0, td) + + SetDynamicDelta(di, params.Next, td.Add(v0)) + SetDynamicAngDelta(di, params.Next, ta.Add(w0)) } // newton: solvers/solver.py: integrate_rigid_body @@ -162,6 +166,8 @@ func StepIntegrateBodies(i uint32) { //gosl:kernel p1a := p1.Sub(slmath.MulQuatVector(q1, com)) // pos corrected to nominal center. + // fmt.Println(params.Next, "integrate:", v0, v1) + SetDynamicPos(di, params.Next, p1a) SetDynamicQuat(di, params.Next, q1) SetDynamicDelta(di, params.Next, v1) @@ -195,11 +201,10 @@ func StepBodyDeltas(i uint32) { //gosl:kernel w0 := DynamicAngDelta(di, params.Next) weight := float32(1.0) - // todo: this is rigid_contact_inv_weight in solver_xpbd.py: from contacts, with restitution - // if constraint_inv_weights: - // inv_weight = constraint_inv_weights[tid] - // if inv_weight > 0.0: - // weight = 1.0 / inv_weight + cWt := Dynamics.Value(int(di), int(params.Next), int(DynContactWeight)) + if cWt > 0 { + weight = 1.0 / cWt + } dp := v0.MulScalar(invMass * weight) dq := w0.MulScalar(weight) @@ -234,6 +239,8 @@ func StepBodyDeltas(i uint32) { //gosl:kernel w1 = math32.Vec3(0, 0, 0) } + // fmt.Println(params.Next, "delta:", v0, v1) + SetDynamicPos(di, params.Next, p1) SetDynamicQuat(di, params.Next, q1) SetDynamicDelta(di, params.Next, v1) diff --git a/physics/step_body.goal b/physics/step_body.goal index 052899da..5f387892 100644 --- a/physics/step_body.goal +++ b/physics/step_body.goal @@ -108,8 +108,12 @@ func DeltasFromJoints(i uint32) { //gosl:kernel a := JointCAngDelta(ji) ta = ta.Add(a) } - SetDynamicDelta(di, params.Next, td) - SetDynamicAngDelta(di, params.Next, ta) + v0 := DynamicDelta(di, params.Next) + w0 := DynamicAngDelta(di, params.Next) + // fmt.Println(params.Next, "joint v:", v0, td) + + SetDynamicDelta(di, params.Next, td.Add(v0)) + SetDynamicAngDelta(di, params.Next, ta.Add(w0)) } // newton: solvers/solver.py: integrate_rigid_body @@ -146,7 +150,7 @@ func StepIntegrateBodies(i uint32) { //gosl:kernel // linear part v1 := v0.Add(f0.MulScalar(invMass).Add(params.Gravity.V().MulScalar(OneIfNonzero(invMass))).MulScalar(params.Dt)) p1 := pcom.Add(v1.MulScalar(params.Dt)) - + // angular part (compute in body frame) wb := slmath.MulQuatVectorInverse(q0, w0) tb := slmath.MulQuatVectorInverse(q0, t0).Sub(slmath.Cross3(wb, inertia.MulVector3(wb))) // coriolis forces @@ -160,6 +164,8 @@ func StepIntegrateBodies(i uint32) { //gosl:kernel p1a := p1.Sub(slmath.MulQuatVector(q1, com)) // pos corrected to nominal center. + // fmt.Println(params.Next, "integrate:", v0, v1) + SetDynamicPos(di, params.Next, p1a) SetDynamicQuat(di, params.Next, q1) SetDynamicDelta(di, params.Next, v1) @@ -193,11 +199,10 @@ func StepBodyDeltas(i uint32) { //gosl:kernel w0 := DynamicAngDelta(di, params.Next) weight := float32(1.0) - // todo: this is rigid_contact_inv_weight in solver_xpbd.py: from contacts, with restitution - // if constraint_inv_weights: - // inv_weight = constraint_inv_weights[tid] - // if inv_weight > 0.0: - // weight = 1.0 / inv_weight + cWt := Dynamics[di, params.Next, DynContactWeight] + if cWt > 0 { + weight = 1.0 / cWt + } dp := v0.MulScalar(invMass * weight) dq := w0.MulScalar(weight) @@ -231,6 +236,8 @@ func StepBodyDeltas(i uint32) { //gosl:kernel if slmath.Length3(w1) < 1e-4 { w1 = math32.Vec3(0, 0, 0) } + + // fmt.Println(params.Next, "delta:", v0, v1) SetDynamicPos(di, params.Next, p1) SetDynamicQuat(di, params.Next, q1) diff --git a/physics/world.go b/physics/world.go index 419b383e..969c82fc 100644 --- a/physics/world.go +++ b/physics/world.go @@ -111,7 +111,8 @@ func (wl *World) NewBody(shape Shapes, size, pos math32.Vector3, rot math32.Quat SetBodyHSize(idx, size) SetBodyPos(idx, pos) SetBodyQuat(idx, rot) - SetBodyGroup(idx, -1) // assume static + SetBodyGroup(idx, -1) // assume static + wl.SetMass(idx, shape, size, 0) // assume static return idx } diff --git a/physics/world.goal b/physics/world.goal index ff32efe8..bd2d5053 100644 --- a/physics/world.goal +++ b/physics/world.goal @@ -109,7 +109,8 @@ func (wl *World) NewBody(shape Shapes, size, pos math32.Vector3, rot math32.Quat SetBodyHSize(idx, size) SetBodyPos(idx, pos) SetBodyQuat(idx, rot) - SetBodyGroup(idx, -1) // assume static + SetBodyGroup(idx, -1) // assume static + wl.SetMass(idx, shape, size, 0) // assume static return idx } From 6c8b880396972ad796c22075db1211e3141c66a7 Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Mon, 22 Dec 2025 10:53:27 +0100 Subject: [PATCH 38/97] physics: gosl formatting for vars improved: outputs newline --- gosl/gotosl/testdata/Compute.golden | 17 ++++++++++------- gosl/gotosl/testdata/CycleUpdt.golden | 2 ++ 2 files changed, 12 insertions(+), 7 deletions(-) diff --git a/gosl/gotosl/testdata/Compute.golden b/gosl/gotosl/testdata/Compute.golden index e0eebc6c..a3abc2cf 100644 --- a/gosl/gotosl/testdata/Compute.golden +++ b/gosl/gotosl/testdata/Compute.golden @@ -127,7 +127,8 @@ const Integ: i32 = 1; const Exp: i32 = 2; const NVars: i32 = 3; fn TransformPoint(xP: vec3, xQ: vec4, p: vec3) -> vec3 { - var dp = MulQuatVector(xQ, p);return dp+(xP); + var dp = MulQuatVector(xQ, p); +return dp+(xP); } fn MulTransforms(aP: vec3, aQ: vec4, bP: vec3, bQ: vec4, oP: ptr>, oQ: ptr>) { var br = MulQuatVector(aQ, bP); @@ -195,7 +196,8 @@ fn ParamStruct_IntegFromRaw(ps: ParamStruct, idx: i32) -> f32 { u32(idx), u32(Exp))]; } fn ParamStruct_AnotherMeth(ps: ParamStruct, ctx: Context, idx: i32, ptrarg: ptr) { - for (var i = 0; i < 10; i++) { + for (var i = 0; + i < 10; i++) { Data[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(Integ))] *= 0.99; } var flag: NeuronFlags; @@ -244,20 +246,21 @@ fn Compute(i: u32) { //gosl:kernel //////// import: "slmath-quaternion.go" fn MulQuatVector(q: vec4, v: vec3) -> vec3 { var xyz = vec3(q.x, q.y, q.z); - var t = MulScalar3(Cross3(xyz, v), f32(f32(2)));return v+(MulScalar3(t, q.w))+(Cross3(xyz, t)); + var t = Cross3(xyz, v)*(2); +return v+(t*(q.w))+(Cross3(xyz, t)); } fn MulQuats(a: vec4,b: vec4) -> vec4 { var q: vec4; q.x = a.x*b.w + a.w*b.x + a.y*b.z - a.z*b.y; q.y = a.y*b.w + a.w*b.y + a.z*b.x - a.x*b.z; q.z = a.z*b.w + a.w*b.z + a.x*b.y - a.y*b.x; - q.w = a.w*b.w - a.x*b.x - a.y*b.y - a.z*b.z;return q; + q.w = a.w*b.w - a.x*b.x - a.y*b.y - a.z*b.z; +return q; } +//////// import: "slmath-vector2.go" + //////// import: "slmath-vector3.go" -fn MulScalar3(v: vec3, s: f32) -> vec3 { - return vec3(v.x*s, v.y*s, v.z*s); -} fn Cross3(v: vec3,o: vec3) -> vec3 { return vec3(v.y*o.z-v.z*o.y, v.z*o.x-v.x*o.z, v.x*o.y-v.y*o.x); } \ No newline at end of file diff --git a/gosl/gotosl/testdata/CycleUpdt.golden b/gosl/gotosl/testdata/CycleUpdt.golden index 1de6f72b..9ce275b4 100644 --- a/gosl/gotosl/testdata/CycleUpdt.golden +++ b/gosl/gotosl/testdata/CycleUpdt.golden @@ -70,4 +70,6 @@ fn CycleUpdt(i: u32) { //gosl:kernel read-write:Ctx //////// import: "slmath-quaternion.go" +//////// import: "slmath-vector2.go" + //////// import: "slmath-vector3.go" \ No newline at end of file From ed81dfeec62a01dc8cd6e12c68b69640241aca16 Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Mon, 22 Dec 2025 13:08:59 +0100 Subject: [PATCH 39/97] physics: gosl major fix: include generated gosl.go file in imports, to resolve all the var access so it knows what kind of vars there are -- should be no package build errors anymore. --- gosl/gotosl/extract.go | 1 + gosl/gotosl/gengpu.go | 22 +- gosl/gotosl/gotosl.go | 2 +- gosl/gotosl/nodes.go | 49 +- gosl/gotosl/translate.go | 16 + physics/contact.go | 85 ++- physics/contact.goal | 88 ++- physics/params.go | 8 +- physics/shaders/CollisionBroad.wgsl | 4 +- physics/shaders/CollisionNarrow.wgsl | 40 +- physics/shaders/DeltasFromContacts.wgsl | 10 +- physics/shaders/DeltasFromJoints.wgsl | 12 +- physics/shaders/StepBodyContacts.wgsl | 102 ++- physics/shaders/StepBodyDeltas.wgsl | 8 +- physics/shaders/StepIntegrateBodies.wgsl | 4 +- physics/shaders/imports/gosl.go | 927 +++++++++++++++++++++++ physics/step_body.go | 15 +- physics/step_body.goal | 15 +- physics/typegen.go | 2 +- physics/vars.go | 2 +- 20 files changed, 1280 insertions(+), 132 deletions(-) create mode 100644 physics/shaders/imports/gosl.go diff --git a/gosl/gotosl/extract.go b/gosl/gotosl/extract.go index 559ba965..681e2231 100644 --- a/gosl/gotosl/extract.go +++ b/gosl/gotosl/extract.go @@ -156,6 +156,7 @@ func (st *State) AppendGoHeader(lines [][]byte) [][]byte { olns = append(olns, []byte("package imports")) olns = append(olns, []byte(`import ( "math" + "sync/atomic" "cogentcore.org/core/math32" "cogentcore.org/lab/gosl/slbool" "cogentcore.org/lab/gosl/slrand" diff --git a/gosl/gotosl/gengpu.go b/gosl/gotosl/gengpu.go index 4e6afa55..2b2adcf0 100644 --- a/gosl/gotosl/gengpu.go +++ b/gosl/gotosl/gengpu.go @@ -7,6 +7,7 @@ package gotosl import ( "fmt" "os" + "path/filepath" "slices" "strings" @@ -28,8 +29,9 @@ func (st *State) genSysVar(sy *System) string { return fmt.Sprintf("GPU%sSystem", st.genSysName(sy)) } -// GenGPU generates and writes the Go GPU helper code -func (st *State) GenGPU() { +// GenGPU generates and writes the Go GPU helper code. +// if imports then generates in imports directory. +func (st *State) GenGPU(imports bool) { var b strings.Builder header := `// Code generated by "gosl"; DO NOT EDIT @@ -45,8 +47,7 @@ import ( "cogentcore.org/lab/tensor" ) -//go:embed %s/*.wgsl -var shaders embed.FS +%s var ( // GPUInitialized is true once the GPU system has been initialized. @@ -67,7 +68,15 @@ var ( ) ` - b.WriteString(fmt.Sprintf(header, st.Package, st.Config.Output)) + pkg := st.Package + shaders := fmt.Sprintf(`//go:embed %s/*.wgsl +var shaders embed.FS`, st.Config.Output) + + if imports { + shaders = `var shaders embed.FS` + pkg = "imports" + } + b.WriteString(fmt.Sprintf(header, pkg, shaders)) sys := maps.Keys(st.Systems) slices.Sort(sys) @@ -179,6 +188,9 @@ func GPURelease() { gs := b.String() fn := "gosl.go" + if imports { + fn = filepath.Join(st.Config.Output, "imports", fn) + } os.WriteFile(fn, []byte(gs), 0644) } diff --git a/gosl/gotosl/gotosl.go b/gosl/gotosl/gotosl.go index 37529e8a..5374a1eb 100644 --- a/gosl/gotosl/gotosl.go +++ b/gosl/gotosl/gotosl.go @@ -298,7 +298,7 @@ func (st *State) Run() error { st.ExtractImports() // get .go from imports st.TranslateDir("./" + st.ImportsDir) - st.GenGPU() + st.GenGPU(false) return nil } diff --git a/gosl/gotosl/nodes.go b/gosl/gotosl/nodes.go index df35c0c8..46520d01 100644 --- a/gosl/gotosl/nodes.go +++ b/gosl/gotosl/nodes.go @@ -1601,14 +1601,16 @@ func (p *printer) selectorPath(x *ast.SelectorExpr) (recvPath, recvType string, } recvPath = baseRecv.Name var idt types.Type - if gvar := p.GoToSL.GetTempVar(baseRecv.Name); gvar != nil { + gvar := p.GoToSL.GetTempVar(baseRecv.Name) + if gvar != nil { idt = p.getTypeNameType(gvar.Var.SLType()) } else { idt = p.getIdType(baseRecv) } - if idt == nil { - err = fmt.Errorf("gosl methodPath ERROR: cannot find type for name: %q", baseRecv.Name) - p.userError(err) + if idt == nil || typeIsInvalid(idt) { + err = fmt.Errorf("gosl methodPath ERROR: cannot find type for name: %q, gvar: %v", baseRecv.Name, gvar) + panic(err) + // p.userError(err) return } bt, err = p.getStructType(idt) @@ -1681,25 +1683,30 @@ func getLocalTypeName(typ types.Type) string { return nm } -func (p *printer) getStructType(typ types.Type) (*types.Struct, error) { - typ = typ.Underlying() - if st, ok := typ.(*types.Struct); ok { - return st, nil - } - if ptr, ok := typ.(*types.Pointer); ok { - typ = ptr.Elem().Underlying() - if st, ok := typ.(*types.Struct); ok { - return st, nil - } - } - if sl, ok := typ.(*types.Slice); ok { - typ = sl.Elem().Underlying() - if st, ok := typ.(*types.Struct); ok { - return st, nil +func typeIsInvalid(typ types.Type) bool { + if b, ok := typ.(*types.Basic); ok { + if b.Kind() == types.Invalid { + return true } } - err := fmt.Errorf("gosl ERROR: type is not a struct and it should be: %q %+t", typ.String(), typ) - p.userError(err) + return false +} + +func (p *printer) getStructType(typ types.Type) (*types.Struct, error) { + utyp := typ.Underlying() + switch x := utyp.(type) { + case *types.Struct: + return x, nil + case *types.Pointer: + return p.getStructType(x.Elem()) + case *types.Slice: + return p.getStructType(x.Elem()) + case *types.Basic: + fmt.Println("basic kind:", x.String()) + } + err := fmt.Errorf("gosl ERROR: type is not a struct and it should be: %q %+T %+T", typ.String(), typ, utyp) + panic(err) + // p.userError(err) return nil, err } diff --git a/gosl/gotosl/translate.go b/gosl/gotosl/translate.go index 3d1a71cf..c5f01f92 100644 --- a/gosl/gotosl/translate.go +++ b/gosl/gotosl/translate.go @@ -78,11 +78,21 @@ func (st *State) TranslateDir(pf string) error { var buf bytes.Buffer doFile(fn, &buf) } + + st.GenGPU(true) // generate an initial gosl.go in imports, so Go doesn't get confused + + pkgs, err = packages.Load(&packages.Config{Mode: packages.NeedName | packages.NeedFiles | packages.NeedTypes | packages.NeedSyntax | packages.NeedTypesSizes | packages.NeedTypesInfo}, pf) + pkg = pkgs[0] + files = pkg.GoFiles + for _, gofp := range files { _, gofn := filepath.Split(gofp) if _, ok := st.GoVarsFiles[gofn]; ok { continue } + if gofn == "gosl.go" { + continue + } var buf bytes.Buffer doFile(gofp, &buf) } @@ -144,6 +154,9 @@ func (st *State) TranslateDir(pf string) error { if _, ok := st.GoVarsFiles[gofn]; ok { continue } + if gofn == "gosl.go" { + continue + } lines, hasR, hasT = doKernelFile(gofp, lines) if hasR { hasSlrand = true @@ -160,6 +173,9 @@ func (st *State) TranslateDir(pf string) error { st.CopyPackageFile("sltype.wgsl", "cogentcore.org/lab/gosl/sltype") } for _, im := range st.SLImportFiles { + if im.Name == "gosl.go" { + continue + } lines = append(lines, []byte("")) lines = append(lines, []byte(fmt.Sprintf("//////// import: %q", im.Name))) lines = append(lines, im.Lines...) diff --git a/physics/contact.go b/physics/contact.go index 1bbe444b..9b161395 100644 --- a/physics/contact.go +++ b/physics/contact.go @@ -460,19 +460,19 @@ func StepBodyContacts(i uint32) { //gosl:kernel diA := GetBodyDynamic(biA) diB := GetBodyDynamic(biB) - r0A := BodyDynamicPos(biA, params.Next) - q0A := BodyDynamicQuat(biA, params.Next) + r1A := BodyDynamicPos(biA, params.Next) + q1A := BodyDynamicQuat(biA, params.Next) - r0B := BodyDynamicPos(biB, params.Next) - q0B := BodyDynamicQuat(biB, params.Next) + r1B := BodyDynamicPos(biB, params.Next) + q1B := BodyDynamicQuat(biB, params.Next) ctA := ContactAPoint(ci) offA := ContactAOff(ci) ctB := ContactBPoint(ci) offB := ContactBOff(ci) - ctAw := slmath.MulSpatialPoint(r0A, q0A, ctA) - ctBw := slmath.MulSpatialPoint(r0B, q0B, ctB) + ctAw := slmath.MulSpatialPoint(r1A, q1A, ctA) + ctBw := slmath.MulSpatialPoint(r1B, q1B, ctB) thickA := Contacts.Value(int(ci), int(ContactAThick)) thickB := Contacts.Value(int(ci), int(ContactBThick)) thick := thickA + thickB @@ -497,27 +497,28 @@ func StepBodyContacts(i uint32) { //gosl:kernel mInvB := Bodies.Value(int(biB), int(BodyInvMass)) iInvB := BodyInvInertia(biB) - var w0A, w0B math32.Vector3 + var w1A, w1B math32.Vector3 if diA >= 0 { - w0A = DynamicAngDelta(diA, params.Next) + w1A = DynamicAngDelta(diA, params.Next) } if diB >= 0 { - w0B = DynamicAngDelta(diB, params.Next) + w1B = DynamicAngDelta(diB, params.Next) } // use average contact material properties mu := 0.5 * (Bodies.Value(int(biA), int(BodyFriction)) + Bodies.Value(int(biB), int(BodyFriction))) frTors := 0.5 * (Bodies.Value(int(biA), int(BodyFrictionTortion)) + Bodies.Value(int(biB), int(BodyFrictionTortion))) frRoll := 0.5 * (Bodies.Value(int(biA), int(BodyFrictionRolling)) + Bodies.Value(int(biB), int(BodyFrictionRolling))) + bounce := 0.5 * (Bodies.Value(int(biA), int(BodyBounce)) + Bodies.Value(int(biB), int(BodyBounce))) // moment arms - dA := ctAw.Sub(slmath.MulSpatialPoint(r0A, q0A, comA)) - dB := ctBw.Sub(slmath.MulSpatialPoint(r0B, q0B, comB)) + dA := ctAw.Sub(slmath.MulSpatialPoint(r1A, q1A, comA)) + dB := ctBw.Sub(slmath.MulSpatialPoint(r1B, q1B, comB)) angA := slmath.Negate3(slmath.Cross3(dA, norm)) angB := slmath.Cross3(dB, norm) - lambdaN := ContactConstraint(d, q0A, q0B, mInvA, mInvB, iInvA, iInvB, nnorm, norm, angA, angB, params.ContactRelax, params.Dt) + lambdaN := ContactConstraint(d, q1A, q1B, mInvA, mInvB, iInvA, iInvB, nnorm, norm, angA, angB, params.ContactRelax, params.Dt) linDeltaA := slmath.Negate3(norm).MulScalar(lambdaN) linDeltaB := norm.MulScalar(lambdaN) @@ -530,8 +531,8 @@ func StepBodyContacts(i uint32) { //gosl:kernel // we include any rotational effects due to thickness from feature // need to use the current rotation to account for friction due to // angular effects (e.g.: slipping contact) - ctAw = ctAw.Add(slmath.MulQuatVector(q0A, offA)) - ctBw = ctBw.Add(slmath.MulQuatVector(q0B, offB)) + ctAw = ctAw.Add(slmath.MulQuatVector(q1A, offA)) + ctBw = ctBw.Add(slmath.MulQuatVector(q1B, offB)) // update delta delta := ctBw.Sub(ctAw) @@ -545,7 +546,7 @@ func StepBodyContacts(i uint32) { //gosl:kernel err := slmath.Length3(frDelta) if err > 0.0 { - lambdaFr := ContactConstraint(err, q0A, q0B, mInvA, mInvB, iInvA, iInvB, slmath.Negate3(perp), perp, angA, angB, params.ContactRelax, params.Dt) + lambdaFr := ContactConstraint(err, q1A, q1B, mInvA, mInvB, iInvA, iInvB, slmath.Negate3(perp), perp, angA, angB, params.ContactRelax, params.Dt) // limit friction based on incremental normal force, // good approximation to limiting on total force @@ -558,14 +559,14 @@ func StepBodyContacts(i uint32) { //gosl:kernel } } - deltaW := w0B.Sub(w0A) + deltaW := w1B.Sub(w1A) if frTors > 0.0 { err := slmath.Dot3(deltaW, norm) * params.Dt if math32.Abs(err) > 0.0 { lin := math32.Vec3(0, 0, 0) - lambdaTors := ContactConstraint(err, q0A, q0B, mInvA, mInvB, iInvA, iInvB, lin, lin, nnorm, norm, params.ContactRelax, params.Dt) + lambdaTors := ContactConstraint(err, q1A, q1B, mInvA, mInvB, iInvA, iInvB, lin, lin, nnorm, norm, params.ContactRelax, params.Dt) lambdaTors = math32.Clamp(lambdaTors, -lambdaN*frTors, lambdaN*frTors) angDeltaA = angDeltaA.Sub(norm.MulScalar(lambdaTors)) @@ -579,7 +580,7 @@ func StepBodyContacts(i uint32) { //gosl:kernel if err > 0.0 { lin := math32.Vec3(0, 0, 0) rollN := slmath.Normal3(deltaW) - lambdaRoll := ContactConstraint(err, q0A, q0B, mInvA, mInvB, iInvA, iInvB, lin, lin, slmath.Negate3(rollN), rollN, params.ContactRelax, params.Dt) + lambdaRoll := ContactConstraint(err, q1A, q1B, mInvA, mInvB, iInvA, iInvB, lin, lin, slmath.Negate3(rollN), rollN, params.ContactRelax, params.Dt) lambdaRoll = max(lambdaRoll, -lambdaN*frRoll) angDeltaA = angDeltaA.Sub(rollN.MulScalar(lambdaRoll)) @@ -587,6 +588,54 @@ func StepBodyContacts(i uint32) { //gosl:kernel } } + // restitution (bounce) + if params.Restitution.IsTrue() && bounce > 0 && (mInvA > 0 || mInvB > 0) { + var vA, vB, vAnew, vBnew, dAnew, dBnew math32.Vector3 + var mInvAr, mInvBr float32 + var q0A, q0B math32.Quat + grav := params.Gravity.V().MulScalar(params.Dt) + if diA >= 0 { + q0A = DynamicQuat(diA, params.Cur) + w0A := DynamicAngDelta(diA, params.Cur) + v0A := DynamicDelta(diA, params.Cur) + v1A := DynamicDelta(diA, params.Next) + + vA = VelocityAtPoint(v0A, w0A, dA).Add(grav) + vAnew = VelocityAtPoint(v1A, w1A, dA) + dAnew = slmath.MulQuatVectorInverse(q0A, slmath.Cross3(dA, nnorm)) // norm is not - here.. + mInvAr = mInvA + slmath.Dot3(dAnew, iInvA.MulVector3(dAnew)) + } + if diB >= 0 { + q0B = DynamicQuat(diB, params.Cur) + w0B := DynamicAngDelta(diB, params.Cur) + v0B := DynamicDelta(diB, params.Cur) + v1B := DynamicDelta(diB, params.Next) + + vB = VelocityAtPoint(v0B, w0B, dB).Add(grav) + vBnew = VelocityAtPoint(v1B, w1B, dB) + dBnew = slmath.MulQuatVectorInverse(q0B, slmath.Cross3(dB, nnorm)) // norm is not - here.. + mInvBr = mInvB + slmath.Dot3(dBnew, iInvB.MulVector3(dBnew)) + } + mInv := mInvAr + mInvBr + relVel0 := slmath.Dot3(nnorm, vA.Sub(vB)) + relVel1 := slmath.Dot3(nnorm, vAnew.Sub(vBnew)) + if relVel0 < 0 { + dv := -(relVel1 - relVel0*bounce) / mInv + if diA >= 0 { + dvA := nnorm.MulScalar(mInvA * dv) + dwA := slmath.MulQuatVector(q0A, iInvA.MulVector3(dAnew).MulScalar(dv)) + linDeltaA = linDeltaA.Add(dvA) + angDeltaA = angDeltaA.Add(dwA) + } + if diB >= 0 { + dvB := nnorm.MulScalar(mInvB * dv) + dwB := slmath.MulQuatVector(q0B, iInvB.MulVector3(dBnew).MulScalar(dv)) + linDeltaB = linDeltaB.Add(dvB) + angDeltaB = angDeltaB.Add(dwB) + } + } + } + Contacts.Set(1.0, int(ci), int(ContactWeight)) SetContactADelta(ci, linDeltaA) SetContactBDelta(ci, linDeltaB) diff --git a/physics/contact.goal b/physics/contact.goal index 78e27203..95f7faab 100644 --- a/physics/contact.goal +++ b/physics/contact.goal @@ -458,19 +458,19 @@ func StepBodyContacts(i uint32) { //gosl:kernel diA := GetBodyDynamic(biA) diB := GetBodyDynamic(biB) - r0A := BodyDynamicPos(biA, params.Next) - q0A := BodyDynamicQuat(biA, params.Next) + r1A := BodyDynamicPos(biA, params.Next) + q1A := BodyDynamicQuat(biA, params.Next) - r0B := BodyDynamicPos(biB, params.Next) - q0B := BodyDynamicQuat(biB, params.Next) + r1B := BodyDynamicPos(biB, params.Next) + q1B := BodyDynamicQuat(biB, params.Next) ctA := ContactAPoint(ci) offA := ContactAOff(ci) ctB := ContactBPoint(ci) offB := ContactBOff(ci) - ctAw := slmath.MulSpatialPoint(r0A, q0A, ctA) - ctBw := slmath.MulSpatialPoint(r0B, q0B, ctB) + ctAw := slmath.MulSpatialPoint(r1A, q1A, ctA) + ctBw := slmath.MulSpatialPoint(r1B, q1B, ctB) thickA := Contacts[ci, ContactAThick] thickB := Contacts[ci, ContactBThick] thick := thickA + thickB @@ -495,27 +495,28 @@ func StepBodyContacts(i uint32) { //gosl:kernel mInvB := Bodies[biB, BodyInvMass] iInvB := BodyInvInertia(biB) - var w0A, w0B math32.Vector3 + var w1A, w1B math32.Vector3 if diA >= 0 { - w0A = DynamicAngDelta(diA, params.Next) + w1A = DynamicAngDelta(diA, params.Next) } if diB >= 0 { - w0B = DynamicAngDelta(diB, params.Next) + w1B = DynamicAngDelta(diB, params.Next) } // use average contact material properties mu := 0.5 * (Bodies[biA, BodyFriction] + Bodies[biB, BodyFriction]) frTors := 0.5 * (Bodies[biA, BodyFrictionTortion] + Bodies[biB, BodyFrictionTortion]) frRoll := 0.5 * (Bodies[biA, BodyFrictionRolling] + Bodies[biB, BodyFrictionRolling]) + bounce := 0.5 * (Bodies[biA, BodyBounce] + Bodies[biB, BodyBounce]) // moment arms - dA := ctAw.Sub(slmath.MulSpatialPoint(r0A, q0A, comA)) - dB := ctBw.Sub(slmath.MulSpatialPoint(r0B, q0B, comB)) + dA := ctAw.Sub(slmath.MulSpatialPoint(r1A, q1A, comA)) + dB := ctBw.Sub(slmath.MulSpatialPoint(r1B, q1B, comB)) angA := slmath.Negate3(slmath.Cross3(dA, norm)) angB := slmath.Cross3(dB, norm) - lambdaN := ContactConstraint(d, q0A, q0B, mInvA, mInvB, iInvA, iInvB, nnorm, norm, angA, angB, params.ContactRelax, params.Dt) + lambdaN := ContactConstraint(d, q1A, q1B, mInvA, mInvB, iInvA, iInvB, nnorm, norm, angA, angB, params.ContactRelax, params.Dt) linDeltaA := slmath.Negate3(norm).MulScalar(lambdaN) linDeltaB := norm.MulScalar(lambdaN) @@ -528,8 +529,8 @@ func StepBodyContacts(i uint32) { //gosl:kernel // we include any rotational effects due to thickness from feature // need to use the current rotation to account for friction due to // angular effects (e.g.: slipping contact) - ctAw = ctAw.Add(slmath.MulQuatVector(q0A, offA)) - ctBw = ctBw.Add(slmath.MulQuatVector(q0B, offB)) + ctAw = ctAw.Add(slmath.MulQuatVector(q1A, offA)) + ctBw = ctBw.Add(slmath.MulQuatVector(q1B, offB)) // update delta delta := ctBw.Sub(ctAw) @@ -543,7 +544,7 @@ func StepBodyContacts(i uint32) { //gosl:kernel err := slmath.Length3(frDelta) if err > 0.0 { - lambdaFr := ContactConstraint(err, q0A, q0B, mInvA, mInvB, iInvA, iInvB, slmath.Negate3(perp), perp, angA, angB, params.ContactRelax, params.Dt) + lambdaFr := ContactConstraint(err, q1A, q1B, mInvA, mInvB, iInvA, iInvB, slmath.Negate3(perp), perp, angA, angB, params.ContactRelax, params.Dt) // limit friction based on incremental normal force, // good approximation to limiting on total force @@ -556,14 +557,14 @@ func StepBodyContacts(i uint32) { //gosl:kernel } } - deltaW := w0B.Sub(w0A) + deltaW := w1B.Sub(w1A) if frTors > 0.0 { err := slmath.Dot3(deltaW, norm) * params.Dt if math32.Abs(err) > 0.0 { lin := math32.Vec3(0,0,0) - lambdaTors := ContactConstraint(err, q0A, q0B, mInvA, mInvB, iInvA, iInvB, lin, lin, nnorm, norm, params.ContactRelax, params.Dt) + lambdaTors := ContactConstraint(err, q1A, q1B, mInvA, mInvB, iInvA, iInvB, lin, lin, nnorm, norm, params.ContactRelax, params.Dt) lambdaTors = math32.Clamp(lambdaTors, -lambdaN * frTors, lambdaN * frTors) angDeltaA = angDeltaA.Sub(norm.MulScalar(lambdaTors)) @@ -577,14 +578,63 @@ func StepBodyContacts(i uint32) { //gosl:kernel if err > 0.0 { lin := math32.Vec3(0,0,0) rollN := slmath.Normal3(deltaW) - lambdaRoll := ContactConstraint(err, q0A, q0B, mInvA, mInvB, iInvA, iInvB, lin, lin, slmath.Negate3(rollN), rollN, params.ContactRelax, params.Dt) + lambdaRoll := ContactConstraint(err, q1A, q1B, mInvA, mInvB, iInvA, iInvB, lin, lin, slmath.Negate3(rollN), rollN, params.ContactRelax, params.Dt) lambdaRoll = max(lambdaRoll, -lambdaN * frRoll) angDeltaA = angDeltaA.Sub(rollN.MulScalar(lambdaRoll)) angDeltaB = angDeltaB.Add(rollN.MulScalar(lambdaRoll)) } } - + + // restitution (bounce) + if params.Restitution.IsTrue() && bounce > 0 && (mInvA > 0 || mInvB > 0) { + var vA, vB, vAnew, vBnew, dAnew, dBnew math32.Vector3 + var mInvAr, mInvBr float32 + var q0A, q0B math32.Quat + grav := params.Gravity.V().MulScalar(params.Dt) + if diA >= 0 { + q0A = DynamicQuat(diA, params.Cur) + w0A := DynamicAngDelta(diA, params.Cur) + v0A := DynamicDelta(diA, params.Cur) + v1A := DynamicDelta(diA, params.Next) + + vA = VelocityAtPoint(v0A, w0A, dA).Add(grav) + vAnew = VelocityAtPoint(v1A, w1A, dA) + dAnew = slmath.MulQuatVectorInverse(q0A, slmath.Cross3(dA, nnorm)) // norm is not - here.. + mInvAr = mInvA + slmath.Dot3(dAnew, iInvA.MulVector3(dAnew)) + } + if diB >= 0 { + q0B = DynamicQuat(diB, params.Cur) + w0B := DynamicAngDelta(diB, params.Cur) + v0B := DynamicDelta(diB, params.Cur) + v1B := DynamicDelta(diB, params.Next) + + vB = VelocityAtPoint(v0B, w0B, dB).Add(grav) + vBnew = VelocityAtPoint(v1B, w1B, dB) + dBnew = slmath.MulQuatVectorInverse(q0B, slmath.Cross3(dB, nnorm)) // norm is not - here.. + mInvBr = mInvB + slmath.Dot3(dBnew, iInvB.MulVector3(dBnew)) + } + mInv := mInvAr + mInvBr + relVel0 := slmath.Dot3(nnorm, vA.Sub(vB)) + relVel1 := slmath.Dot3(nnorm, vAnew.Sub(vBnew)) + if relVel0 < 0 { + dv := -(relVel1 - relVel0 * bounce) / mInv + if diA >= 0 { + dvA := nnorm.MulScalar(mInvA * dv) + dwA := slmath.MulQuatVector(q0A, iInvA.MulVector3(dAnew).MulScalar(dv)) + linDeltaA = linDeltaA.Add(dvA) + angDeltaA = angDeltaA.Add(dwA) + } + if diB >= 0 { + dvB := nnorm.MulScalar(mInvB * dv) + dwB := slmath.MulQuatVector(q0B, iInvB.MulVector3(dBnew).MulScalar(dv)) + linDeltaB = linDeltaB.Add(dvB) + angDeltaB = angDeltaB.Add(dwB) + } + } + } + + Contacts[ci, ContactWeight] = 1.0 SetContactADelta(ci, linDeltaA) SetContactBDelta(ci, linDeltaB) diff --git a/physics/params.go b/physics/params.go index 11cdc537..6271f2bb 100644 --- a/physics/params.go +++ b/physics/params.go @@ -40,11 +40,11 @@ type PhysParams struct { // AngularDamping is damping of angular motion. AngularDamping float32 `default:"0"` - // Contact weighting: todo: requires atomic add + // Contact weighting: balances contact forces? ContactWeighting slbool.Bool `default:"true"` - // Restitution - Restitution slbool.Bool `default:"false"` + // Restitution takes into account bounciness of objects. + Restitution slbool.Bool `default:"true"` // MaxGeomIter is number of iterations to perform in shape-based // geometry collision computations @@ -101,7 +101,7 @@ func (pr *PhysParams) Defaults() { pr.AngularDamping = 0 pr.MaxGeomIter = 10 pr.ContactWeighting.SetBool(true) - pr.Restitution.SetBool(false) + pr.Restitution.SetBool(true) pr.ContactMargin = 0.1 } diff --git a/physics/shaders/CollisionBroad.wgsl b/physics/shaders/CollisionBroad.wgsl index d7691cfe..a6e2da56 100644 --- a/physics/shaders/CollisionBroad.wgsl +++ b/physics/shaders/CollisionBroad.wgsl @@ -394,9 +394,9 @@ struct GeomData { //////// import: "shapegeom.go" fn ClosestPointPlane(width: f32,length: f32, pt: vec3) -> vec3 { var cp = pt; + cp.y = f32(0); if (width == 0.0) { - cp.y = f32(0); - return cp; + return cp; } cp.x = clamp(pt.x, -width, width); cp.z = clamp(pt.z, -length, length); diff --git a/physics/shaders/CollisionNarrow.wgsl b/physics/shaders/CollisionNarrow.wgsl index 9e62c01c..9e39ac0e 100644 --- a/physics/shaders/CollisionNarrow.wgsl +++ b/physics/shaders/CollisionNarrow.wgsl @@ -566,16 +566,16 @@ return Dot3(diff, *norm); fn ColCapsuleCapsule(cpi: i32,maxIter: i32, gdA: ptr, gdB: ptr, pA: ptr>,pB: ptr>,norm: ptr>) -> f32 { var hhA = (*gdA).Size.y; var hhB = (*gdB).Size.y; - var e0 = vec3(0.0, 0.0, hhA*f32(cpi%2)); - var e1 = vec3(0.0, 0.0, -hhA*f32((cpi+1)%2)); + var e0 = vec3(0, 0, hhA*f32(cpi%2)); + var e1 = vec3(0, 0, -hhA*f32((cpi+1)%2)); var edge0w = MulSpatialPoint((*gdA).WbR, (*gdA).WbQ, e0); var edge1w = MulSpatialPoint((*gdA).WbR, (*gdA).WbQ, e1); var edge0b = MulSpatialPoint((*gdA).BwR, (*gdA).BwQ, edge0w); var edge1b = MulSpatialPoint((*gdA).BwR, (*gdA).BwQ, edge1w); var u = ClosestEdgeCapsule((*gdB).Size.x, (*gdB).Size.y, edge0b, edge1b, maxIter); var pAw = edge0w*(1 - u)+(edge1w*(u)); - var p0Bw = MulSpatialPoint((*gdB).WbR, (*gdB).WbQ, vec3(0.0, 0.0, hhB)); - var p1Bw = MulSpatialPoint((*gdB).WbR, (*gdB).WbQ, vec3(0.0, 0.0, -hhB)); + var p0Bw = MulSpatialPoint((*gdB).WbR, (*gdB).WbQ, vec3(0, hhB, 0)); + var p1Bw = MulSpatialPoint((*gdB).WbR, (*gdB).WbQ, vec3(0, -hhB, 0)); var pBw = ClosestPointLineSegment(p0Bw, p1Bw, pAw); var diff = pAw-(pBw); *norm = Normal3(diff); @@ -604,8 +604,8 @@ return Dot3(diff, *norm); } fn ColBoxCapsule(cpi: i32,maxIter: i32, gdA: ptr, gdB: ptr, pA: ptr>,pB: ptr>,norm: ptr>) -> f32 { var hhB = (*gdB).Size.y; - var e0 = vec3(0.0, 0.0, -hhB*f32(cpi%2)); - var e1 = vec3(0.0, 0.0, hhB*f32((cpi+1)%2)); + var e0 = vec3(0, -hhB*f32(cpi%2), 0); + var e1 = vec3(0, hhB*f32((cpi+1)%2), 0); var edge0w = MulSpatialPoint((*gdB).WbR, (*gdB).WbQ, e0); var edge1w = MulSpatialPoint((*gdB).WbR, (*gdB).WbQ, e1); var edge0a = MulSpatialPoint((*gdA).BwR, (*gdA).BwQ, edge0w); @@ -634,11 +634,11 @@ fn ColBoxPlane(cpi: i32,maxIter: i32, gdA: ptr, gdB: ptr(0.0, 0.0, 1.0)); - if (width > 0.0 && length > 0.0) { + *norm = MulQuatVector((*gdB).WbQ, vec3(0, 1, 0)); + if (width > 0 && length > 0) { if (abs(queryB.x) > width || abs(queryB.z) > length) { return f32( // invalid - 1.0e6); + 1e6); } } } else { @@ -657,7 +657,7 @@ fn ColBoxPlane(cpi: i32,maxIter: i32, gdA: ptr, gdB: ptr width || abs(queryB.z) > length) { return f32( // invalid - 1.0e6); + 1e6); } diff = pAw-(pBw); var comA = (*gdA).WbR; @@ -665,7 +665,7 @@ fn ColBoxPlane(cpi: i32,maxIter: i32, gdA: ptr, gdB: ptr width || abs(queryB.z) > length) { *norm = Normal3(comA-(pBw)); } else { - *norm = MulQuatVector((*gdB).WbQ, vec3(0.0, 0.0, 1.0)); + *norm = MulQuatVector((*gdB).WbQ, vec3(0, 0, 1)); } } *pA = pAw; @@ -686,8 +686,8 @@ return Dot3(diff, *norm); fn ColSphereCapsule(cpi: i32,maxIter: i32, gdA: ptr, gdB: ptr, pA: ptr>,pB: ptr>,norm: ptr>) -> f32 { var pAw = (*gdA).WbR; var hhB = (*gdB).Size.y; - var AB = MulSpatialPoint((*gdB).WbR, (*gdB).WbQ, vec3(0.0, 0.0, hhB)); - var BB = MulSpatialPoint((*gdB).WbR, (*gdB).WbQ, vec3(0.0, 0.0, -hhB)); + var AB = MulSpatialPoint((*gdB).WbR, (*gdB).WbQ, vec3(0, hhB, 0)); + var BB = MulSpatialPoint((*gdB).WbR, (*gdB).WbQ, vec3(0, -hhB, 0)); var pBw = ClosestPointLineSegment(AB, BB, pAw); var diff = pAw-(pBw); *norm = Normal3(diff); @@ -700,16 +700,16 @@ fn ColSpherePlane(cpi: i32,maxIter: i32, gdA: ptr, gdB: ptr(0.0, 0.0, 1.0)); + *norm = MulQuatVector((*gdB).WbQ, vec3(0, 1, 0)); *pA = pAw; *pB = pBw; return Dot3(diff, *norm); } fn ColCylinderPlane(cpi: i32,maxIter: i32, gdA: ptr, gdB: ptr, pA: ptr>,pB: ptr>,norm: ptr>) -> f32 { - var plNorm = MulQuatVector((*gdB).WbQ, vec3(0.0, 1.0, 0.0)); - var plPos = MulSpatialPoint((*gdB).WbR, (*gdB).WbQ, vec3(0.0, 0.0, 0.0)); - var cylCtr = MulSpatialPoint((*gdA).WbR, (*gdA).WbQ, vec3(0.0, 0.0, 0.0)); - var cylAx = Normal3(MulQuatVector((*gdA).WbQ, vec3(0.0, 0.0, 1.0))); + var plNorm = MulQuatVector((*gdB).WbQ, vec3(0, 1, 0)); + var plPos = MulSpatialPoint((*gdB).WbR, (*gdB).WbQ, vec3(0, 0, 0)); + var cylCtr = MulSpatialPoint((*gdA).WbR, (*gdA).WbQ, vec3(0, 0, 0)); + var cylAx = Normal3(MulQuatVector((*gdA).WbQ, vec3(0, 1, 0))); var cylRad = (*gdA).Size.x; var cylHh = (*gdA).Size.y; var dist: f32; @@ -790,9 +790,9 @@ return min(max(dx, dy), 0.0) + Length2(vec2(max(dx, 0.0), max(dy, 0.0))); } fn ClosestPointPlane(width: f32,length: f32, pt: vec3) -> vec3 { var cp = pt; + cp.y = f32(0); if (width == 0.0) { - cp.y = f32(0); - return cp; + return cp; } cp.x = clamp(pt.x, -width, width); cp.z = clamp(pt.z, -length, length); diff --git a/physics/shaders/DeltasFromContacts.wgsl b/physics/shaders/DeltasFromContacts.wgsl index 0d008198..d7415820 100644 --- a/physics/shaders/DeltasFromContacts.wgsl +++ b/physics/shaders/DeltasFromContacts.wgsl @@ -143,8 +143,8 @@ fn DeltasFromContacts(i: u32) { //gosl:kernel } var cmax = ContactsN[0]; var bi = DynamicBody(di); - var td = DynamicDelta(di, params.Next); - var ta = DynamicAngDelta(di, params.Next); + var td = vec3(0, 0, 0); + var ta = vec3(0, 0, 0); var tw = f32(0); for (var ci=0; ci vec3 { + return vec3(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaZ))]); +} fn SetDynamicDelta(idx: i32,cni: i32, delta: vec3) { Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaX))] = delta.x; Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaY))] = delta.y; Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaZ))] = delta.z; } +fn DynamicAngDelta(idx: i32,cni: i32) -> vec3 { + return vec3(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaZ))]); +} fn SetDynamicAngDelta(idx: i32,cni: i32, angDelta: vec3) { Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaX))] = angDelta.x; Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaY))] = angDelta.y; @@ -354,8 +360,10 @@ fn DeltasFromJoints(i: u32) { //gosl:kernel var a = JointCAngDelta(ji); ta = ta+(a); } - SetDynamicDelta(di, params.Next, td); - SetDynamicAngDelta(di, params.Next, ta); + var v0 = DynamicDelta(di, params.Next); + var w0 = DynamicAngDelta(di, params.Next); + SetDynamicDelta(di, params.Next, td+(v0)); + SetDynamicAngDelta(di, params.Next, ta+(w0)); } //////// import: "step_joint.go" \ No newline at end of file diff --git a/physics/shaders/StepBodyContacts.wgsl b/physics/shaders/StepBodyContacts.wgsl index aaa9e6bb..25cf1646 100644 --- a/physics/shaders/StepBodyContacts.wgsl +++ b/physics/shaders/StepBodyContacts.wgsl @@ -172,21 +172,21 @@ let params = Params[0];; var ci = i32(i); ; var biB = GetContactB(ci); ; var diA = GetBodyDynamic(biA); ; var diB = GetBodyDynamic(biB); -; var r0A = BodyDynamicPos(biA, params.Next); -; var q0A = BodyDynamicQuat(biA, params.Next); -; var r0B = BodyDynamicPos(biB, params.Next); -; var q0B = BodyDynamicQuat(biB, params.Next); +; var r1A = BodyDynamicPos(biA, params.Next); +; var q1A = BodyDynamicQuat(biA, params.Next); +; var r1B = BodyDynamicPos(biB, params.Next); +; var q1B = BodyDynamicQuat(biB, params.Next); ; var ctA = ContactAPoint(ci); ; var offA = ContactAOff(ci); ; var ctB = ContactBPoint(ci); ; var offB = ContactBOff(ci); -; var ctAw = MulSpatialPoint(r0A, q0A, ctA); -; var ctBw = MulSpatialPoint(r0B, q0B, ctB); +; var ctAw = MulSpatialPoint(r1A, q1A, ctA); +; var ctBw = MulSpatialPoint(r1B, q1B, ctB); ; var thickA = Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(ci), u32(ContactAThick))]; ; var thickB = Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(ci), u32(ContactBThick))]; ; var thick = thickA + thickB; -; var norm = ContactNorm(ci); -; var nnorm = Negate3(norm); +; var nnorm = ContactNorm(ci); +; var norm = Negate3(nnorm); ; var d = Dot3(norm, ctBw-(ctAw)) - thick; ; if (d >= 0.0) { // now separated Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(ci), u32(ContactWeight))] = 0.0; @@ -201,29 +201,30 @@ let params = Params[0];; var ci = i32(i); ; var comB = BodyCom(biB); ; var mInvB = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(biB), u32(BodyInvMass))]; ; var iInvB = BodyInvInertia(biB); -; var w0A: vec3; -var w0B: vec3;; if (diA >= 0) { - w0A = DynamicAngDelta(diA, params.Next); +; var w1A: vec3; +var w1B: vec3;; if (diA >= 0) { + w1A = DynamicAngDelta(diA, params.Next); }; if (diB >= 0) { - w0B = DynamicAngDelta(diB, params.Next); + w1B = DynamicAngDelta(diB, params.Next); }; var mu = 0.5 * (Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(biA), u32(BodyFriction))] + Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(biB), u32(BodyFriction))]); ; var frTors = 0.5 * (Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(biA), u32(BodyFrictionTortion))] + Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(biB), u32(BodyFrictionTortion))]); ; var frRoll = 0.5 * (Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(biA), u32(BodyFrictionRolling))] + Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(biB), u32(BodyFrictionRolling))]); -; -var dA = ctAw-(MulSpatialPoint(r0A, q0A, comA)); -; var dB = ctBw-(MulSpatialPoint(r0B, q0B, comB)); +; var bounce = 0.5 * (Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(biA), u32(BodyBounce))] + Bodies[Index2D(TensorStrides[0], TensorStrides[1], +u32(biB), u32(BodyBounce))]); +; var dA = ctAw-(MulSpatialPoint(r1A, q1A, comA)); +; var dB = ctBw-(MulSpatialPoint(r1B, q1B, comB)); ; var angA = Negate3(Cross3(dA, norm)); ; var angB = Cross3(dB, norm); -; var lambdaN = ContactConstraint(d, q0A, q0B, mInvA, mInvB, iInvA, iInvB, nnorm, norm, angA, angB, params.ContactRelax, params.Dt); +; var lambdaN = ContactConstraint(d, q1A, q1B, mInvA, mInvB, iInvA, iInvB, nnorm, norm, angA, angB, params.ContactRelax, params.Dt); ; var linDeltaA = Negate3(norm)*(lambdaN); ; var linDeltaB = norm*(lambdaN); ; var angDeltaA = angA*(lambdaN); ; var angDeltaB = angB*(lambdaN); ; if (mu > 0.0) { - ctAw = ctAw+(MulQuatVector(q0A, offA)); - ctBw = ctBw+(MulQuatVector(q0B, offB)); + ctAw = ctAw+(MulQuatVector(q1A, offA)); + ctBw = ctBw+(MulQuatVector(q1B, offB)); var delta = ctBw-(ctAw); var frDelta = delta-(norm*(Dot3(norm, delta))); var perp = Normal3(frDelta); @@ -231,19 +232,19 @@ if (mu > 0.0) { angB = Cross3(dB, perp); var err = Length3(frDelta); if (err > 0.0) { - var lambdaFr = ContactConstraint(err, q0A, q0B, mInvA, mInvB, iInvA, iInvB, Negate3(perp), perp, angA, angB, params.ContactRelax, params.Dt); + var lambdaFr = ContactConstraint(err, q1A, q1B, mInvA, mInvB, iInvA, iInvB, Negate3(perp), perp, angA, angB, params.ContactRelax, params.Dt); lambdaFr = max(lambdaFr, -lambdaN*mu); linDeltaA = linDeltaA-(perp*(lambdaFr)); linDeltaB = linDeltaB+(perp*(lambdaFr)); angDeltaA = angDeltaA+(angA*(lambdaFr)); angDeltaB = angDeltaB+(angB*(lambdaFr)); } -}; var deltaW = w0B-(w0A); +}; var deltaW = w1B-(w1A); ; if (frTors > 0.0) { var err = Dot3(deltaW, norm) * params.Dt; if (abs(err) > 0.0) { var lin = vec3(0, 0, 0); - var lambdaTors = ContactConstraint(err, q0A, q0B, mInvA, mInvB, iInvA, iInvB, lin, lin, nnorm, norm, params.ContactRelax, params.Dt); + var lambdaTors = ContactConstraint(err, q1A, q1B, mInvA, mInvB, iInvA, iInvB, lin, lin, nnorm, norm, params.ContactRelax, params.Dt); lambdaTors = clamp(lambdaTors, -lambdaN*frTors, lambdaN*frTors); angDeltaA = angDeltaA-(norm*(lambdaTors)); angDeltaB = angDeltaB+(norm*(lambdaTors)); @@ -254,11 +255,62 @@ if (mu > 0.0) { if (err > 0.0) { var lin = vec3(0, 0, 0); var rollN = Normal3(deltaW); - var lambdaRoll = ContactConstraint(err, q0A, q0B, mInvA, mInvB, iInvA, iInvB, lin, lin, Negate3(rollN), rollN, params.ContactRelax, params.Dt); + var lambdaRoll = ContactConstraint(err, q1A, q1B, mInvA, mInvB, iInvA, iInvB, lin, lin, Negate3(rollN), rollN, params.ContactRelax, params.Dt); lambdaRoll = max(lambdaRoll, -lambdaN*frRoll); angDeltaA = angDeltaA-(rollN*(lambdaRoll)); angDeltaB = angDeltaB+(rollN*(lambdaRoll)); } +}; +if (params.Restitution == 1 && bounce > 0 && (mInvA > 0 || mInvB > 0)) { + var vA: vec3; + var vB: vec3; + var vAnew: vec3; + var vBnew: vec3; + var dAnew: vec3; + var dBnew: vec3; + var mInvAr: f32; + var mInvBr: f32; + var q0A: vec4; + var q0B: vec4; + var grav = vec3(params.Gravity.x,params.Gravity.y,params.Gravity.z)*(params.Dt); + if (diA >= 0) { + q0A = DynamicQuat(diA, params.Cur); + var w0A = DynamicAngDelta(diA, params.Cur); + var v0A = DynamicDelta(diA, params.Cur); + var v1A = DynamicDelta(diA, params.Next); + vA = VelocityAtPoint(v0A, w0A, dA)+(grav); + vAnew = VelocityAtPoint(v1A, w1A, dA); + dAnew = MulQuatVectorInverse(q0A, Cross3(dA, nnorm)); // norm is not - here.. + mInvAr = mInvA + Dot3(dAnew, iInvA*(dAnew)); + } + if (diB >= 0) { + q0B = DynamicQuat(diB, params.Cur); + var w0B = DynamicAngDelta(diB, params.Cur); + var v0B = DynamicDelta(diB, params.Cur); + var v1B = DynamicDelta(diB, params.Next); + vB = VelocityAtPoint(v0B, w0B, dB)+(grav); + vBnew = VelocityAtPoint(v1B, w1B, dB); + dBnew = MulQuatVectorInverse(q0B, Cross3(dB, nnorm)); // norm is not - here.. + mInvBr = mInvB + Dot3(dBnew, iInvB*(dBnew)); + } + var mInv = mInvAr + mInvBr; + var relVel0 = Dot3(nnorm, vA-(vB)); + var relVel1 = Dot3(nnorm, vAnew-(vBnew)); + if (relVel0 < 0) { + var dv = -(relVel1 - relVel0*bounce) / mInv; + if (diA >= 0) { + var dvA = nnorm*(mInvA * dv); + var dwA = MulQuatVector(q0A, iInvA*(dAnew)*(dv)); + linDeltaA = linDeltaA+(dvA); + angDeltaA = angDeltaA+(dwA); + } + if (diB >= 0) { + var dvB = nnorm*(mInvB * dv); + var dwB = MulQuatVector(q0B, iInvB*(dBnew)*(dv)); + linDeltaB = linDeltaB+(dvB); + angDeltaB = angDeltaB+(dwB); + } + } }; Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(ci), u32(ContactWeight))] = 1.0;; SetContactADelta(ci, linDeltaA);; SetContactBDelta(ci, linDeltaB);; SetContactAAngDelta(ci, angDeltaA);; SetContactBAngDelta(ci, angDeltaB); } fn ContactConstraint(err: f32, q0A: vec4,q0B: vec4, mInvA: f32,mInvB: f32, iInvA: mat3x3f,iInvB: mat3x3f, linA: vec3,linB: vec3,angA: vec3,angB: vec3, relaxation: f32,dt: f32) -> f32 { var denom = f32(0.0); @@ -321,6 +373,9 @@ fn DynamicPos(idx: i32,cni: i32) -> vec3 { fn DynamicQuat(idx: i32,cni: i32) -> vec4 { return vec4(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatZ))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatW))]); } +fn DynamicDelta(idx: i32,cni: i32) -> vec3 { + return vec3(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaZ))]); +} fn DynamicAngDelta(idx: i32,cni: i32) -> vec3 { return vec3(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaZ))]); } @@ -503,5 +558,8 @@ fn Cross3(v: vec3,o: vec3) -> vec3 { //////// import: "step.go" //////// import: "step_body.go" +fn VelocityAtPoint(lin: vec3,ang: vec3,r: vec3) -> vec3 { + return lin+(Cross3(ang, r)); +} //////// import: "step_joint.go" \ No newline at end of file diff --git a/physics/shaders/StepBodyDeltas.wgsl b/physics/shaders/StepBodyDeltas.wgsl index f7dfceee..74f7bd68 100644 --- a/physics/shaders/StepBodyDeltas.wgsl +++ b/physics/shaders/StepBodyDeltas.wgsl @@ -399,9 +399,11 @@ fn StepBodyDeltas(i: u32) { //gosl:kernel var v0 = DynamicDelta(di, params.Next); var w0 = DynamicAngDelta(di, params.Next); var weight = f32(1.0); - var cWt = Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(di), u32(params.Next), u32(DynContactWeight))]; - if (cWt > 0) { - weight = 1.0 / cWt; + if (params.ContactWeighting == 1) { + var cWt = Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(di), u32(params.Next), u32(DynContactWeight))]; + if (cWt > 0) { + weight = 1.0 / cWt; + } } var dp = v0*(invMass * weight); var dq = w0*(weight); diff --git a/physics/shaders/StepIntegrateBodies.wgsl b/physics/shaders/StepIntegrateBodies.wgsl index 9c8829eb..859c618f 100644 --- a/physics/shaders/StepIntegrateBodies.wgsl +++ b/physics/shaders/StepIntegrateBodies.wgsl @@ -391,6 +391,8 @@ fn StepIntegrateBodies(i: u32) { //gosl:kernel var invMass = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyInvMass))]; var inertia = BodyInertia(bi); var invInertia = BodyInvInertia(bi); + var grav = vec3(params.Gravity.x,params.Gravity.y, + params.Gravity.z); var com = BodyCom(bi); var r0 = DynamicPos(di, params.Cur); var q0 = DynamicQuat(di, params.Cur); @@ -399,7 +401,7 @@ fn StepIntegrateBodies(i: u32) { //gosl:kernel var f0 = DynamicForce(di, params.Next); var t0 = DynamicTorque(di, params.Next); var pcom = MulQuatVector(q0, com)+(r0); - var v1 = v0+(f0*(invMass)+(vec3(params.Gravity.x,params.Gravity.y,params.Gravity.z)*(OneIfNonzero(invMass)))*(params.Dt)); + var v1 = v0+(f0*(invMass)+(grav*(OneIfNonzero(invMass)))*(params.Dt)); var p1 = pcom+(v1*(params.Dt)); var wb = MulQuatVectorInverse(q0, w0); var tb = MulQuatVectorInverse(q0, t0)-(Cross3(wb, inertia*(wb))); // coriolis forces diff --git a/physics/shaders/imports/gosl.go b/physics/shaders/imports/gosl.go new file mode 100644 index 00000000..ce4a7ec8 --- /dev/null +++ b/physics/shaders/imports/gosl.go @@ -0,0 +1,927 @@ +// Code generated by "gosl"; DO NOT EDIT + +package imports + +import ( + "embed" + "fmt" + "math" + "unsafe" + "cogentcore.org/core/gpu" + "cogentcore.org/lab/tensor" +) + +var shaders embed.FS + +var ( + // GPUInitialized is true once the GPU system has been initialized. + // Prevents multiple initializations. + GPUInitialized bool + + // ComputeGPU is the compute gpu device. + // Set this prior to calling GPUInit() to use an existing device. + ComputeGPU *gpu.GPU + + // BorrowedGPU is true if our ComputeGPU is set externally, + // versus created specifically for this system. If external, + // we don't release it. + BorrowedGPU bool + + // UseGPU indicates whether to use GPU vs. CPU. + UseGPU bool +) +// GPUSystem is a GPU compute System with kernels operating on the +// same set of data variables. +var GPUSystem *gpu.ComputeSystem + +// GPUVars is an enum for GPU variables, for specifying what to sync. +type GPUVars int32 //enums:enum + +const ( + ParamsVar GPUVars = 0 + BodiesVar GPUVars = 1 + JointsVar GPUVars = 2 + JointDoFsVar GPUVars = 3 + BodyJointsVar GPUVars = 4 + BodyCollidePairsVar GPUVars = 5 + DynamicsVar GPUVars = 6 + BroadContactsNVar GPUVars = 7 + BroadContactsVar GPUVars = 8 + ContactsNVar GPUVars = 9 + ContactsVar GPUVars = 10 + JointControlsVar GPUVars = 11 +) + +// Tensor stride variables +var TensorStrides tensor.Uint32 + +// GPUInit initializes the GPU compute system, +// configuring system(s), variables and kernels. +// It is safe to call multiple times: detects if already run. +func GPUInit() { + if GPUInitialized { + return + } + GPUInitialized = true + if ComputeGPU == nil { // set prior to this call to use an external + ComputeGPU = gpu.NewComputeGPU() + } else { + BorrowedGPU = true + } + gp := ComputeGPU + + _ = fmt.Sprintf("%g",math.NaN()) // keep imports happy + { + sy := gpu.NewComputeSystem(gp, "Default") + GPUSystem = sy + vars := sy.Vars() + { + sgp := vars.AddGroup(gpu.Storage, "Params") + var vr *gpu.Var + _ = vr + vr = sgp.Add("TensorStrides", gpu.Uint32, 1, gpu.ComputeShader) + vr.ReadOnly = true + vr = sgp.AddStruct("Params", int(unsafe.Sizeof(PhysParams{})), 1, gpu.ComputeShader) + vr.ReadOnly = true + sgp.SetNValues(1) + } + { + sgp := vars.AddGroup(gpu.Storage, "Bodies") + var vr *gpu.Var + _ = vr + vr = sgp.Add("Bodies", gpu.Float32, 1, gpu.ComputeShader) + vr = sgp.Add("Joints", gpu.Float32, 1, gpu.ComputeShader) + vr = sgp.Add("JointDoFs", gpu.Float32, 1, gpu.ComputeShader) + vr = sgp.Add("BodyJoints", gpu.Int32, 1, gpu.ComputeShader) + vr = sgp.Add("BodyCollidePairs", gpu.Int32, 1, gpu.ComputeShader) + sgp.SetNValues(1) + } + { + sgp := vars.AddGroup(gpu.Storage, "Dynamics") + var vr *gpu.Var + _ = vr + vr = sgp.Add("Dynamics", gpu.Float32, 1, gpu.ComputeShader) + vr = sgp.Add("BroadContactsN", gpu.Int32, 1, gpu.ComputeShader) + vr = sgp.Add("BroadContacts", gpu.Float32, 1, gpu.ComputeShader) + vr = sgp.Add("ContactsN", gpu.Int32, 1, gpu.ComputeShader) + vr = sgp.Add("Contacts", gpu.Float32, 1, gpu.ComputeShader) + sgp.SetNValues(1) + } + { + sgp := vars.AddGroup(gpu.Storage, "Controls") + var vr *gpu.Var + _ = vr + vr = sgp.Add("JointControls", gpu.Float32, 1, gpu.ComputeShader) + sgp.SetNValues(1) + } + var pl *gpu.ComputePipeline + pl = gpu.NewComputePipelineShaderFS(shaders, "", sy) + pl.AddVarUsed(0, "TensorStrides") + pl = gpu.NewComputePipelineShaderFS(shaders, "", sy) + pl.AddVarUsed(0, "TensorStrides") + pl = gpu.NewComputePipelineShaderFS(shaders, "", sy) + pl.AddVarUsed(0, "TensorStrides") + pl = gpu.NewComputePipelineShaderFS(shaders, "", sy) + pl.AddVarUsed(0, "TensorStrides") + pl = gpu.NewComputePipelineShaderFS(shaders, "", sy) + pl.AddVarUsed(0, "TensorStrides") + pl = gpu.NewComputePipelineShaderFS(shaders, "", sy) + pl.AddVarUsed(0, "TensorStrides") + pl = gpu.NewComputePipelineShaderFS(shaders, "", sy) + pl.AddVarUsed(0, "TensorStrides") + pl = gpu.NewComputePipelineShaderFS(shaders, "", sy) + pl.AddVarUsed(0, "TensorStrides") + pl = gpu.NewComputePipelineShaderFS(shaders, "", sy) + pl.AddVarUsed(0, "TensorStrides") + pl = gpu.NewComputePipelineShaderFS(shaders, "", sy) + pl.AddVarUsed(0, "TensorStrides") + pl = gpu.NewComputePipelineShaderFS(shaders, "", sy) + pl.AddVarUsed(0, "TensorStrides") + pl = gpu.NewComputePipelineShaderFS(shaders, "", sy) + pl.AddVarUsed(0, "TensorStrides") + pl = gpu.NewComputePipelineShaderFS(shaders, "", sy) + pl.AddVarUsed(0, "TensorStrides") + sy.Config() + } +} + +// GPURelease releases the GPU compute system resources. +// Call this at program exit. +func GPURelease() { + if GPUSystem != nil { + GPUSystem.Release() + GPUSystem = nil + } + + if !BorrowedGPU && ComputeGPU != nil { + ComputeGPU.Release() + + } + ComputeGPU = nil +} + +// RunCollisionBroad runs the CollisionBroad kernel with given number of elements, +// on either the CPU or GPU depending on the UseGPU variable. +// Can call multiple Run* kernels in a row, which are then all launched +// in the same command submission on the GPU, which is by far the most efficient. +// MUST call RunDone (with optional vars to sync) after all Run calls. +// Alternatively, a single-shot RunOneCollisionBroad call does Run and Done for a +// single run-and-sync case. +func RunCollisionBroad(n int) { + if UseGPU { + RunCollisionBroadGPU(n) + } else { + RunCollisionBroadCPU(n) + } +} + +// RunCollisionBroadGPU runs the CollisionBroad kernel on the GPU. See [RunCollisionBroad] for more info. +func RunCollisionBroadGPU(n int) { + sy := GPUSystem + pl := sy.ComputePipelines["CollisionBroad"] + ce, _ := sy.BeginComputePass() + pl.Dispatch1D(ce, n, 64) +} + +// RunCollisionBroadCPU runs the CollisionBroad kernel on the CPU. +func RunCollisionBroadCPU(n int) { + gpu.VectorizeFunc(0, n, CollisionBroad) +} + +// RunOneCollisionBroad runs the CollisionBroad kernel with given number of elements, +// on either the CPU or GPU depending on the UseGPU variable. +// This version then calls RunDone with the given variables to sync +// after the Run, for a single-shot Run-and-Done call. If multiple kernels +// can be run in sequence, it is much more efficient to do multiple Run* +// calls followed by a RunDone call. +func RunOneCollisionBroad(n int, syncVars ...GPUVars) { + if UseGPU { + RunCollisionBroadGPU(n) + RunDone(syncVars...) + } else { + RunCollisionBroadCPU(n) + } +} +// RunCollisionInit runs the CollisionInit kernel with given number of elements, +// on either the CPU or GPU depending on the UseGPU variable. +// Can call multiple Run* kernels in a row, which are then all launched +// in the same command submission on the GPU, which is by far the most efficient. +// MUST call RunDone (with optional vars to sync) after all Run calls. +// Alternatively, a single-shot RunOneCollisionInit call does Run and Done for a +// single run-and-sync case. +func RunCollisionInit(n int) { + if UseGPU { + RunCollisionInitGPU(n) + } else { + RunCollisionInitCPU(n) + } +} + +// RunCollisionInitGPU runs the CollisionInit kernel on the GPU. See [RunCollisionInit] for more info. +func RunCollisionInitGPU(n int) { + sy := GPUSystem + pl := sy.ComputePipelines["CollisionInit"] + ce, _ := sy.BeginComputePass() + pl.Dispatch1D(ce, n, 64) +} + +// RunCollisionInitCPU runs the CollisionInit kernel on the CPU. +func RunCollisionInitCPU(n int) { + gpu.VectorizeFunc(0, n, CollisionInit) +} + +// RunOneCollisionInit runs the CollisionInit kernel with given number of elements, +// on either the CPU or GPU depending on the UseGPU variable. +// This version then calls RunDone with the given variables to sync +// after the Run, for a single-shot Run-and-Done call. If multiple kernels +// can be run in sequence, it is much more efficient to do multiple Run* +// calls followed by a RunDone call. +func RunOneCollisionInit(n int, syncVars ...GPUVars) { + if UseGPU { + RunCollisionInitGPU(n) + RunDone(syncVars...) + } else { + RunCollisionInitCPU(n) + } +} +// RunCollisionNarrow runs the CollisionNarrow kernel with given number of elements, +// on either the CPU or GPU depending on the UseGPU variable. +// Can call multiple Run* kernels in a row, which are then all launched +// in the same command submission on the GPU, which is by far the most efficient. +// MUST call RunDone (with optional vars to sync) after all Run calls. +// Alternatively, a single-shot RunOneCollisionNarrow call does Run and Done for a +// single run-and-sync case. +func RunCollisionNarrow(n int) { + if UseGPU { + RunCollisionNarrowGPU(n) + } else { + RunCollisionNarrowCPU(n) + } +} + +// RunCollisionNarrowGPU runs the CollisionNarrow kernel on the GPU. See [RunCollisionNarrow] for more info. +func RunCollisionNarrowGPU(n int) { + sy := GPUSystem + pl := sy.ComputePipelines["CollisionNarrow"] + ce, _ := sy.BeginComputePass() + pl.Dispatch1D(ce, n, 64) +} + +// RunCollisionNarrowCPU runs the CollisionNarrow kernel on the CPU. +func RunCollisionNarrowCPU(n int) { + gpu.VectorizeFunc(0, n, CollisionNarrow) +} + +// RunOneCollisionNarrow runs the CollisionNarrow kernel with given number of elements, +// on either the CPU or GPU depending on the UseGPU variable. +// This version then calls RunDone with the given variables to sync +// after the Run, for a single-shot Run-and-Done call. If multiple kernels +// can be run in sequence, it is much more efficient to do multiple Run* +// calls followed by a RunDone call. +func RunOneCollisionNarrow(n int, syncVars ...GPUVars) { + if UseGPU { + RunCollisionNarrowGPU(n) + RunDone(syncVars...) + } else { + RunCollisionNarrowCPU(n) + } +} +// RunDeltasFromContacts runs the DeltasFromContacts kernel with given number of elements, +// on either the CPU or GPU depending on the UseGPU variable. +// Can call multiple Run* kernels in a row, which are then all launched +// in the same command submission on the GPU, which is by far the most efficient. +// MUST call RunDone (with optional vars to sync) after all Run calls. +// Alternatively, a single-shot RunOneDeltasFromContacts call does Run and Done for a +// single run-and-sync case. +func RunDeltasFromContacts(n int) { + if UseGPU { + RunDeltasFromContactsGPU(n) + } else { + RunDeltasFromContactsCPU(n) + } +} + +// RunDeltasFromContactsGPU runs the DeltasFromContacts kernel on the GPU. See [RunDeltasFromContacts] for more info. +func RunDeltasFromContactsGPU(n int) { + sy := GPUSystem + pl := sy.ComputePipelines["DeltasFromContacts"] + ce, _ := sy.BeginComputePass() + pl.Dispatch1D(ce, n, 64) +} + +// RunDeltasFromContactsCPU runs the DeltasFromContacts kernel on the CPU. +func RunDeltasFromContactsCPU(n int) { + gpu.VectorizeFunc(0, n, DeltasFromContacts) +} + +// RunOneDeltasFromContacts runs the DeltasFromContacts kernel with given number of elements, +// on either the CPU or GPU depending on the UseGPU variable. +// This version then calls RunDone with the given variables to sync +// after the Run, for a single-shot Run-and-Done call. If multiple kernels +// can be run in sequence, it is much more efficient to do multiple Run* +// calls followed by a RunDone call. +func RunOneDeltasFromContacts(n int, syncVars ...GPUVars) { + if UseGPU { + RunDeltasFromContactsGPU(n) + RunDone(syncVars...) + } else { + RunDeltasFromContactsCPU(n) + } +} +// RunDeltasFromJoints runs the DeltasFromJoints kernel with given number of elements, +// on either the CPU or GPU depending on the UseGPU variable. +// Can call multiple Run* kernels in a row, which are then all launched +// in the same command submission on the GPU, which is by far the most efficient. +// MUST call RunDone (with optional vars to sync) after all Run calls. +// Alternatively, a single-shot RunOneDeltasFromJoints call does Run and Done for a +// single run-and-sync case. +func RunDeltasFromJoints(n int) { + if UseGPU { + RunDeltasFromJointsGPU(n) + } else { + RunDeltasFromJointsCPU(n) + } +} + +// RunDeltasFromJointsGPU runs the DeltasFromJoints kernel on the GPU. See [RunDeltasFromJoints] for more info. +func RunDeltasFromJointsGPU(n int) { + sy := GPUSystem + pl := sy.ComputePipelines["DeltasFromJoints"] + ce, _ := sy.BeginComputePass() + pl.Dispatch1D(ce, n, 64) +} + +// RunDeltasFromJointsCPU runs the DeltasFromJoints kernel on the CPU. +func RunDeltasFromJointsCPU(n int) { + gpu.VectorizeFunc(0, n, DeltasFromJoints) +} + +// RunOneDeltasFromJoints runs the DeltasFromJoints kernel with given number of elements, +// on either the CPU or GPU depending on the UseGPU variable. +// This version then calls RunDone with the given variables to sync +// after the Run, for a single-shot Run-and-Done call. If multiple kernels +// can be run in sequence, it is much more efficient to do multiple Run* +// calls followed by a RunDone call. +func RunOneDeltasFromJoints(n int, syncVars ...GPUVars) { + if UseGPU { + RunDeltasFromJointsGPU(n) + RunDone(syncVars...) + } else { + RunDeltasFromJointsCPU(n) + } +} +// RunDynamicsCurToNext runs the DynamicsCurToNext kernel with given number of elements, +// on either the CPU or GPU depending on the UseGPU variable. +// Can call multiple Run* kernels in a row, which are then all launched +// in the same command submission on the GPU, which is by far the most efficient. +// MUST call RunDone (with optional vars to sync) after all Run calls. +// Alternatively, a single-shot RunOneDynamicsCurToNext call does Run and Done for a +// single run-and-sync case. +func RunDynamicsCurToNext(n int) { + if UseGPU { + RunDynamicsCurToNextGPU(n) + } else { + RunDynamicsCurToNextCPU(n) + } +} + +// RunDynamicsCurToNextGPU runs the DynamicsCurToNext kernel on the GPU. See [RunDynamicsCurToNext] for more info. +func RunDynamicsCurToNextGPU(n int) { + sy := GPUSystem + pl := sy.ComputePipelines["DynamicsCurToNext"] + ce, _ := sy.BeginComputePass() + pl.Dispatch1D(ce, n, 64) +} + +// RunDynamicsCurToNextCPU runs the DynamicsCurToNext kernel on the CPU. +func RunDynamicsCurToNextCPU(n int) { + gpu.VectorizeFunc(0, n, DynamicsCurToNext) +} + +// RunOneDynamicsCurToNext runs the DynamicsCurToNext kernel with given number of elements, +// on either the CPU or GPU depending on the UseGPU variable. +// This version then calls RunDone with the given variables to sync +// after the Run, for a single-shot Run-and-Done call. If multiple kernels +// can be run in sequence, it is much more efficient to do multiple Run* +// calls followed by a RunDone call. +func RunOneDynamicsCurToNext(n int, syncVars ...GPUVars) { + if UseGPU { + RunDynamicsCurToNextGPU(n) + RunDone(syncVars...) + } else { + RunDynamicsCurToNextCPU(n) + } +} +// RunForcesFromJoints runs the ForcesFromJoints kernel with given number of elements, +// on either the CPU or GPU depending on the UseGPU variable. +// Can call multiple Run* kernels in a row, which are then all launched +// in the same command submission on the GPU, which is by far the most efficient. +// MUST call RunDone (with optional vars to sync) after all Run calls. +// Alternatively, a single-shot RunOneForcesFromJoints call does Run and Done for a +// single run-and-sync case. +func RunForcesFromJoints(n int) { + if UseGPU { + RunForcesFromJointsGPU(n) + } else { + RunForcesFromJointsCPU(n) + } +} + +// RunForcesFromJointsGPU runs the ForcesFromJoints kernel on the GPU. See [RunForcesFromJoints] for more info. +func RunForcesFromJointsGPU(n int) { + sy := GPUSystem + pl := sy.ComputePipelines["ForcesFromJoints"] + ce, _ := sy.BeginComputePass() + pl.Dispatch1D(ce, n, 64) +} + +// RunForcesFromJointsCPU runs the ForcesFromJoints kernel on the CPU. +func RunForcesFromJointsCPU(n int) { + gpu.VectorizeFunc(0, n, ForcesFromJoints) +} + +// RunOneForcesFromJoints runs the ForcesFromJoints kernel with given number of elements, +// on either the CPU or GPU depending on the UseGPU variable. +// This version then calls RunDone with the given variables to sync +// after the Run, for a single-shot Run-and-Done call. If multiple kernels +// can be run in sequence, it is much more efficient to do multiple Run* +// calls followed by a RunDone call. +func RunOneForcesFromJoints(n int, syncVars ...GPUVars) { + if UseGPU { + RunForcesFromJointsGPU(n) + RunDone(syncVars...) + } else { + RunForcesFromJointsCPU(n) + } +} +// RunInitDynamics runs the InitDynamics kernel with given number of elements, +// on either the CPU or GPU depending on the UseGPU variable. +// Can call multiple Run* kernels in a row, which are then all launched +// in the same command submission on the GPU, which is by far the most efficient. +// MUST call RunDone (with optional vars to sync) after all Run calls. +// Alternatively, a single-shot RunOneInitDynamics call does Run and Done for a +// single run-and-sync case. +func RunInitDynamics(n int) { + if UseGPU { + RunInitDynamicsGPU(n) + } else { + RunInitDynamicsCPU(n) + } +} + +// RunInitDynamicsGPU runs the InitDynamics kernel on the GPU. See [RunInitDynamics] for more info. +func RunInitDynamicsGPU(n int) { + sy := GPUSystem + pl := sy.ComputePipelines["InitDynamics"] + ce, _ := sy.BeginComputePass() + pl.Dispatch1D(ce, n, 64) +} + +// RunInitDynamicsCPU runs the InitDynamics kernel on the CPU. +func RunInitDynamicsCPU(n int) { + gpu.VectorizeFunc(0, n, InitDynamics) +} + +// RunOneInitDynamics runs the InitDynamics kernel with given number of elements, +// on either the CPU or GPU depending on the UseGPU variable. +// This version then calls RunDone with the given variables to sync +// after the Run, for a single-shot Run-and-Done call. If multiple kernels +// can be run in sequence, it is much more efficient to do multiple Run* +// calls followed by a RunDone call. +func RunOneInitDynamics(n int, syncVars ...GPUVars) { + if UseGPU { + RunInitDynamicsGPU(n) + RunDone(syncVars...) + } else { + RunInitDynamicsCPU(n) + } +} +// RunStepBodyContacts runs the StepBodyContacts kernel with given number of elements, +// on either the CPU or GPU depending on the UseGPU variable. +// Can call multiple Run* kernels in a row, which are then all launched +// in the same command submission on the GPU, which is by far the most efficient. +// MUST call RunDone (with optional vars to sync) after all Run calls. +// Alternatively, a single-shot RunOneStepBodyContacts call does Run and Done for a +// single run-and-sync case. +func RunStepBodyContacts(n int) { + if UseGPU { + RunStepBodyContactsGPU(n) + } else { + RunStepBodyContactsCPU(n) + } +} + +// RunStepBodyContactsGPU runs the StepBodyContacts kernel on the GPU. See [RunStepBodyContacts] for more info. +func RunStepBodyContactsGPU(n int) { + sy := GPUSystem + pl := sy.ComputePipelines["StepBodyContacts"] + ce, _ := sy.BeginComputePass() + pl.Dispatch1D(ce, n, 64) +} + +// RunStepBodyContactsCPU runs the StepBodyContacts kernel on the CPU. +func RunStepBodyContactsCPU(n int) { + gpu.VectorizeFunc(0, n, StepBodyContacts) +} + +// RunOneStepBodyContacts runs the StepBodyContacts kernel with given number of elements, +// on either the CPU or GPU depending on the UseGPU variable. +// This version then calls RunDone with the given variables to sync +// after the Run, for a single-shot Run-and-Done call. If multiple kernels +// can be run in sequence, it is much more efficient to do multiple Run* +// calls followed by a RunDone call. +func RunOneStepBodyContacts(n int, syncVars ...GPUVars) { + if UseGPU { + RunStepBodyContactsGPU(n) + RunDone(syncVars...) + } else { + RunStepBodyContactsCPU(n) + } +} +// RunStepBodyDeltas runs the StepBodyDeltas kernel with given number of elements, +// on either the CPU or GPU depending on the UseGPU variable. +// Can call multiple Run* kernels in a row, which are then all launched +// in the same command submission on the GPU, which is by far the most efficient. +// MUST call RunDone (with optional vars to sync) after all Run calls. +// Alternatively, a single-shot RunOneStepBodyDeltas call does Run and Done for a +// single run-and-sync case. +func RunStepBodyDeltas(n int) { + if UseGPU { + RunStepBodyDeltasGPU(n) + } else { + RunStepBodyDeltasCPU(n) + } +} + +// RunStepBodyDeltasGPU runs the StepBodyDeltas kernel on the GPU. See [RunStepBodyDeltas] for more info. +func RunStepBodyDeltasGPU(n int) { + sy := GPUSystem + pl := sy.ComputePipelines["StepBodyDeltas"] + ce, _ := sy.BeginComputePass() + pl.Dispatch1D(ce, n, 64) +} + +// RunStepBodyDeltasCPU runs the StepBodyDeltas kernel on the CPU. +func RunStepBodyDeltasCPU(n int) { + gpu.VectorizeFunc(0, n, StepBodyDeltas) +} + +// RunOneStepBodyDeltas runs the StepBodyDeltas kernel with given number of elements, +// on either the CPU or GPU depending on the UseGPU variable. +// This version then calls RunDone with the given variables to sync +// after the Run, for a single-shot Run-and-Done call. If multiple kernels +// can be run in sequence, it is much more efficient to do multiple Run* +// calls followed by a RunDone call. +func RunOneStepBodyDeltas(n int, syncVars ...GPUVars) { + if UseGPU { + RunStepBodyDeltasGPU(n) + RunDone(syncVars...) + } else { + RunStepBodyDeltasCPU(n) + } +} +// RunStepIntegrateBodies runs the StepIntegrateBodies kernel with given number of elements, +// on either the CPU or GPU depending on the UseGPU variable. +// Can call multiple Run* kernels in a row, which are then all launched +// in the same command submission on the GPU, which is by far the most efficient. +// MUST call RunDone (with optional vars to sync) after all Run calls. +// Alternatively, a single-shot RunOneStepIntegrateBodies call does Run and Done for a +// single run-and-sync case. +func RunStepIntegrateBodies(n int) { + if UseGPU { + RunStepIntegrateBodiesGPU(n) + } else { + RunStepIntegrateBodiesCPU(n) + } +} + +// RunStepIntegrateBodiesGPU runs the StepIntegrateBodies kernel on the GPU. See [RunStepIntegrateBodies] for more info. +func RunStepIntegrateBodiesGPU(n int) { + sy := GPUSystem + pl := sy.ComputePipelines["StepIntegrateBodies"] + ce, _ := sy.BeginComputePass() + pl.Dispatch1D(ce, n, 64) +} + +// RunStepIntegrateBodiesCPU runs the StepIntegrateBodies kernel on the CPU. +func RunStepIntegrateBodiesCPU(n int) { + gpu.VectorizeFunc(0, n, StepIntegrateBodies) +} + +// RunOneStepIntegrateBodies runs the StepIntegrateBodies kernel with given number of elements, +// on either the CPU or GPU depending on the UseGPU variable. +// This version then calls RunDone with the given variables to sync +// after the Run, for a single-shot Run-and-Done call. If multiple kernels +// can be run in sequence, it is much more efficient to do multiple Run* +// calls followed by a RunDone call. +func RunOneStepIntegrateBodies(n int, syncVars ...GPUVars) { + if UseGPU { + RunStepIntegrateBodiesGPU(n) + RunDone(syncVars...) + } else { + RunStepIntegrateBodiesCPU(n) + } +} +// RunStepJointForces runs the StepJointForces kernel with given number of elements, +// on either the CPU or GPU depending on the UseGPU variable. +// Can call multiple Run* kernels in a row, which are then all launched +// in the same command submission on the GPU, which is by far the most efficient. +// MUST call RunDone (with optional vars to sync) after all Run calls. +// Alternatively, a single-shot RunOneStepJointForces call does Run and Done for a +// single run-and-sync case. +func RunStepJointForces(n int) { + if UseGPU { + RunStepJointForcesGPU(n) + } else { + RunStepJointForcesCPU(n) + } +} + +// RunStepJointForcesGPU runs the StepJointForces kernel on the GPU. See [RunStepJointForces] for more info. +func RunStepJointForcesGPU(n int) { + sy := GPUSystem + pl := sy.ComputePipelines["StepJointForces"] + ce, _ := sy.BeginComputePass() + pl.Dispatch1D(ce, n, 64) +} + +// RunStepJointForcesCPU runs the StepJointForces kernel on the CPU. +func RunStepJointForcesCPU(n int) { + gpu.VectorizeFunc(0, n, StepJointForces) +} + +// RunOneStepJointForces runs the StepJointForces kernel with given number of elements, +// on either the CPU or GPU depending on the UseGPU variable. +// This version then calls RunDone with the given variables to sync +// after the Run, for a single-shot Run-and-Done call. If multiple kernels +// can be run in sequence, it is much more efficient to do multiple Run* +// calls followed by a RunDone call. +func RunOneStepJointForces(n int, syncVars ...GPUVars) { + if UseGPU { + RunStepJointForcesGPU(n) + RunDone(syncVars...) + } else { + RunStepJointForcesCPU(n) + } +} +// RunStepSolveJoints runs the StepSolveJoints kernel with given number of elements, +// on either the CPU or GPU depending on the UseGPU variable. +// Can call multiple Run* kernels in a row, which are then all launched +// in the same command submission on the GPU, which is by far the most efficient. +// MUST call RunDone (with optional vars to sync) after all Run calls. +// Alternatively, a single-shot RunOneStepSolveJoints call does Run and Done for a +// single run-and-sync case. +func RunStepSolveJoints(n int) { + if UseGPU { + RunStepSolveJointsGPU(n) + } else { + RunStepSolveJointsCPU(n) + } +} + +// RunStepSolveJointsGPU runs the StepSolveJoints kernel on the GPU. See [RunStepSolveJoints] for more info. +func RunStepSolveJointsGPU(n int) { + sy := GPUSystem + pl := sy.ComputePipelines["StepSolveJoints"] + ce, _ := sy.BeginComputePass() + pl.Dispatch1D(ce, n, 64) +} + +// RunStepSolveJointsCPU runs the StepSolveJoints kernel on the CPU. +func RunStepSolveJointsCPU(n int) { + gpu.VectorizeFunc(0, n, StepSolveJoints) +} + +// RunOneStepSolveJoints runs the StepSolveJoints kernel with given number of elements, +// on either the CPU or GPU depending on the UseGPU variable. +// This version then calls RunDone with the given variables to sync +// after the Run, for a single-shot Run-and-Done call. If multiple kernels +// can be run in sequence, it is much more efficient to do multiple Run* +// calls followed by a RunDone call. +func RunOneStepSolveJoints(n int, syncVars ...GPUVars) { + if UseGPU { + RunStepSolveJointsGPU(n) + RunDone(syncVars...) + } else { + RunStepSolveJointsCPU(n) + } +} +// RunDone must be called after Run* calls to start compute kernels. +// This actually submits the kernel jobs to the GPU, and adds commands +// to synchronize the given variables back from the GPU to the CPU. +// After this function completes, the GPU results will be available in +// the specified variables. +func RunDone(syncVars ...GPUVars) { + if !UseGPU { + return + } + sy := GPUSystem + sy.ComputeEncoder.End() + ReadFromGPU(syncVars...) + sy.EndComputePass() + SyncFromGPU(syncVars...) +} + +// ToGPU copies given variables to the GPU for the system. +func ToGPU(vars ...GPUVars) { + if !UseGPU { + return + } + sy := GPUSystem + syVars := sy.Vars() + for _, vr := range vars { + switch vr { + case ParamsVar: + v, _ := syVars.ValueByIndex(0, "Params", 0) + gpu.SetValueFrom(v, Params) + case BodiesVar: + v, _ := syVars.ValueByIndex(1, "Bodies", 0) + gpu.SetValueFrom(v, Bodies.Values) + case JointsVar: + v, _ := syVars.ValueByIndex(1, "Joints", 0) + gpu.SetValueFrom(v, Joints.Values) + case JointDoFsVar: + v, _ := syVars.ValueByIndex(1, "JointDoFs", 0) + gpu.SetValueFrom(v, JointDoFs.Values) + case BodyJointsVar: + v, _ := syVars.ValueByIndex(1, "BodyJoints", 0) + gpu.SetValueFrom(v, BodyJoints.Values) + case BodyCollidePairsVar: + v, _ := syVars.ValueByIndex(1, "BodyCollidePairs", 0) + gpu.SetValueFrom(v, BodyCollidePairs.Values) + case DynamicsVar: + v, _ := syVars.ValueByIndex(2, "Dynamics", 0) + gpu.SetValueFrom(v, Dynamics.Values) + case BroadContactsNVar: + v, _ := syVars.ValueByIndex(2, "BroadContactsN", 0) + gpu.SetValueFrom(v, BroadContactsN.Values) + case BroadContactsVar: + v, _ := syVars.ValueByIndex(2, "BroadContacts", 0) + gpu.SetValueFrom(v, BroadContacts.Values) + case ContactsNVar: + v, _ := syVars.ValueByIndex(2, "ContactsN", 0) + gpu.SetValueFrom(v, ContactsN.Values) + case ContactsVar: + v, _ := syVars.ValueByIndex(2, "Contacts", 0) + gpu.SetValueFrom(v, Contacts.Values) + case JointControlsVar: + v, _ := syVars.ValueByIndex(3, "JointControls", 0) + gpu.SetValueFrom(v, JointControls.Values) + } + } +} +// RunGPUSync can be called to synchronize data between CPU and GPU. +// Any prior ToGPU* calls will execute to send data to the GPU, +// and any subsequent RunDone* calls will copy data back from the GPU. +func RunGPUSync() { + if !UseGPU { + return + } + sy := GPUSystem + sy.BeginComputePass() +} + +// ToGPUTensorStrides gets tensor strides and starts copying to the GPU. +func ToGPUTensorStrides() { + if !UseGPU { + return + } + sy := GPUSystem + syVars := sy.Vars() + TensorStrides.SetShapeSizes(110) + TensorStrides.SetInt1D(Bodies.Shape().Strides[0], 0) + TensorStrides.SetInt1D(Bodies.Shape().Strides[1], 1) + TensorStrides.SetInt1D(Joints.Shape().Strides[0], 10) + TensorStrides.SetInt1D(Joints.Shape().Strides[1], 11) + TensorStrides.SetInt1D(JointDoFs.Shape().Strides[0], 20) + TensorStrides.SetInt1D(JointDoFs.Shape().Strides[1], 21) + TensorStrides.SetInt1D(BodyJoints.Shape().Strides[0], 30) + TensorStrides.SetInt1D(BodyJoints.Shape().Strides[1], 31) + TensorStrides.SetInt1D(BodyJoints.Shape().Strides[2], 32) + TensorStrides.SetInt1D(BodyCollidePairs.Shape().Strides[0], 40) + TensorStrides.SetInt1D(BodyCollidePairs.Shape().Strides[1], 41) + TensorStrides.SetInt1D(Dynamics.Shape().Strides[0], 50) + TensorStrides.SetInt1D(Dynamics.Shape().Strides[1], 51) + TensorStrides.SetInt1D(Dynamics.Shape().Strides[2], 52) + TensorStrides.SetInt1D(BroadContactsN.Shape().Strides[0], 60) + TensorStrides.SetInt1D(BroadContacts.Shape().Strides[0], 70) + TensorStrides.SetInt1D(BroadContacts.Shape().Strides[1], 71) + TensorStrides.SetInt1D(ContactsN.Shape().Strides[0], 80) + TensorStrides.SetInt1D(Contacts.Shape().Strides[0], 90) + TensorStrides.SetInt1D(Contacts.Shape().Strides[1], 91) + TensorStrides.SetInt1D(JointControls.Shape().Strides[0], 100) + TensorStrides.SetInt1D(JointControls.Shape().Strides[1], 101) + v, _ := syVars.ValueByIndex(0, "TensorStrides", 0) + gpu.SetValueFrom(v, TensorStrides.Values) +} + +// ReadFromGPU starts the process of copying vars to the GPU. +func ReadFromGPU(vars ...GPUVars) { + sy := GPUSystem + syVars := sy.Vars() + for _, vr := range vars { + switch vr { + case ParamsVar: + v, _ := syVars.ValueByIndex(0, "Params", 0) + v.GPUToRead(sy.CommandEncoder) + case BodiesVar: + v, _ := syVars.ValueByIndex(1, "Bodies", 0) + v.GPUToRead(sy.CommandEncoder) + case JointsVar: + v, _ := syVars.ValueByIndex(1, "Joints", 0) + v.GPUToRead(sy.CommandEncoder) + case JointDoFsVar: + v, _ := syVars.ValueByIndex(1, "JointDoFs", 0) + v.GPUToRead(sy.CommandEncoder) + case BodyJointsVar: + v, _ := syVars.ValueByIndex(1, "BodyJoints", 0) + v.GPUToRead(sy.CommandEncoder) + case BodyCollidePairsVar: + v, _ := syVars.ValueByIndex(1, "BodyCollidePairs", 0) + v.GPUToRead(sy.CommandEncoder) + case DynamicsVar: + v, _ := syVars.ValueByIndex(2, "Dynamics", 0) + v.GPUToRead(sy.CommandEncoder) + case BroadContactsNVar: + v, _ := syVars.ValueByIndex(2, "BroadContactsN", 0) + v.GPUToRead(sy.CommandEncoder) + case BroadContactsVar: + v, _ := syVars.ValueByIndex(2, "BroadContacts", 0) + v.GPUToRead(sy.CommandEncoder) + case ContactsNVar: + v, _ := syVars.ValueByIndex(2, "ContactsN", 0) + v.GPUToRead(sy.CommandEncoder) + case ContactsVar: + v, _ := syVars.ValueByIndex(2, "Contacts", 0) + v.GPUToRead(sy.CommandEncoder) + case JointControlsVar: + v, _ := syVars.ValueByIndex(3, "JointControls", 0) + v.GPUToRead(sy.CommandEncoder) + } + } +} + +// SyncFromGPU synchronizes vars from the GPU to the actual variable. +func SyncFromGPU(vars ...GPUVars) { + sy := GPUSystem + syVars := sy.Vars() + for _, vr := range vars { + switch vr { + case ParamsVar: + v, _ := syVars.ValueByIndex(0, "Params", 0) + v.ReadSync() + gpu.ReadToBytes(v, Params) + case BodiesVar: + v, _ := syVars.ValueByIndex(1, "Bodies", 0) + v.ReadSync() + gpu.ReadToBytes(v, Bodies.Values) + case JointsVar: + v, _ := syVars.ValueByIndex(1, "Joints", 0) + v.ReadSync() + gpu.ReadToBytes(v, Joints.Values) + case JointDoFsVar: + v, _ := syVars.ValueByIndex(1, "JointDoFs", 0) + v.ReadSync() + gpu.ReadToBytes(v, JointDoFs.Values) + case BodyJointsVar: + v, _ := syVars.ValueByIndex(1, "BodyJoints", 0) + v.ReadSync() + gpu.ReadToBytes(v, BodyJoints.Values) + case BodyCollidePairsVar: + v, _ := syVars.ValueByIndex(1, "BodyCollidePairs", 0) + v.ReadSync() + gpu.ReadToBytes(v, BodyCollidePairs.Values) + case DynamicsVar: + v, _ := syVars.ValueByIndex(2, "Dynamics", 0) + v.ReadSync() + gpu.ReadToBytes(v, Dynamics.Values) + case BroadContactsNVar: + v, _ := syVars.ValueByIndex(2, "BroadContactsN", 0) + v.ReadSync() + gpu.ReadToBytes(v, BroadContactsN.Values) + case BroadContactsVar: + v, _ := syVars.ValueByIndex(2, "BroadContacts", 0) + v.ReadSync() + gpu.ReadToBytes(v, BroadContacts.Values) + case ContactsNVar: + v, _ := syVars.ValueByIndex(2, "ContactsN", 0) + v.ReadSync() + gpu.ReadToBytes(v, ContactsN.Values) + case ContactsVar: + v, _ := syVars.ValueByIndex(2, "Contacts", 0) + v.ReadSync() + gpu.ReadToBytes(v, Contacts.Values) + case JointControlsVar: + v, _ := syVars.ValueByIndex(3, "JointControls", 0) + v.ReadSync() + gpu.ReadToBytes(v, JointControls.Values) + } + } +} + +// GetParams returns a pointer to the given global variable: +// [Params] []PhysParams at given index. This directly processed in the GPU code, +// so this function call is an equivalent for the CPU. +func GetParams(idx uint32) *PhysParams { + return &Params[idx] +} diff --git a/physics/step_body.go b/physics/step_body.go index ae9d69ab..358df988 100644 --- a/physics/step_body.go +++ b/physics/step_body.go @@ -132,6 +132,7 @@ func StepIntegrateBodies(i uint32) { //gosl:kernel invMass := Bodies.Value(int(bi), int(BodyInvMass)) inertia := BodyInertia(bi) invInertia := BodyInvInertia(bi) + grav := params.Gravity.V() com := BodyCom(bi) @@ -150,7 +151,7 @@ func StepIntegrateBodies(i uint32) { //gosl:kernel pcom := slmath.MulQuatVector(q0, com).Add(r0) // linear part - v1 := v0.Add(f0.MulScalar(invMass).Add(params.Gravity.V().MulScalar(OneIfNonzero(invMass))).MulScalar(params.Dt)) + v1 := v0.Add(f0.MulScalar(invMass).Add(grav.MulScalar(OneIfNonzero(invMass))).MulScalar(params.Dt)) p1 := pcom.Add(v1.MulScalar(params.Dt)) // angular part (compute in body frame) @@ -201,9 +202,11 @@ func StepBodyDeltas(i uint32) { //gosl:kernel w0 := DynamicAngDelta(di, params.Next) weight := float32(1.0) - cWt := Dynamics.Value(int(di), int(params.Next), int(DynContactWeight)) - if cWt > 0 { - weight = 1.0 / cWt + if params.ContactWeighting.IsTrue() { + cWt := Dynamics.Value(int(di), int(params.Next), int(DynContactWeight)) + if cWt > 0 { + weight = 1.0 / cWt + } } dp := v0.MulScalar(invMass * weight) @@ -247,4 +250,8 @@ func StepBodyDeltas(i uint32) { //gosl:kernel SetDynamicAngDelta(di, params.Next, w1) } +func VelocityAtPoint(lin, ang, r math32.Vector3) math32.Vector3 { + return lin.Add(slmath.Cross3(ang, r)) +} + //gosl:end diff --git a/physics/step_body.goal b/physics/step_body.goal index 5f387892..a5d4cb72 100644 --- a/physics/step_body.goal +++ b/physics/step_body.goal @@ -130,6 +130,7 @@ func StepIntegrateBodies(i uint32) { //gosl:kernel invMass := Bodies[bi, BodyInvMass] inertia := BodyInertia(bi) invInertia := BodyInvInertia(bi) + grav := params.Gravity.V() com := BodyCom(bi) @@ -148,7 +149,7 @@ func StepIntegrateBodies(i uint32) { //gosl:kernel pcom := slmath.MulQuatVector(q0, com).Add(r0) // linear part - v1 := v0.Add(f0.MulScalar(invMass).Add(params.Gravity.V().MulScalar(OneIfNonzero(invMass))).MulScalar(params.Dt)) + v1 := v0.Add(f0.MulScalar(invMass).Add(grav.MulScalar(OneIfNonzero(invMass))).MulScalar(params.Dt)) p1 := pcom.Add(v1.MulScalar(params.Dt)) // angular part (compute in body frame) @@ -199,9 +200,11 @@ func StepBodyDeltas(i uint32) { //gosl:kernel w0 := DynamicAngDelta(di, params.Next) weight := float32(1.0) - cWt := Dynamics[di, params.Next, DynContactWeight] - if cWt > 0 { - weight = 1.0 / cWt + if params.ContactWeighting.IsTrue() { + cWt := Dynamics[di, params.Next, DynContactWeight] + if cWt > 0 { + weight = 1.0 / cWt + } } dp := v0.MulScalar(invMass * weight) @@ -245,4 +248,8 @@ func StepBodyDeltas(i uint32) { //gosl:kernel SetDynamicAngDelta(di, params.Next, w1) } +func VelocityAtPoint(lin, ang, r math32.Vector3) math32.Vector3 { + return lin.Add(slmath.Cross3(ang, r)) +} + //gosl:end diff --git a/physics/typegen.go b/physics/typegen.go index b3d329d9..82e7cdff 100644 --- a/physics/typegen.go +++ b/physics/typegen.go @@ -24,7 +24,7 @@ var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.JointVars", var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.JointDoFVars", IDName: "joint-do-f-vars", Doc: "JointDoFVars are joint DoF state variables stored in tensor.Float32,\none for each DoF."}) -var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.PhysParams", IDName: "phys-params", Doc: "PhysParams are the physics parameters", Directives: []types.Directive{{Tool: "gosl", Directive: "start"}}, Fields: []types.Field{{Name: "Iterations", Doc: "Iterations is the number of iterations to perform."}, {Name: "Dt", Doc: "Dt is the integration stepsize."}, {Name: "SoftRelax", Doc: "SoftRelax is soft-body relaxation constant."}, {Name: "JointLinearRelax", Doc: "JointLinearRelax is joint linear relaxation constant."}, {Name: "JointAngularRelax", Doc: "JointAngularRelax is joint angular relaxation constant."}, {Name: "JointLinearComply", Doc: "JointLinearComply is joint linear compliance constant."}, {Name: "JointAngularComply", Doc: "JointAngularComply is joint angular compliance constant."}, {Name: "ContactRelax", Doc: "ContactRelax is rigid contact relaxation constant."}, {Name: "AngularDamping", Doc: "AngularDamping is damping of angular motion."}, {Name: "ContactWeighting", Doc: "Contact weighting: todo: requires atomic add"}, {Name: "Restitution", Doc: "Restitution"}, {Name: "MaxGeomIter", Doc: "MaxGeomIter is number of iterations to perform in shape-based\ngeometry collision computations"}, {Name: "ContactMargin", Doc: "Contact margin is the extra distance for broadphase collision\naround rigid bodies."}, {Name: "ContactsMax", Doc: "Maximum number of contacts to process at any given point."}, {Name: "Cur", Doc: "Index for the current state (0 or 1, alternates with Next)."}, {Name: "Next", Doc: "Index for the next state (1 or 0, alternates with Cur)."}, {Name: "BodiesN", Doc: "BodiesN is number of rigid bodies."}, {Name: "DynamicsN", Doc: "DynamicsN is number of dynamics bodies."}, {Name: "JointsN", Doc: "JointsN is number of joints."}, {Name: "JointDoFsN", Doc: "JointDoFsN is number of joint DoFs."}, {Name: "BodyJointsMax", Doc: "BodyJointsMax is max number of joints per body + 1 for actual n."}, {Name: "BodyCollidePairsN", Doc: "BodyCollidePairsN is the total number of pre-compiled collision pairs\nto examine."}, {Name: "pad"}, {Name: "pad1"}, {Name: "Gravity", Doc: "Gravity is the gravity acceleration function"}}}) +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.PhysParams", IDName: "phys-params", Doc: "PhysParams are the physics parameters", Directives: []types.Directive{{Tool: "gosl", Directive: "start"}}, Fields: []types.Field{{Name: "Iterations", Doc: "Iterations is the number of iterations to perform."}, {Name: "Dt", Doc: "Dt is the integration stepsize."}, {Name: "SoftRelax", Doc: "SoftRelax is soft-body relaxation constant."}, {Name: "JointLinearRelax", Doc: "JointLinearRelax is joint linear relaxation constant."}, {Name: "JointAngularRelax", Doc: "JointAngularRelax is joint angular relaxation constant."}, {Name: "JointLinearComply", Doc: "JointLinearComply is joint linear compliance constant."}, {Name: "JointAngularComply", Doc: "JointAngularComply is joint angular compliance constant."}, {Name: "ContactRelax", Doc: "ContactRelax is rigid contact relaxation constant."}, {Name: "AngularDamping", Doc: "AngularDamping is damping of angular motion."}, {Name: "ContactWeighting", Doc: "Contact weighting: balances contact forces?"}, {Name: "Restitution", Doc: "Restitution takes into account bounciness of objects."}, {Name: "MaxGeomIter", Doc: "MaxGeomIter is number of iterations to perform in shape-based\ngeometry collision computations"}, {Name: "ContactMargin", Doc: "Contact margin is the extra distance for broadphase collision\naround rigid bodies."}, {Name: "ContactsMax", Doc: "Maximum number of contacts to process at any given point."}, {Name: "Cur", Doc: "Index for the current state (0 or 1, alternates with Next)."}, {Name: "Next", Doc: "Index for the next state (1 or 0, alternates with Cur)."}, {Name: "BodiesN", Doc: "BodiesN is number of rigid bodies."}, {Name: "DynamicsN", Doc: "DynamicsN is number of dynamics bodies."}, {Name: "JointsN", Doc: "JointsN is number of joints."}, {Name: "JointDoFsN", Doc: "JointDoFsN is number of joint DoFs."}, {Name: "BodyJointsMax", Doc: "BodyJointsMax is max number of joints per body + 1 for actual n."}, {Name: "BodyCollidePairsN", Doc: "BodyCollidePairsN is the total number of pre-compiled collision pairs\nto examine."}, {Name: "pad"}, {Name: "pad1"}, {Name: "Gravity", Doc: "Gravity is the gravity acceleration function"}}}) var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.GeomData", IDName: "geom-data", Doc: "GeomData contains all geometric data for narrow-phase collision.", Directives: []types.Directive{{Tool: "gosl", Directive: "start"}}, Fields: []types.Field{{Name: "BodyIdx"}, {Name: "Shape"}, {Name: "MinSize", Doc: "MinSize is the min of the Size dimensions."}, {Name: "Thick", Doc: "Thickness of shape."}, {Name: "Radius", Doc: "Radius is the effective radius for sphere-like elements (Sphere, Capsule, Cone)"}, {Name: "Size"}, {Name: "WbR", Doc: "World-to-Body transform\nPosition (R) (i.e., BodyPos)"}, {Name: "WbQ", Doc: "Quaternion (Q) (i.e., BodyQuat)"}, {Name: "BwR", Doc: "Body-to-World transform (inverse)\nPosition (R)"}, {Name: "BwQ", Doc: "Quaternion (Q)"}}}) diff --git a/physics/vars.go b/physics/vars.go index 390d0d9a..0bcf01d9 100644 --- a/physics/vars.go +++ b/physics/vars.go @@ -7,7 +7,7 @@ package physics import "cogentcore.org/lab/tensor" // note: add -keep to inspect intermediate .go code -//go:generate gosl -exclude=Update,Defaults,ShouldDisplay -max-buffer-size=2147483616 +//go:generate gosl -keep -exclude=Update,Defaults,ShouldDisplay -max-buffer-size=2147483616 //gosl:start From 7077754e9cca9e9f8cd38638b8b658f083bf9a86 Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Mon, 22 Dec 2025 14:12:42 +0100 Subject: [PATCH 40/97] physics: key fix for gosl.go in gosl -- working properly now --- gosl/gotosl/nodes.go | 13 +++++++++++-- physics/examples/test1/test1.go | 12 ++++++++++-- 2 files changed, 21 insertions(+), 4 deletions(-) diff --git a/gosl/gotosl/nodes.go b/gosl/gotosl/nodes.go index 46520d01..5fe94dc1 100644 --- a/gosl/gotosl/nodes.go +++ b/gosl/gotosl/nodes.go @@ -526,6 +526,7 @@ func (p *printer) goslFixArgs(args []ast.Expr, params *types.Tuple) ([]ast.Expr, if gvar := p.GoToSL.GetTempVar(x.Name); gvar != nil { if !(gvar.Var.ReadOnly && !gvar.ReadWrite) { x.Name = "&" + x.Name + fmt.Println("fix amper", x.Name) ags[i] = x } } @@ -1977,8 +1978,16 @@ func (p *printer) methodExpr(x *ast.CallExpr, depth int) { recvType = id.Name // is a package path } } else { - pathType = typ - recvPath = recvPath + if gvar := p.GoToSL.GetTempVar(id.Name); gvar != nil { + recvType = gvar.Var.SLType() + if !(gvar.Var.ReadOnly && !gvar.ReadWrite) { + recvPath = "&" + recvPath + } + pathType = p.getTypeNameType(gvar.Var.SLType()) + } else { + pathType = typ + recvPath = recvPath + } } } else { pathIsPackage = true diff --git a/physics/examples/test1/test1.go b/physics/examples/test1/test1.go index d0d71107..f2b3d931 100644 --- a/physics/examples/test1/test1.go +++ b/physics/examples/test1/test1.go @@ -75,7 +75,7 @@ func main() { wr.Init(wl) params := physics.GetParams(0) - params.Dt = 0.01 + params.Dt = 0.001 // params.Gravity.Y = 0 fmt.Println(params.ContactRelax) @@ -94,13 +94,19 @@ func main() { sc.SaveCamera("1") sc.SaveCamera("default") + isRunning := false + stepNButton := func(p *tree.Plan, n int) { nm := fmt.Sprintf("Step %d", n) tree.AddAt(p, nm, func(w *core.Button) { w.SetText(nm).SetIcon(icons.PlayArrow). SetTooltip(fmt.Sprintf("Step state %d times", n)). OnClick(func(e events.Event) { + if isRunning { + return + } go func() { + isRunning = true for range n { wl.Step() wr.Update() @@ -108,9 +114,10 @@ func main() { se.AsyncLock() se.NeedsRender() se.AsyncUnlock() - time.Sleep(10 * time.Millisecond) + time.Sleep(1 * time.Millisecond) } } + isRunning = false }() }) w.Styler(func(s *styles.Style) { @@ -135,6 +142,7 @@ func main() { stepNButton(p, 1) stepNButton(p, 10) stepNButton(p, 100) + stepNButton(p, 1000) }) }) b.RunMainWindow() From 45724d787d9a339370a7d7de9d096350acb2ce6b Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Mon, 22 Dec 2025 15:00:55 +0100 Subject: [PATCH 41/97] physics: balls example working -- looks like contact.relax needs to be lower... --- physics/contact.go | 13 ++++++++----- physics/contact.goal | 13 ++++++++----- physics/examples/test1/test1.go | 5 ++++- physics/shaders/StepBodyContacts.wgsl | 12 +++++++----- physics/vars.go | 2 +- physics/world.go | 1 + physics/world.goal | 1 + physics/world/view.go | 4 ++-- 8 files changed, 32 insertions(+), 19 deletions(-) diff --git a/physics/contact.go b/physics/contact.go index 9b161395..060adc05 100644 --- a/physics/contact.go +++ b/physics/contact.go @@ -531,17 +531,20 @@ func StepBodyContacts(i uint32) { //gosl:kernel // we include any rotational effects due to thickness from feature // need to use the current rotation to account for friction due to // angular effects (e.g.: slipping contact) - ctAw = ctAw.Add(slmath.MulQuatVector(q1A, offA)) - ctBw = ctBw.Add(slmath.MulQuatVector(q1B, offB)) + ctAm := ctAw.Add(slmath.MulQuatVector(q1A, offA)) + ctBm := ctBw.Add(slmath.MulQuatVector(q1B, offB)) // update delta - delta := ctBw.Sub(ctAw) + delta := ctBm.Sub(ctAm) frDelta := delta.Sub(norm.MulScalar(slmath.Dot3(norm, delta))) perp := slmath.Normal3(frDelta) - angA = slmath.Negate3(slmath.Cross3(dA, perp)) - angB = slmath.Cross3(dB, perp) + dAm := ctAm.Sub(slmath.MulSpatialPoint(r1A, q1A, comA)) + dBm := ctBm.Sub(slmath.MulSpatialPoint(r1B, q1B, comB)) + + angA = slmath.Negate3(slmath.Cross3(dAm, perp)) + angB = slmath.Cross3(dBm, perp) err := slmath.Length3(frDelta) diff --git a/physics/contact.goal b/physics/contact.goal index 95f7faab..486d82b5 100644 --- a/physics/contact.goal +++ b/physics/contact.goal @@ -529,17 +529,20 @@ func StepBodyContacts(i uint32) { //gosl:kernel // we include any rotational effects due to thickness from feature // need to use the current rotation to account for friction due to // angular effects (e.g.: slipping contact) - ctAw = ctAw.Add(slmath.MulQuatVector(q1A, offA)) - ctBw = ctBw.Add(slmath.MulQuatVector(q1B, offB)) + ctAm := ctAw.Add(slmath.MulQuatVector(q1A, offA)) + ctBm := ctBw.Add(slmath.MulQuatVector(q1B, offB)) // update delta - delta := ctBw.Sub(ctAw) + delta := ctBm.Sub(ctAm) frDelta := delta.Sub(norm.MulScalar(slmath.Dot3(norm, delta))) perp := slmath.Normal3(frDelta) - angA = slmath.Negate3(slmath.Cross3(dA, perp)) - angB = slmath.Cross3(dB, perp) + dAm := ctAm.Sub(slmath.MulSpatialPoint(r1A, q1A, comA)) + dBm := ctBm.Sub(slmath.MulSpatialPoint(r1B, q1B, comB)) + + angA = slmath.Negate3(slmath.Cross3(dAm, perp)) + angB = slmath.Cross3(dBm, perp) err := slmath.Length3(frDelta) diff --git a/physics/examples/test1/test1.go b/physics/examples/test1/test1.go index f2b3d931..9abf4b3f 100644 --- a/physics/examples/test1/test1.go +++ b/physics/examples/test1/test1.go @@ -46,6 +46,7 @@ func main() { wr := world.NewWorld(sc) wl := physics.NewWorld() + // wl.GPU = true fv.SetStruct(wl) split.SetSplits(0.2, 0.8) @@ -75,8 +76,10 @@ func main() { wr.Init(wl) params := physics.GetParams(0) - params.Dt = 0.001 + params.Dt = 0.01 // params.Gravity.Y = 0 + params.ContactRelax = 0.1 + params.Restitution.SetBool(false) fmt.Println(params.ContactRelax) wr.Update() diff --git a/physics/shaders/StepBodyContacts.wgsl b/physics/shaders/StepBodyContacts.wgsl index 25cf1646..2c999fdf 100644 --- a/physics/shaders/StepBodyContacts.wgsl +++ b/physics/shaders/StepBodyContacts.wgsl @@ -223,13 +223,15 @@ u32(biB), u32(BodyBounce))]); ; var angDeltaB = angB*(lambdaN); ; if (mu > 0.0) { - ctAw = ctAw+(MulQuatVector(q1A, offA)); - ctBw = ctBw+(MulQuatVector(q1B, offB)); - var delta = ctBw-(ctAw); + var ctAm = ctAw+(MulQuatVector(q1A, offA)); + var ctBm = ctBw+(MulQuatVector(q1B, offB)); + var delta = ctBm-(ctAm); var frDelta = delta-(norm*(Dot3(norm, delta))); var perp = Normal3(frDelta); - angA = Negate3(Cross3(dA, perp)); - angB = Cross3(dB, perp); + var dAm = ctAm-(MulSpatialPoint(r1A, q1A, comA)); + var dBm = ctBm-(MulSpatialPoint(r1B, q1B, comB)); + angA = Negate3(Cross3(dAm, perp)); + angB = Cross3(dBm, perp); var err = Length3(frDelta); if (err > 0.0) { var lambdaFr = ContactConstraint(err, q1A, q1B, mInvA, mInvB, iInvA, iInvB, Negate3(perp), perp, angA, angB, params.ContactRelax, params.Dt); diff --git a/physics/vars.go b/physics/vars.go index 0bcf01d9..390d0d9a 100644 --- a/physics/vars.go +++ b/physics/vars.go @@ -7,7 +7,7 @@ package physics import "cogentcore.org/lab/tensor" // note: add -keep to inspect intermediate .go code -//go:generate gosl -keep -exclude=Update,Defaults,ShouldDisplay -max-buffer-size=2147483616 +//go:generate gosl -exclude=Update,Defaults,ShouldDisplay -max-buffer-size=2147483616 //gosl:start diff --git a/physics/world.go b/physics/world.go index 969c82fc..9f880785 100644 --- a/physics/world.go +++ b/physics/world.go @@ -83,6 +83,7 @@ func NewWorld() *World { // Init makes initial vars. func (wl *World) Init() { + wl.GPU = true wl.Params = make([]PhysParams, 1) wl.Params[0].Defaults() wl.Bodies = tensor.NewFloat32(0, int(BodyVarsN)) diff --git a/physics/world.goal b/physics/world.goal index bd2d5053..db1101aa 100644 --- a/physics/world.goal +++ b/physics/world.goal @@ -81,6 +81,7 @@ func NewWorld() *World { // Init makes initial vars. func (wl *World) Init() { + wl.GPU = true wl.Params = make([]PhysParams, 1) wl.Params[0].Defaults() wl.Bodies = tensor.NewFloat32(0, int(BodyVarsN)) diff --git a/physics/world/view.go b/physics/world/view.go index d3bbe78f..f61df6a2 100644 --- a/physics/world/view.go +++ b/physics/world/view.go @@ -157,8 +157,8 @@ func (vw *View) PlaneInit(sld *xyz.Solid) { } sld.SetMeshName(mnm) if vw.Size.X == 0 { - inf := float32(1e6) - sld.Pose.Scale = math32.Vec3(inf, inf, 1) + inf := float32(1e3) + sld.Pose.Scale = math32.Vec3(inf, 1, inf) } else { sld.Pose.Scale = vw.Size.MulScalar(2) } From bdfdae493f626a1c36229e4bc0029b0826c13797 Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Mon, 22 Dec 2025 22:19:40 +0100 Subject: [PATCH 42/97] physics: gpu working -- very efficient for even 10k balls it seems --- physics/config.go | 20 +++- physics/config.goal | 20 +++- physics/contact.go | 7 +- physics/contact.goal | 7 +- physics/examples/balls/balls.go | 165 ++++++++++++++++++++++++++++++++ physics/examples/test1/test1.go | 4 +- physics/step.go | 9 +- physics/step.goal | 9 +- physics/world.go | 4 +- physics/world.goal | 4 +- 10 files changed, 230 insertions(+), 19 deletions(-) create mode 100644 physics/examples/balls/balls.go diff --git a/physics/config.go b/physics/config.go index 2890bc7d..5cbc9756 100644 --- a/physics/config.go +++ b/physics/config.go @@ -6,7 +6,11 @@ package physics -// import "fmt" +import ( + // "fmt" + + "cogentcore.org/lab/tensor" +) // Config does final configuration prior to running // after everything has been added. Does SetAsCurrent, GPUInit. @@ -43,8 +47,13 @@ func (wl *World) ConfigJoints() { bjc[jci] = append(bjc[jci], ji) maxi = max(maxi, len(bjc[jci])) } - maxi = maxi + 1 // extra for n params.BodyJointsMax = int32(maxi) + if nd == 0 { + nd = 1 + } + if maxi == 0 { + maxi = 1 + } wl.BodyJoints.SetShapeSizes(int(nd), 2, maxi) for di := range nd { np := int32(len(bjp[di])) @@ -58,6 +67,11 @@ func (wl *World) ConfigJoints() { wl.BodyJoints.Set(ji, int(di), int(1), int(1+i)) } } + if nj == 0 { + wl.Joints = tensor.NewFloat32(1, int(JointVarsN)) + wl.JointDoFs = tensor.NewFloat32(1, int(JointDoFVarsN)) + wl.JointControls = tensor.NewFloat32(1, int(JointControlVarsN)) + } } // ConfigBodies updates computed body values from current values. @@ -76,5 +90,7 @@ func (wl *World) ConfigBodies() { // InitState initializes the simulation state. func (wl *World) InitState() { params := GetParams(0) + wl.ToGPUInfra() RunInitDynamics(int(params.DynamicsN)) + RunDone(DynamicsVar) } diff --git a/physics/config.goal b/physics/config.goal index fb2aaaac..34609d31 100644 --- a/physics/config.goal +++ b/physics/config.goal @@ -4,7 +4,11 @@ package physics -// import "fmt" +import ( + // "fmt" + + "cogentcore.org/lab/tensor" +) // Config does final configuration prior to running // after everything has been added. Does SetAsCurrent, GPUInit. @@ -41,8 +45,13 @@ func (wl *World) ConfigJoints() { bjc[jci] = append(bjc[jci], ji) maxi = max(maxi, len(bjc[jci])) } - maxi = maxi+1 // extra for n params.BodyJointsMax = int32(maxi) + if nd == 0 { + nd = 1 + } + if maxi == 0 { + maxi = 1 + } wl.BodyJoints.SetShapeSizes(int(nd), 2, maxi) for di := range nd { np := int32(len(bjp[di])) @@ -56,6 +65,11 @@ func (wl *World) ConfigJoints() { wl.BodyJoints[di, 1, 1+i] = ji } } + if nj == 0 { + wl.Joints = tensor.NewFloat32(1, int(JointVarsN)) + wl.JointDoFs = tensor.NewFloat32(1, int(JointDoFVarsN)) + wl.JointControls = tensor.NewFloat32(1, int(JointControlVarsN)) + } } // ConfigBodies updates computed body values from current values. @@ -74,6 +88,8 @@ func (wl *World) ConfigBodies() { // InitState initializes the simulation state. func (wl *World) InitState() { params := GetParams(0) + wl.ToGPUInfra() RunInitDynamics(int(params.DynamicsN)) + RunDone(DynamicsVar) } diff --git a/physics/contact.go b/physics/contact.go index 060adc05..8cc541f2 100644 --- a/physics/contact.go +++ b/physics/contact.go @@ -747,6 +747,9 @@ func (wl *World) ConfigBodyCollidePairs() { ga := GetBodyGroup(a) dia := GetBodyDynamic(a) for b := range nb { + if a == b { + continue + } wb := GetBodyWorld(b) gb := GetBodyGroup(b) if !WorldsCollide(wa, wb) { @@ -763,7 +766,7 @@ func (wl *World) ConfigBodyCollidePairs() { if np >= nalc { nalc += int(nb) pt.SetShapeSizes(nalc, 2) - fmt.Println("body pairs realoc", nalc) + // fmt.Println("body pairs realoc", nalc) } sA := GetBodyShape(a) @@ -782,7 +785,7 @@ func (wl *World) ConfigBodyCollidePairs() { pt.SetShapeSizes(np, 2) wl.BodyCollidePairs = pt BodyCollidePairs = pt - fmt.Println("body pairs over alloc", nalc, np) + fmt.Println("body pairs over alloc", nalc-np, "total:", np) } // newton: geometry/kernels.py: count_contact_points diff --git a/physics/contact.goal b/physics/contact.goal index 486d82b5..20346838 100644 --- a/physics/contact.goal +++ b/physics/contact.goal @@ -746,6 +746,9 @@ func (wl *World) ConfigBodyCollidePairs() { ga := GetBodyGroup(a) dia := GetBodyDynamic(a) for b := range nb { + if a == b { + continue + } wb := GetBodyWorld(b) gb := GetBodyGroup(b) if !WorldsCollide(wa, wb) { @@ -762,7 +765,7 @@ func (wl *World) ConfigBodyCollidePairs() { if np >= nalc { nalc += int(nb) pt.SetShapeSizes(nalc, 2) - fmt.Println("body pairs realoc", nalc) + // fmt.Println("body pairs realoc", nalc) } sA := GetBodyShape(a) @@ -781,7 +784,7 @@ func (wl *World) ConfigBodyCollidePairs() { pt.SetShapeSizes(np, 2) wl.BodyCollidePairs = pt BodyCollidePairs = pt - fmt.Println("body pairs over alloc", nalc, np) + fmt.Println("body pairs over alloc", nalc - np, "total:", np) } // newton: geometry/kernels.py: count_contact_points diff --git a/physics/examples/balls/balls.go b/physics/examples/balls/balls.go new file mode 100644 index 00000000..4e1e6d70 --- /dev/null +++ b/physics/examples/balls/balls.go @@ -0,0 +1,165 @@ +// Copyright (c) 2019, Cogent Core. All rights reserved. +// Use of this source code is governed by a BSD-style +// license that can be found in the LICENSE file. + +package main + +//go:generate core generate + +import ( + "fmt" + "math/rand/v2" + + "cogentcore.org/core/colors" + "cogentcore.org/core/core" + "cogentcore.org/core/events" + "cogentcore.org/core/icons" + "cogentcore.org/core/math32" + "cogentcore.org/core/styles" + "cogentcore.org/core/styles/abilities" + "cogentcore.org/core/tree" + "cogentcore.org/core/xyz" + "cogentcore.org/core/xyz/xyzcore" + "cogentcore.org/lab/physics" + "cogentcore.org/lab/physics/world" +) + +func main() { + // gpu.Debug = true + b := core.NewBody("test1").SetTitle("Physics Balls") + split := core.NewSplits(b) + // tv := core.NewTree(core.NewFrame(split)) + fv := core.NewForm(split) + tbvw := core.NewTabs(split) + scfr, _ := tbvw.NewTab("3D View") + + se := xyzcore.NewSceneEditor(scfr) + se.UpdateWidget() + sc := se.SceneXYZ() + + sc.Background = colors.Scheme.Select.Container + xyz.NewAmbient(sc, "ambient", 0.3, xyz.DirectSun) + + dir := xyz.NewDirectional(sc, "dir", 1, xyz.DirectSun) + dir.Pos.Set(0, 2, 1) + + wr := world.NewWorld(sc) + + wl := physics.NewWorld() + wl.GPU = true + fv.SetStruct(wl) + + split.SetSplits(0.2, 0.8) + + rot := math32.NewQuat(0, 0, 0, 1) + wr.NewBody(wl, "floor", physics.Plane, "#D0D0D040", math32.Vec3(0, 0, 0), + math32.Vec3(0, 0, 0), rot) + + nballs := 10000 + size := float32(0.1) + bounce := float32(0.5) + box := 2 * size * math32.Sqrt(float32(nballs)) + height := float32(20) + for i := range nballs { + _ = i + ht := rand.Float32() * height + x := rand.Float32()*box - 0.5*box + z := rand.Float32()*box - 0.5*box + clr := colors.Names[i%len(colors.Names)] + b1 := wr.NewDynamic(wl, "body", physics.Sphere, clr, 1.0, math32.Vec3(size, size, size), + math32.Vec3(x, size+ht, z), rot) + // todo: helper methods on view to set this stuff: + physics.Bodies.Set(bounce, int(b1.Index), int(physics.BodyBounce)) + physics.SetBodyGroup(b1.Index, int32(i)) // no self collisions + } + wr.Init(wl) + + params := physics.GetParams(0) + params.Dt = 0.001 + // params.Gravity.Y = 0 + params.ContactRelax = 0.1 + params.Restitution.SetBool(false) + fmt.Println(params.ContactRelax) + + wl.Config() + wr.Update() + + sc.Camera.Pose.Pos = math32.Vec3(0, 40, 3.5) + sc.Camera.LookAt(math32.Vec3(0, 5, 0), math32.Vec3(0, 1, 0)) + sc.SaveCamera("3") + + sc.Camera.Pose.Pos = math32.Vec3(0, 20, 30) + sc.Camera.LookAt(math32.Vec3(0, 5, 0), math32.Vec3(0, 1, 0)) + sc.SaveCamera("2") + + sc.Camera.Pose.Pos = math32.Vec3(-1.33, 2.24, 3.55) + sc.Camera.LookAt(math32.Vec3(0, .5, 0), math32.Vec3(0, 1, 0)) + sc.SaveCamera("1") + sc.SaveCamera("default") + + isRunning := false + stop := false + + stepNButton := func(p *tree.Plan, n int) { + nm := fmt.Sprintf("Step %d", n) + tree.AddAt(p, nm, func(w *core.Button) { + w.SetText(nm).SetIcon(icons.PlayArrow). + SetTooltip(fmt.Sprintf("Step state %d times", n)). + OnClick(func(e events.Event) { + if isRunning { + return + } + go func() { + isRunning = true + for range n { + wl.Step() + wr.Update() + if se.IsVisible() { + se.AsyncLock() + se.NeedsRender() + se.AsyncUnlock() + // time.Sleep(1 * time.Nanosecond) + } + if stop { + stop = false + break + } + } + isRunning = false + }() + }) + w.Styler(func(s *styles.Style) { + s.SetAbilities(true, abilities.RepeatClickable) + }) + }) + } + + b.AddTopBar(func(bar *core.Frame) { + core.NewToolbar(bar).Maker(func(p *tree.Plan) { + tree.Add(p, func(w *core.Button) { + w.SetText("Init").SetIcon(icons.Reset). + SetTooltip("Reset state"). + OnClick(func(e events.Event) { + wl.InitState() + wr.Update() + if se.IsVisible() { + se.NeedsRender() + } + }) + }) + tree.Add(p, func(w *core.Button) { + w.SetText("Stop").SetIcon(icons.Stop). + SetTooltip("Stop running"). + OnClick(func(e events.Event) { + stop = true + }) + }) + stepNButton(p, 1) + stepNButton(p, 10) + stepNButton(p, 100) + stepNButton(p, 1000) + stepNButton(p, 10000) + }) + }) + b.RunMainWindow() +} diff --git a/physics/examples/test1/test1.go b/physics/examples/test1/test1.go index 9abf4b3f..67c11cf5 100644 --- a/physics/examples/test1/test1.go +++ b/physics/examples/test1/test1.go @@ -46,7 +46,7 @@ func main() { wr := world.NewWorld(sc) wl := physics.NewWorld() - // wl.GPU = true + wl.GPU = true fv.SetStruct(wl) split.SetSplits(0.2, 0.8) @@ -82,6 +82,8 @@ func main() { params.Restitution.SetBool(false) fmt.Println(params.ContactRelax) + wl.Config() + wr.Update() sc.Camera.Pose.Pos = math32.Vec3(0, 40, 3.5) diff --git a/physics/step.go b/physics/step.go index 1c1e26e0..319825aa 100644 --- a/physics/step.go +++ b/physics/step.go @@ -57,7 +57,7 @@ func (wl *World) Step() { params.Cur = 0 params.Next = 1 } - ToGPU(JointControlsVar) + ToGPU(ParamsVar, JointControlsVar) wl.StepCollision() wl.StepJointForces() wl.StepIntegrateBodies() @@ -76,6 +76,7 @@ func (wl *World) StepCollision() { // note: time getting BroadContactsN back down and using that vs. running full RunCollisionNarrow(int(params.ContactsMax)) RunDone(ContactsNVar) // we do multiple iterations so useful to have this + // fmt.Println("contacts:", ContactsN.Value(0), "max:", params.ContactsMax) } func (wl *World) StepJointForces() { @@ -98,6 +99,8 @@ func (wl *World) StepSolveJoints() { func (wl *World) StepBodyContacts() { params := GetParams(0) cmax := int(ContactsN.Values[0]) - RunStepBodyContacts(cmax) - RunDeltasFromContacts(int(params.DynamicsN)) + if cmax > 0 { + RunStepBodyContacts(cmax) + RunDeltasFromContacts(int(params.DynamicsN)) + } } diff --git a/physics/step.goal b/physics/step.goal index d2083817..7257363e 100644 --- a/physics/step.goal +++ b/physics/step.goal @@ -55,7 +55,7 @@ func (wl *World) Step() { params.Cur = 0 params.Next = 1 } - ToGPU(JointControlsVar) + ToGPU(ParamsVar, JointControlsVar) wl.StepCollision() wl.StepJointForces() wl.StepIntegrateBodies() @@ -74,6 +74,7 @@ func (wl *World) StepCollision() { // note: time getting BroadContactsN back down and using that vs. running full RunCollisionNarrow(int(params.ContactsMax)) RunDone(ContactsNVar) // we do multiple iterations so useful to have this + // fmt.Println("contacts:", ContactsN.Value(0), "max:", params.ContactsMax) } func (wl *World) StepJointForces() { @@ -96,6 +97,8 @@ func (wl *World) StepSolveJoints() { func (wl *World) StepBodyContacts() { params := GetParams(0) cmax := int(ContactsN.Values[0]) - RunStepBodyContacts(cmax) - RunDeltasFromContacts(int(params.DynamicsN)) + if cmax > 0 { + RunStepBodyContacts(cmax) + RunDeltasFromContacts(int(params.DynamicsN)) + } } diff --git a/physics/world.go b/physics/world.go index 9f880785..fcafa30e 100644 --- a/physics/world.go +++ b/physics/world.go @@ -153,9 +153,9 @@ func (wl *World) SetAsCurrentVars() { Params = wl.Params Bodies = wl.Bodies Joints = wl.Joints + JointDoFs = wl.JointDoFs BodyJoints = wl.BodyJoints BodyCollidePairs = wl.BodyCollidePairs - JointDoFs = wl.JointDoFs Dynamics = wl.Dynamics BroadContactsN = wl.BroadContactsN BroadContacts = wl.BroadContacts @@ -176,5 +176,5 @@ func (wl *World) GPUInit() { // the GPU. This is done in GPUInit, and if current switched. func (wl *World) ToGPUInfra() { ToGPUTensorStrides() - ToGPU(ParamsVar, BodiesVar, JointsVar, BodyJointsVar, BodyCollidePairsVar, JointDoFsVar, DynamicsVar, BroadContactsNVar, BroadContactsVar, ContactsNVar, ContactsVar) + ToGPU(ParamsVar, BodiesVar, JointsVar, JointDoFsVar, BodyJointsVar, BodyCollidePairsVar, DynamicsVar, BroadContactsNVar, BroadContactsVar, ContactsNVar, ContactsVar, JointControlsVar) } diff --git a/physics/world.goal b/physics/world.goal index db1101aa..acf31f53 100644 --- a/physics/world.goal +++ b/physics/world.goal @@ -151,9 +151,9 @@ func (wl *World) SetAsCurrentVars() { Params = wl.Params Bodies = wl.Bodies Joints = wl.Joints + JointDoFs = wl.JointDoFs BodyJoints = wl.BodyJoints BodyCollidePairs = wl.BodyCollidePairs - JointDoFs = wl.JointDoFs Dynamics = wl.Dynamics BroadContactsN = wl.BroadContactsN BroadContacts = wl.BroadContacts @@ -174,5 +174,5 @@ func (wl *World) GPUInit() { // the GPU. This is done in GPUInit, and if current switched. func (wl *World) ToGPUInfra() { ToGPUTensorStrides() - ToGPU(ParamsVar, BodiesVar, JointsVar, BodyJointsVar, BodyCollidePairsVar, JointDoFsVar, DynamicsVar, BroadContactsNVar, BroadContactsVar, ContactsNVar, ContactsVar) + ToGPU(ParamsVar, BodiesVar, JointsVar, JointDoFsVar, BodyJointsVar, BodyCollidePairsVar, DynamicsVar, BroadContactsNVar, BroadContactsVar, ContactsNVar, ContactsVar, JointControlsVar) } From 84d33aaf90a581b1c69646f4cb573a4e6484017c Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Mon, 22 Dec 2025 22:24:40 +0100 Subject: [PATCH 43/97] physics: minor test fix --- goal/testdata/test.goal | 3 - physics/shaders/imports/gosl.go | 927 -------------------------------- 2 files changed, 930 deletions(-) delete mode 100644 physics/shaders/imports/gosl.go diff --git a/goal/testdata/test.goal b/goal/testdata/test.goal index c5818450..a93be7ec 100644 --- a/goal/testdata/test.goal +++ b/goal/testdata/test.goal @@ -9,9 +9,6 @@ fmt.Println(x) fmt.Println(nd) fmt.Println(sz) fmt.Println(sh) -//gosl:wgsl -// a += 5 -//gosl:end type MyStru struct { Name string diff --git a/physics/shaders/imports/gosl.go b/physics/shaders/imports/gosl.go deleted file mode 100644 index ce4a7ec8..00000000 --- a/physics/shaders/imports/gosl.go +++ /dev/null @@ -1,927 +0,0 @@ -// Code generated by "gosl"; DO NOT EDIT - -package imports - -import ( - "embed" - "fmt" - "math" - "unsafe" - "cogentcore.org/core/gpu" - "cogentcore.org/lab/tensor" -) - -var shaders embed.FS - -var ( - // GPUInitialized is true once the GPU system has been initialized. - // Prevents multiple initializations. - GPUInitialized bool - - // ComputeGPU is the compute gpu device. - // Set this prior to calling GPUInit() to use an existing device. - ComputeGPU *gpu.GPU - - // BorrowedGPU is true if our ComputeGPU is set externally, - // versus created specifically for this system. If external, - // we don't release it. - BorrowedGPU bool - - // UseGPU indicates whether to use GPU vs. CPU. - UseGPU bool -) -// GPUSystem is a GPU compute System with kernels operating on the -// same set of data variables. -var GPUSystem *gpu.ComputeSystem - -// GPUVars is an enum for GPU variables, for specifying what to sync. -type GPUVars int32 //enums:enum - -const ( - ParamsVar GPUVars = 0 - BodiesVar GPUVars = 1 - JointsVar GPUVars = 2 - JointDoFsVar GPUVars = 3 - BodyJointsVar GPUVars = 4 - BodyCollidePairsVar GPUVars = 5 - DynamicsVar GPUVars = 6 - BroadContactsNVar GPUVars = 7 - BroadContactsVar GPUVars = 8 - ContactsNVar GPUVars = 9 - ContactsVar GPUVars = 10 - JointControlsVar GPUVars = 11 -) - -// Tensor stride variables -var TensorStrides tensor.Uint32 - -// GPUInit initializes the GPU compute system, -// configuring system(s), variables and kernels. -// It is safe to call multiple times: detects if already run. -func GPUInit() { - if GPUInitialized { - return - } - GPUInitialized = true - if ComputeGPU == nil { // set prior to this call to use an external - ComputeGPU = gpu.NewComputeGPU() - } else { - BorrowedGPU = true - } - gp := ComputeGPU - - _ = fmt.Sprintf("%g",math.NaN()) // keep imports happy - { - sy := gpu.NewComputeSystem(gp, "Default") - GPUSystem = sy - vars := sy.Vars() - { - sgp := vars.AddGroup(gpu.Storage, "Params") - var vr *gpu.Var - _ = vr - vr = sgp.Add("TensorStrides", gpu.Uint32, 1, gpu.ComputeShader) - vr.ReadOnly = true - vr = sgp.AddStruct("Params", int(unsafe.Sizeof(PhysParams{})), 1, gpu.ComputeShader) - vr.ReadOnly = true - sgp.SetNValues(1) - } - { - sgp := vars.AddGroup(gpu.Storage, "Bodies") - var vr *gpu.Var - _ = vr - vr = sgp.Add("Bodies", gpu.Float32, 1, gpu.ComputeShader) - vr = sgp.Add("Joints", gpu.Float32, 1, gpu.ComputeShader) - vr = sgp.Add("JointDoFs", gpu.Float32, 1, gpu.ComputeShader) - vr = sgp.Add("BodyJoints", gpu.Int32, 1, gpu.ComputeShader) - vr = sgp.Add("BodyCollidePairs", gpu.Int32, 1, gpu.ComputeShader) - sgp.SetNValues(1) - } - { - sgp := vars.AddGroup(gpu.Storage, "Dynamics") - var vr *gpu.Var - _ = vr - vr = sgp.Add("Dynamics", gpu.Float32, 1, gpu.ComputeShader) - vr = sgp.Add("BroadContactsN", gpu.Int32, 1, gpu.ComputeShader) - vr = sgp.Add("BroadContacts", gpu.Float32, 1, gpu.ComputeShader) - vr = sgp.Add("ContactsN", gpu.Int32, 1, gpu.ComputeShader) - vr = sgp.Add("Contacts", gpu.Float32, 1, gpu.ComputeShader) - sgp.SetNValues(1) - } - { - sgp := vars.AddGroup(gpu.Storage, "Controls") - var vr *gpu.Var - _ = vr - vr = sgp.Add("JointControls", gpu.Float32, 1, gpu.ComputeShader) - sgp.SetNValues(1) - } - var pl *gpu.ComputePipeline - pl = gpu.NewComputePipelineShaderFS(shaders, "", sy) - pl.AddVarUsed(0, "TensorStrides") - pl = gpu.NewComputePipelineShaderFS(shaders, "", sy) - pl.AddVarUsed(0, "TensorStrides") - pl = gpu.NewComputePipelineShaderFS(shaders, "", sy) - pl.AddVarUsed(0, "TensorStrides") - pl = gpu.NewComputePipelineShaderFS(shaders, "", sy) - pl.AddVarUsed(0, "TensorStrides") - pl = gpu.NewComputePipelineShaderFS(shaders, "", sy) - pl.AddVarUsed(0, "TensorStrides") - pl = gpu.NewComputePipelineShaderFS(shaders, "", sy) - pl.AddVarUsed(0, "TensorStrides") - pl = gpu.NewComputePipelineShaderFS(shaders, "", sy) - pl.AddVarUsed(0, "TensorStrides") - pl = gpu.NewComputePipelineShaderFS(shaders, "", sy) - pl.AddVarUsed(0, "TensorStrides") - pl = gpu.NewComputePipelineShaderFS(shaders, "", sy) - pl.AddVarUsed(0, "TensorStrides") - pl = gpu.NewComputePipelineShaderFS(shaders, "", sy) - pl.AddVarUsed(0, "TensorStrides") - pl = gpu.NewComputePipelineShaderFS(shaders, "", sy) - pl.AddVarUsed(0, "TensorStrides") - pl = gpu.NewComputePipelineShaderFS(shaders, "", sy) - pl.AddVarUsed(0, "TensorStrides") - pl = gpu.NewComputePipelineShaderFS(shaders, "", sy) - pl.AddVarUsed(0, "TensorStrides") - sy.Config() - } -} - -// GPURelease releases the GPU compute system resources. -// Call this at program exit. -func GPURelease() { - if GPUSystem != nil { - GPUSystem.Release() - GPUSystem = nil - } - - if !BorrowedGPU && ComputeGPU != nil { - ComputeGPU.Release() - - } - ComputeGPU = nil -} - -// RunCollisionBroad runs the CollisionBroad kernel with given number of elements, -// on either the CPU or GPU depending on the UseGPU variable. -// Can call multiple Run* kernels in a row, which are then all launched -// in the same command submission on the GPU, which is by far the most efficient. -// MUST call RunDone (with optional vars to sync) after all Run calls. -// Alternatively, a single-shot RunOneCollisionBroad call does Run and Done for a -// single run-and-sync case. -func RunCollisionBroad(n int) { - if UseGPU { - RunCollisionBroadGPU(n) - } else { - RunCollisionBroadCPU(n) - } -} - -// RunCollisionBroadGPU runs the CollisionBroad kernel on the GPU. See [RunCollisionBroad] for more info. -func RunCollisionBroadGPU(n int) { - sy := GPUSystem - pl := sy.ComputePipelines["CollisionBroad"] - ce, _ := sy.BeginComputePass() - pl.Dispatch1D(ce, n, 64) -} - -// RunCollisionBroadCPU runs the CollisionBroad kernel on the CPU. -func RunCollisionBroadCPU(n int) { - gpu.VectorizeFunc(0, n, CollisionBroad) -} - -// RunOneCollisionBroad runs the CollisionBroad kernel with given number of elements, -// on either the CPU or GPU depending on the UseGPU variable. -// This version then calls RunDone with the given variables to sync -// after the Run, for a single-shot Run-and-Done call. If multiple kernels -// can be run in sequence, it is much more efficient to do multiple Run* -// calls followed by a RunDone call. -func RunOneCollisionBroad(n int, syncVars ...GPUVars) { - if UseGPU { - RunCollisionBroadGPU(n) - RunDone(syncVars...) - } else { - RunCollisionBroadCPU(n) - } -} -// RunCollisionInit runs the CollisionInit kernel with given number of elements, -// on either the CPU or GPU depending on the UseGPU variable. -// Can call multiple Run* kernels in a row, which are then all launched -// in the same command submission on the GPU, which is by far the most efficient. -// MUST call RunDone (with optional vars to sync) after all Run calls. -// Alternatively, a single-shot RunOneCollisionInit call does Run and Done for a -// single run-and-sync case. -func RunCollisionInit(n int) { - if UseGPU { - RunCollisionInitGPU(n) - } else { - RunCollisionInitCPU(n) - } -} - -// RunCollisionInitGPU runs the CollisionInit kernel on the GPU. See [RunCollisionInit] for more info. -func RunCollisionInitGPU(n int) { - sy := GPUSystem - pl := sy.ComputePipelines["CollisionInit"] - ce, _ := sy.BeginComputePass() - pl.Dispatch1D(ce, n, 64) -} - -// RunCollisionInitCPU runs the CollisionInit kernel on the CPU. -func RunCollisionInitCPU(n int) { - gpu.VectorizeFunc(0, n, CollisionInit) -} - -// RunOneCollisionInit runs the CollisionInit kernel with given number of elements, -// on either the CPU or GPU depending on the UseGPU variable. -// This version then calls RunDone with the given variables to sync -// after the Run, for a single-shot Run-and-Done call. If multiple kernels -// can be run in sequence, it is much more efficient to do multiple Run* -// calls followed by a RunDone call. -func RunOneCollisionInit(n int, syncVars ...GPUVars) { - if UseGPU { - RunCollisionInitGPU(n) - RunDone(syncVars...) - } else { - RunCollisionInitCPU(n) - } -} -// RunCollisionNarrow runs the CollisionNarrow kernel with given number of elements, -// on either the CPU or GPU depending on the UseGPU variable. -// Can call multiple Run* kernels in a row, which are then all launched -// in the same command submission on the GPU, which is by far the most efficient. -// MUST call RunDone (with optional vars to sync) after all Run calls. -// Alternatively, a single-shot RunOneCollisionNarrow call does Run and Done for a -// single run-and-sync case. -func RunCollisionNarrow(n int) { - if UseGPU { - RunCollisionNarrowGPU(n) - } else { - RunCollisionNarrowCPU(n) - } -} - -// RunCollisionNarrowGPU runs the CollisionNarrow kernel on the GPU. See [RunCollisionNarrow] for more info. -func RunCollisionNarrowGPU(n int) { - sy := GPUSystem - pl := sy.ComputePipelines["CollisionNarrow"] - ce, _ := sy.BeginComputePass() - pl.Dispatch1D(ce, n, 64) -} - -// RunCollisionNarrowCPU runs the CollisionNarrow kernel on the CPU. -func RunCollisionNarrowCPU(n int) { - gpu.VectorizeFunc(0, n, CollisionNarrow) -} - -// RunOneCollisionNarrow runs the CollisionNarrow kernel with given number of elements, -// on either the CPU or GPU depending on the UseGPU variable. -// This version then calls RunDone with the given variables to sync -// after the Run, for a single-shot Run-and-Done call. If multiple kernels -// can be run in sequence, it is much more efficient to do multiple Run* -// calls followed by a RunDone call. -func RunOneCollisionNarrow(n int, syncVars ...GPUVars) { - if UseGPU { - RunCollisionNarrowGPU(n) - RunDone(syncVars...) - } else { - RunCollisionNarrowCPU(n) - } -} -// RunDeltasFromContacts runs the DeltasFromContacts kernel with given number of elements, -// on either the CPU or GPU depending on the UseGPU variable. -// Can call multiple Run* kernels in a row, which are then all launched -// in the same command submission on the GPU, which is by far the most efficient. -// MUST call RunDone (with optional vars to sync) after all Run calls. -// Alternatively, a single-shot RunOneDeltasFromContacts call does Run and Done for a -// single run-and-sync case. -func RunDeltasFromContacts(n int) { - if UseGPU { - RunDeltasFromContactsGPU(n) - } else { - RunDeltasFromContactsCPU(n) - } -} - -// RunDeltasFromContactsGPU runs the DeltasFromContacts kernel on the GPU. See [RunDeltasFromContacts] for more info. -func RunDeltasFromContactsGPU(n int) { - sy := GPUSystem - pl := sy.ComputePipelines["DeltasFromContacts"] - ce, _ := sy.BeginComputePass() - pl.Dispatch1D(ce, n, 64) -} - -// RunDeltasFromContactsCPU runs the DeltasFromContacts kernel on the CPU. -func RunDeltasFromContactsCPU(n int) { - gpu.VectorizeFunc(0, n, DeltasFromContacts) -} - -// RunOneDeltasFromContacts runs the DeltasFromContacts kernel with given number of elements, -// on either the CPU or GPU depending on the UseGPU variable. -// This version then calls RunDone with the given variables to sync -// after the Run, for a single-shot Run-and-Done call. If multiple kernels -// can be run in sequence, it is much more efficient to do multiple Run* -// calls followed by a RunDone call. -func RunOneDeltasFromContacts(n int, syncVars ...GPUVars) { - if UseGPU { - RunDeltasFromContactsGPU(n) - RunDone(syncVars...) - } else { - RunDeltasFromContactsCPU(n) - } -} -// RunDeltasFromJoints runs the DeltasFromJoints kernel with given number of elements, -// on either the CPU or GPU depending on the UseGPU variable. -// Can call multiple Run* kernels in a row, which are then all launched -// in the same command submission on the GPU, which is by far the most efficient. -// MUST call RunDone (with optional vars to sync) after all Run calls. -// Alternatively, a single-shot RunOneDeltasFromJoints call does Run and Done for a -// single run-and-sync case. -func RunDeltasFromJoints(n int) { - if UseGPU { - RunDeltasFromJointsGPU(n) - } else { - RunDeltasFromJointsCPU(n) - } -} - -// RunDeltasFromJointsGPU runs the DeltasFromJoints kernel on the GPU. See [RunDeltasFromJoints] for more info. -func RunDeltasFromJointsGPU(n int) { - sy := GPUSystem - pl := sy.ComputePipelines["DeltasFromJoints"] - ce, _ := sy.BeginComputePass() - pl.Dispatch1D(ce, n, 64) -} - -// RunDeltasFromJointsCPU runs the DeltasFromJoints kernel on the CPU. -func RunDeltasFromJointsCPU(n int) { - gpu.VectorizeFunc(0, n, DeltasFromJoints) -} - -// RunOneDeltasFromJoints runs the DeltasFromJoints kernel with given number of elements, -// on either the CPU or GPU depending on the UseGPU variable. -// This version then calls RunDone with the given variables to sync -// after the Run, for a single-shot Run-and-Done call. If multiple kernels -// can be run in sequence, it is much more efficient to do multiple Run* -// calls followed by a RunDone call. -func RunOneDeltasFromJoints(n int, syncVars ...GPUVars) { - if UseGPU { - RunDeltasFromJointsGPU(n) - RunDone(syncVars...) - } else { - RunDeltasFromJointsCPU(n) - } -} -// RunDynamicsCurToNext runs the DynamicsCurToNext kernel with given number of elements, -// on either the CPU or GPU depending on the UseGPU variable. -// Can call multiple Run* kernels in a row, which are then all launched -// in the same command submission on the GPU, which is by far the most efficient. -// MUST call RunDone (with optional vars to sync) after all Run calls. -// Alternatively, a single-shot RunOneDynamicsCurToNext call does Run and Done for a -// single run-and-sync case. -func RunDynamicsCurToNext(n int) { - if UseGPU { - RunDynamicsCurToNextGPU(n) - } else { - RunDynamicsCurToNextCPU(n) - } -} - -// RunDynamicsCurToNextGPU runs the DynamicsCurToNext kernel on the GPU. See [RunDynamicsCurToNext] for more info. -func RunDynamicsCurToNextGPU(n int) { - sy := GPUSystem - pl := sy.ComputePipelines["DynamicsCurToNext"] - ce, _ := sy.BeginComputePass() - pl.Dispatch1D(ce, n, 64) -} - -// RunDynamicsCurToNextCPU runs the DynamicsCurToNext kernel on the CPU. -func RunDynamicsCurToNextCPU(n int) { - gpu.VectorizeFunc(0, n, DynamicsCurToNext) -} - -// RunOneDynamicsCurToNext runs the DynamicsCurToNext kernel with given number of elements, -// on either the CPU or GPU depending on the UseGPU variable. -// This version then calls RunDone with the given variables to sync -// after the Run, for a single-shot Run-and-Done call. If multiple kernels -// can be run in sequence, it is much more efficient to do multiple Run* -// calls followed by a RunDone call. -func RunOneDynamicsCurToNext(n int, syncVars ...GPUVars) { - if UseGPU { - RunDynamicsCurToNextGPU(n) - RunDone(syncVars...) - } else { - RunDynamicsCurToNextCPU(n) - } -} -// RunForcesFromJoints runs the ForcesFromJoints kernel with given number of elements, -// on either the CPU or GPU depending on the UseGPU variable. -// Can call multiple Run* kernels in a row, which are then all launched -// in the same command submission on the GPU, which is by far the most efficient. -// MUST call RunDone (with optional vars to sync) after all Run calls. -// Alternatively, a single-shot RunOneForcesFromJoints call does Run and Done for a -// single run-and-sync case. -func RunForcesFromJoints(n int) { - if UseGPU { - RunForcesFromJointsGPU(n) - } else { - RunForcesFromJointsCPU(n) - } -} - -// RunForcesFromJointsGPU runs the ForcesFromJoints kernel on the GPU. See [RunForcesFromJoints] for more info. -func RunForcesFromJointsGPU(n int) { - sy := GPUSystem - pl := sy.ComputePipelines["ForcesFromJoints"] - ce, _ := sy.BeginComputePass() - pl.Dispatch1D(ce, n, 64) -} - -// RunForcesFromJointsCPU runs the ForcesFromJoints kernel on the CPU. -func RunForcesFromJointsCPU(n int) { - gpu.VectorizeFunc(0, n, ForcesFromJoints) -} - -// RunOneForcesFromJoints runs the ForcesFromJoints kernel with given number of elements, -// on either the CPU or GPU depending on the UseGPU variable. -// This version then calls RunDone with the given variables to sync -// after the Run, for a single-shot Run-and-Done call. If multiple kernels -// can be run in sequence, it is much more efficient to do multiple Run* -// calls followed by a RunDone call. -func RunOneForcesFromJoints(n int, syncVars ...GPUVars) { - if UseGPU { - RunForcesFromJointsGPU(n) - RunDone(syncVars...) - } else { - RunForcesFromJointsCPU(n) - } -} -// RunInitDynamics runs the InitDynamics kernel with given number of elements, -// on either the CPU or GPU depending on the UseGPU variable. -// Can call multiple Run* kernels in a row, which are then all launched -// in the same command submission on the GPU, which is by far the most efficient. -// MUST call RunDone (with optional vars to sync) after all Run calls. -// Alternatively, a single-shot RunOneInitDynamics call does Run and Done for a -// single run-and-sync case. -func RunInitDynamics(n int) { - if UseGPU { - RunInitDynamicsGPU(n) - } else { - RunInitDynamicsCPU(n) - } -} - -// RunInitDynamicsGPU runs the InitDynamics kernel on the GPU. See [RunInitDynamics] for more info. -func RunInitDynamicsGPU(n int) { - sy := GPUSystem - pl := sy.ComputePipelines["InitDynamics"] - ce, _ := sy.BeginComputePass() - pl.Dispatch1D(ce, n, 64) -} - -// RunInitDynamicsCPU runs the InitDynamics kernel on the CPU. -func RunInitDynamicsCPU(n int) { - gpu.VectorizeFunc(0, n, InitDynamics) -} - -// RunOneInitDynamics runs the InitDynamics kernel with given number of elements, -// on either the CPU or GPU depending on the UseGPU variable. -// This version then calls RunDone with the given variables to sync -// after the Run, for a single-shot Run-and-Done call. If multiple kernels -// can be run in sequence, it is much more efficient to do multiple Run* -// calls followed by a RunDone call. -func RunOneInitDynamics(n int, syncVars ...GPUVars) { - if UseGPU { - RunInitDynamicsGPU(n) - RunDone(syncVars...) - } else { - RunInitDynamicsCPU(n) - } -} -// RunStepBodyContacts runs the StepBodyContacts kernel with given number of elements, -// on either the CPU or GPU depending on the UseGPU variable. -// Can call multiple Run* kernels in a row, which are then all launched -// in the same command submission on the GPU, which is by far the most efficient. -// MUST call RunDone (with optional vars to sync) after all Run calls. -// Alternatively, a single-shot RunOneStepBodyContacts call does Run and Done for a -// single run-and-sync case. -func RunStepBodyContacts(n int) { - if UseGPU { - RunStepBodyContactsGPU(n) - } else { - RunStepBodyContactsCPU(n) - } -} - -// RunStepBodyContactsGPU runs the StepBodyContacts kernel on the GPU. See [RunStepBodyContacts] for more info. -func RunStepBodyContactsGPU(n int) { - sy := GPUSystem - pl := sy.ComputePipelines["StepBodyContacts"] - ce, _ := sy.BeginComputePass() - pl.Dispatch1D(ce, n, 64) -} - -// RunStepBodyContactsCPU runs the StepBodyContacts kernel on the CPU. -func RunStepBodyContactsCPU(n int) { - gpu.VectorizeFunc(0, n, StepBodyContacts) -} - -// RunOneStepBodyContacts runs the StepBodyContacts kernel with given number of elements, -// on either the CPU or GPU depending on the UseGPU variable. -// This version then calls RunDone with the given variables to sync -// after the Run, for a single-shot Run-and-Done call. If multiple kernels -// can be run in sequence, it is much more efficient to do multiple Run* -// calls followed by a RunDone call. -func RunOneStepBodyContacts(n int, syncVars ...GPUVars) { - if UseGPU { - RunStepBodyContactsGPU(n) - RunDone(syncVars...) - } else { - RunStepBodyContactsCPU(n) - } -} -// RunStepBodyDeltas runs the StepBodyDeltas kernel with given number of elements, -// on either the CPU or GPU depending on the UseGPU variable. -// Can call multiple Run* kernels in a row, which are then all launched -// in the same command submission on the GPU, which is by far the most efficient. -// MUST call RunDone (with optional vars to sync) after all Run calls. -// Alternatively, a single-shot RunOneStepBodyDeltas call does Run and Done for a -// single run-and-sync case. -func RunStepBodyDeltas(n int) { - if UseGPU { - RunStepBodyDeltasGPU(n) - } else { - RunStepBodyDeltasCPU(n) - } -} - -// RunStepBodyDeltasGPU runs the StepBodyDeltas kernel on the GPU. See [RunStepBodyDeltas] for more info. -func RunStepBodyDeltasGPU(n int) { - sy := GPUSystem - pl := sy.ComputePipelines["StepBodyDeltas"] - ce, _ := sy.BeginComputePass() - pl.Dispatch1D(ce, n, 64) -} - -// RunStepBodyDeltasCPU runs the StepBodyDeltas kernel on the CPU. -func RunStepBodyDeltasCPU(n int) { - gpu.VectorizeFunc(0, n, StepBodyDeltas) -} - -// RunOneStepBodyDeltas runs the StepBodyDeltas kernel with given number of elements, -// on either the CPU or GPU depending on the UseGPU variable. -// This version then calls RunDone with the given variables to sync -// after the Run, for a single-shot Run-and-Done call. If multiple kernels -// can be run in sequence, it is much more efficient to do multiple Run* -// calls followed by a RunDone call. -func RunOneStepBodyDeltas(n int, syncVars ...GPUVars) { - if UseGPU { - RunStepBodyDeltasGPU(n) - RunDone(syncVars...) - } else { - RunStepBodyDeltasCPU(n) - } -} -// RunStepIntegrateBodies runs the StepIntegrateBodies kernel with given number of elements, -// on either the CPU or GPU depending on the UseGPU variable. -// Can call multiple Run* kernels in a row, which are then all launched -// in the same command submission on the GPU, which is by far the most efficient. -// MUST call RunDone (with optional vars to sync) after all Run calls. -// Alternatively, a single-shot RunOneStepIntegrateBodies call does Run and Done for a -// single run-and-sync case. -func RunStepIntegrateBodies(n int) { - if UseGPU { - RunStepIntegrateBodiesGPU(n) - } else { - RunStepIntegrateBodiesCPU(n) - } -} - -// RunStepIntegrateBodiesGPU runs the StepIntegrateBodies kernel on the GPU. See [RunStepIntegrateBodies] for more info. -func RunStepIntegrateBodiesGPU(n int) { - sy := GPUSystem - pl := sy.ComputePipelines["StepIntegrateBodies"] - ce, _ := sy.BeginComputePass() - pl.Dispatch1D(ce, n, 64) -} - -// RunStepIntegrateBodiesCPU runs the StepIntegrateBodies kernel on the CPU. -func RunStepIntegrateBodiesCPU(n int) { - gpu.VectorizeFunc(0, n, StepIntegrateBodies) -} - -// RunOneStepIntegrateBodies runs the StepIntegrateBodies kernel with given number of elements, -// on either the CPU or GPU depending on the UseGPU variable. -// This version then calls RunDone with the given variables to sync -// after the Run, for a single-shot Run-and-Done call. If multiple kernels -// can be run in sequence, it is much more efficient to do multiple Run* -// calls followed by a RunDone call. -func RunOneStepIntegrateBodies(n int, syncVars ...GPUVars) { - if UseGPU { - RunStepIntegrateBodiesGPU(n) - RunDone(syncVars...) - } else { - RunStepIntegrateBodiesCPU(n) - } -} -// RunStepJointForces runs the StepJointForces kernel with given number of elements, -// on either the CPU or GPU depending on the UseGPU variable. -// Can call multiple Run* kernels in a row, which are then all launched -// in the same command submission on the GPU, which is by far the most efficient. -// MUST call RunDone (with optional vars to sync) after all Run calls. -// Alternatively, a single-shot RunOneStepJointForces call does Run and Done for a -// single run-and-sync case. -func RunStepJointForces(n int) { - if UseGPU { - RunStepJointForcesGPU(n) - } else { - RunStepJointForcesCPU(n) - } -} - -// RunStepJointForcesGPU runs the StepJointForces kernel on the GPU. See [RunStepJointForces] for more info. -func RunStepJointForcesGPU(n int) { - sy := GPUSystem - pl := sy.ComputePipelines["StepJointForces"] - ce, _ := sy.BeginComputePass() - pl.Dispatch1D(ce, n, 64) -} - -// RunStepJointForcesCPU runs the StepJointForces kernel on the CPU. -func RunStepJointForcesCPU(n int) { - gpu.VectorizeFunc(0, n, StepJointForces) -} - -// RunOneStepJointForces runs the StepJointForces kernel with given number of elements, -// on either the CPU or GPU depending on the UseGPU variable. -// This version then calls RunDone with the given variables to sync -// after the Run, for a single-shot Run-and-Done call. If multiple kernels -// can be run in sequence, it is much more efficient to do multiple Run* -// calls followed by a RunDone call. -func RunOneStepJointForces(n int, syncVars ...GPUVars) { - if UseGPU { - RunStepJointForcesGPU(n) - RunDone(syncVars...) - } else { - RunStepJointForcesCPU(n) - } -} -// RunStepSolveJoints runs the StepSolveJoints kernel with given number of elements, -// on either the CPU or GPU depending on the UseGPU variable. -// Can call multiple Run* kernels in a row, which are then all launched -// in the same command submission on the GPU, which is by far the most efficient. -// MUST call RunDone (with optional vars to sync) after all Run calls. -// Alternatively, a single-shot RunOneStepSolveJoints call does Run and Done for a -// single run-and-sync case. -func RunStepSolveJoints(n int) { - if UseGPU { - RunStepSolveJointsGPU(n) - } else { - RunStepSolveJointsCPU(n) - } -} - -// RunStepSolveJointsGPU runs the StepSolveJoints kernel on the GPU. See [RunStepSolveJoints] for more info. -func RunStepSolveJointsGPU(n int) { - sy := GPUSystem - pl := sy.ComputePipelines["StepSolveJoints"] - ce, _ := sy.BeginComputePass() - pl.Dispatch1D(ce, n, 64) -} - -// RunStepSolveJointsCPU runs the StepSolveJoints kernel on the CPU. -func RunStepSolveJointsCPU(n int) { - gpu.VectorizeFunc(0, n, StepSolveJoints) -} - -// RunOneStepSolveJoints runs the StepSolveJoints kernel with given number of elements, -// on either the CPU or GPU depending on the UseGPU variable. -// This version then calls RunDone with the given variables to sync -// after the Run, for a single-shot Run-and-Done call. If multiple kernels -// can be run in sequence, it is much more efficient to do multiple Run* -// calls followed by a RunDone call. -func RunOneStepSolveJoints(n int, syncVars ...GPUVars) { - if UseGPU { - RunStepSolveJointsGPU(n) - RunDone(syncVars...) - } else { - RunStepSolveJointsCPU(n) - } -} -// RunDone must be called after Run* calls to start compute kernels. -// This actually submits the kernel jobs to the GPU, and adds commands -// to synchronize the given variables back from the GPU to the CPU. -// After this function completes, the GPU results will be available in -// the specified variables. -func RunDone(syncVars ...GPUVars) { - if !UseGPU { - return - } - sy := GPUSystem - sy.ComputeEncoder.End() - ReadFromGPU(syncVars...) - sy.EndComputePass() - SyncFromGPU(syncVars...) -} - -// ToGPU copies given variables to the GPU for the system. -func ToGPU(vars ...GPUVars) { - if !UseGPU { - return - } - sy := GPUSystem - syVars := sy.Vars() - for _, vr := range vars { - switch vr { - case ParamsVar: - v, _ := syVars.ValueByIndex(0, "Params", 0) - gpu.SetValueFrom(v, Params) - case BodiesVar: - v, _ := syVars.ValueByIndex(1, "Bodies", 0) - gpu.SetValueFrom(v, Bodies.Values) - case JointsVar: - v, _ := syVars.ValueByIndex(1, "Joints", 0) - gpu.SetValueFrom(v, Joints.Values) - case JointDoFsVar: - v, _ := syVars.ValueByIndex(1, "JointDoFs", 0) - gpu.SetValueFrom(v, JointDoFs.Values) - case BodyJointsVar: - v, _ := syVars.ValueByIndex(1, "BodyJoints", 0) - gpu.SetValueFrom(v, BodyJoints.Values) - case BodyCollidePairsVar: - v, _ := syVars.ValueByIndex(1, "BodyCollidePairs", 0) - gpu.SetValueFrom(v, BodyCollidePairs.Values) - case DynamicsVar: - v, _ := syVars.ValueByIndex(2, "Dynamics", 0) - gpu.SetValueFrom(v, Dynamics.Values) - case BroadContactsNVar: - v, _ := syVars.ValueByIndex(2, "BroadContactsN", 0) - gpu.SetValueFrom(v, BroadContactsN.Values) - case BroadContactsVar: - v, _ := syVars.ValueByIndex(2, "BroadContacts", 0) - gpu.SetValueFrom(v, BroadContacts.Values) - case ContactsNVar: - v, _ := syVars.ValueByIndex(2, "ContactsN", 0) - gpu.SetValueFrom(v, ContactsN.Values) - case ContactsVar: - v, _ := syVars.ValueByIndex(2, "Contacts", 0) - gpu.SetValueFrom(v, Contacts.Values) - case JointControlsVar: - v, _ := syVars.ValueByIndex(3, "JointControls", 0) - gpu.SetValueFrom(v, JointControls.Values) - } - } -} -// RunGPUSync can be called to synchronize data between CPU and GPU. -// Any prior ToGPU* calls will execute to send data to the GPU, -// and any subsequent RunDone* calls will copy data back from the GPU. -func RunGPUSync() { - if !UseGPU { - return - } - sy := GPUSystem - sy.BeginComputePass() -} - -// ToGPUTensorStrides gets tensor strides and starts copying to the GPU. -func ToGPUTensorStrides() { - if !UseGPU { - return - } - sy := GPUSystem - syVars := sy.Vars() - TensorStrides.SetShapeSizes(110) - TensorStrides.SetInt1D(Bodies.Shape().Strides[0], 0) - TensorStrides.SetInt1D(Bodies.Shape().Strides[1], 1) - TensorStrides.SetInt1D(Joints.Shape().Strides[0], 10) - TensorStrides.SetInt1D(Joints.Shape().Strides[1], 11) - TensorStrides.SetInt1D(JointDoFs.Shape().Strides[0], 20) - TensorStrides.SetInt1D(JointDoFs.Shape().Strides[1], 21) - TensorStrides.SetInt1D(BodyJoints.Shape().Strides[0], 30) - TensorStrides.SetInt1D(BodyJoints.Shape().Strides[1], 31) - TensorStrides.SetInt1D(BodyJoints.Shape().Strides[2], 32) - TensorStrides.SetInt1D(BodyCollidePairs.Shape().Strides[0], 40) - TensorStrides.SetInt1D(BodyCollidePairs.Shape().Strides[1], 41) - TensorStrides.SetInt1D(Dynamics.Shape().Strides[0], 50) - TensorStrides.SetInt1D(Dynamics.Shape().Strides[1], 51) - TensorStrides.SetInt1D(Dynamics.Shape().Strides[2], 52) - TensorStrides.SetInt1D(BroadContactsN.Shape().Strides[0], 60) - TensorStrides.SetInt1D(BroadContacts.Shape().Strides[0], 70) - TensorStrides.SetInt1D(BroadContacts.Shape().Strides[1], 71) - TensorStrides.SetInt1D(ContactsN.Shape().Strides[0], 80) - TensorStrides.SetInt1D(Contacts.Shape().Strides[0], 90) - TensorStrides.SetInt1D(Contacts.Shape().Strides[1], 91) - TensorStrides.SetInt1D(JointControls.Shape().Strides[0], 100) - TensorStrides.SetInt1D(JointControls.Shape().Strides[1], 101) - v, _ := syVars.ValueByIndex(0, "TensorStrides", 0) - gpu.SetValueFrom(v, TensorStrides.Values) -} - -// ReadFromGPU starts the process of copying vars to the GPU. -func ReadFromGPU(vars ...GPUVars) { - sy := GPUSystem - syVars := sy.Vars() - for _, vr := range vars { - switch vr { - case ParamsVar: - v, _ := syVars.ValueByIndex(0, "Params", 0) - v.GPUToRead(sy.CommandEncoder) - case BodiesVar: - v, _ := syVars.ValueByIndex(1, "Bodies", 0) - v.GPUToRead(sy.CommandEncoder) - case JointsVar: - v, _ := syVars.ValueByIndex(1, "Joints", 0) - v.GPUToRead(sy.CommandEncoder) - case JointDoFsVar: - v, _ := syVars.ValueByIndex(1, "JointDoFs", 0) - v.GPUToRead(sy.CommandEncoder) - case BodyJointsVar: - v, _ := syVars.ValueByIndex(1, "BodyJoints", 0) - v.GPUToRead(sy.CommandEncoder) - case BodyCollidePairsVar: - v, _ := syVars.ValueByIndex(1, "BodyCollidePairs", 0) - v.GPUToRead(sy.CommandEncoder) - case DynamicsVar: - v, _ := syVars.ValueByIndex(2, "Dynamics", 0) - v.GPUToRead(sy.CommandEncoder) - case BroadContactsNVar: - v, _ := syVars.ValueByIndex(2, "BroadContactsN", 0) - v.GPUToRead(sy.CommandEncoder) - case BroadContactsVar: - v, _ := syVars.ValueByIndex(2, "BroadContacts", 0) - v.GPUToRead(sy.CommandEncoder) - case ContactsNVar: - v, _ := syVars.ValueByIndex(2, "ContactsN", 0) - v.GPUToRead(sy.CommandEncoder) - case ContactsVar: - v, _ := syVars.ValueByIndex(2, "Contacts", 0) - v.GPUToRead(sy.CommandEncoder) - case JointControlsVar: - v, _ := syVars.ValueByIndex(3, "JointControls", 0) - v.GPUToRead(sy.CommandEncoder) - } - } -} - -// SyncFromGPU synchronizes vars from the GPU to the actual variable. -func SyncFromGPU(vars ...GPUVars) { - sy := GPUSystem - syVars := sy.Vars() - for _, vr := range vars { - switch vr { - case ParamsVar: - v, _ := syVars.ValueByIndex(0, "Params", 0) - v.ReadSync() - gpu.ReadToBytes(v, Params) - case BodiesVar: - v, _ := syVars.ValueByIndex(1, "Bodies", 0) - v.ReadSync() - gpu.ReadToBytes(v, Bodies.Values) - case JointsVar: - v, _ := syVars.ValueByIndex(1, "Joints", 0) - v.ReadSync() - gpu.ReadToBytes(v, Joints.Values) - case JointDoFsVar: - v, _ := syVars.ValueByIndex(1, "JointDoFs", 0) - v.ReadSync() - gpu.ReadToBytes(v, JointDoFs.Values) - case BodyJointsVar: - v, _ := syVars.ValueByIndex(1, "BodyJoints", 0) - v.ReadSync() - gpu.ReadToBytes(v, BodyJoints.Values) - case BodyCollidePairsVar: - v, _ := syVars.ValueByIndex(1, "BodyCollidePairs", 0) - v.ReadSync() - gpu.ReadToBytes(v, BodyCollidePairs.Values) - case DynamicsVar: - v, _ := syVars.ValueByIndex(2, "Dynamics", 0) - v.ReadSync() - gpu.ReadToBytes(v, Dynamics.Values) - case BroadContactsNVar: - v, _ := syVars.ValueByIndex(2, "BroadContactsN", 0) - v.ReadSync() - gpu.ReadToBytes(v, BroadContactsN.Values) - case BroadContactsVar: - v, _ := syVars.ValueByIndex(2, "BroadContacts", 0) - v.ReadSync() - gpu.ReadToBytes(v, BroadContacts.Values) - case ContactsNVar: - v, _ := syVars.ValueByIndex(2, "ContactsN", 0) - v.ReadSync() - gpu.ReadToBytes(v, ContactsN.Values) - case ContactsVar: - v, _ := syVars.ValueByIndex(2, "Contacts", 0) - v.ReadSync() - gpu.ReadToBytes(v, Contacts.Values) - case JointControlsVar: - v, _ := syVars.ValueByIndex(3, "JointControls", 0) - v.ReadSync() - gpu.ReadToBytes(v, JointControls.Values) - } - } -} - -// GetParams returns a pointer to the given global variable: -// [Params] []PhysParams at given index. This directly processed in the GPU code, -// so this function call is an equivalent for the CPU. -func GetParams(idx uint32) *PhysParams { - return &Params[idx] -} From 08ff799718a8d5b25aaa2d8b575a117ffd536b9b Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Mon, 22 Dec 2025 23:42:37 +0100 Subject: [PATCH 44/97] physics: balls in a box -- too much fun! --- physics/contact.go | 3 +- physics/contact.goal | 5 +-- physics/examples/balls/balls.go | 49 +++++++++++++++++++-------- physics/shaders/StepBodyContacts.wgsl | 5 +-- physics/step.go | 24 ++++++++++--- physics/step.goal | 24 ++++++++++--- 6 files changed, 81 insertions(+), 29 deletions(-) diff --git a/physics/contact.go b/physics/contact.go index 8cc541f2..6e399633 100644 --- a/physics/contact.go +++ b/physics/contact.go @@ -478,9 +478,10 @@ func StepBodyContacts(i uint32) { //gosl:kernel thick := thickA + thickB nnorm := ContactNorm(ci) norm := slmath.Negate3(nnorm) + // margin := params.ContactMargin d := slmath.Dot3(norm, ctBw.Sub(ctAw)) - thick - if d >= 0.0 { // now separated + if d >= 0.0 { // todo: should this be margin or not? Contacts.Set(0.0, int(ci), int(ContactWeight)) z := math32.Vec3(0, 0, 0) SetContactADelta(ci, z) diff --git a/physics/contact.goal b/physics/contact.goal index 20346838..79fd4bfa 100644 --- a/physics/contact.goal +++ b/physics/contact.goal @@ -452,7 +452,7 @@ func StepBodyContacts(i uint32) { //gosl:kernel if ci >= cmax { return } - + biA := GetContactA(ci) biB := GetContactB(ci) diA := GetBodyDynamic(biA) @@ -476,9 +476,10 @@ func StepBodyContacts(i uint32) { //gosl:kernel thick := thickA + thickB nnorm := ContactNorm(ci) norm := slmath.Negate3(nnorm) + // margin := params.ContactMargin d := slmath.Dot3(norm, ctBw.Sub(ctAw)) - thick - if d >= 0.0 { // now separated + if d >= 0.0 { // todo: should this be margin or not? Contacts[ci, ContactWeight] = 0.0 z := math32.Vec3(0,0,0) SetContactADelta(ci, z) diff --git a/physics/examples/balls/balls.go b/physics/examples/balls/balls.go index 4e1e6d70..f084640e 100644 --- a/physics/examples/balls/balls.go +++ b/physics/examples/balls/balls.go @@ -52,14 +52,31 @@ func main() { split.SetSplits(0.2, 0.8) rot := math32.NewQuat(0, 0, 0, 1) - wr.NewBody(wl, "floor", physics.Plane, "#D0D0D040", math32.Vec3(0, 0, 0), + wr.NewBody(wl, "floor", physics.Plane, "#D0D0D080", math32.Vec3(0, 0, 0), math32.Vec3(0, 0, 0), rot) - nballs := 10000 - size := float32(0.1) - bounce := float32(0.5) - box := 2 * size * math32.Sqrt(float32(nballs)) + width := float32(50) + depth := float32(50) height := float32(20) + thick := float32(.1) + hw := width / 2 + hd := depth / 2 + hh := height / 2 + ht := thick / 2 + wr.NewBody(wl, "back-wall", physics.Box, "#0000FFA0", math32.Vec3(hw, hh, ht), + math32.Vec3(0, hh, -hd), rot) + wr.NewBody(wl, "left-wall", physics.Box, "#FF0000A0", math32.Vec3(ht, hh, hd), + math32.Vec3(-hw, hh, 0), rot) + wr.NewBody(wl, "right-wall", physics.Box, "#00FF00A0", math32.Vec3(ht, hh, hd), + math32.Vec3(hw, hh, 0), rot) + wr.NewBody(wl, "front-wall", physics.Box, "#FFFF00A0", math32.Vec3(hw, hh, ht), + math32.Vec3(0, hh, hd), rot) + + nballs := 1000 + size := float32(0.2) + bounce := float32(0.5) + box := width * .9 + // height := float32(20) for i := range nballs { _ = i ht := rand.Float32() * height @@ -70,16 +87,17 @@ func main() { math32.Vec3(x, size+ht, z), rot) // todo: helper methods on view to set this stuff: physics.Bodies.Set(bounce, int(b1.Index), int(physics.BodyBounce)) - physics.SetBodyGroup(b1.Index, int32(i)) // no self collisions + // physics.SetBodyGroup(b1.Index, int32(i)) // no self collisions } wr.Init(wl) params := physics.GetParams(0) - params.Dt = 0.001 + params.Dt = 0.0001 // leaks balls > 0.0005 + subSteps := 100 // major speedup by inner-stepping // params.Gravity.Y = 0 - params.ContactRelax = 0.1 - params.Restitution.SetBool(false) - fmt.Println(params.ContactRelax) + params.ContactRelax = 0.1 // 0.1 seems most physical -- 0.2 getting a bit more ke? + params.Restitution.SetBool(false) // not working! + params.ContactMargin = 0.1 // 0.1 better than .01 -- leaks a few wl.Config() wr.Update() @@ -88,12 +106,12 @@ func main() { sc.Camera.LookAt(math32.Vec3(0, 5, 0), math32.Vec3(0, 1, 0)) sc.SaveCamera("3") - sc.Camera.Pose.Pos = math32.Vec3(0, 20, 30) - sc.Camera.LookAt(math32.Vec3(0, 5, 0), math32.Vec3(0, 1, 0)) - sc.SaveCamera("2") - sc.Camera.Pose.Pos = math32.Vec3(-1.33, 2.24, 3.55) sc.Camera.LookAt(math32.Vec3(0, .5, 0), math32.Vec3(0, 1, 0)) + sc.SaveCamera("2") + + sc.Camera.Pose.Pos = math32.Vec3(0, 80, 75) + sc.Camera.LookAt(math32.Vec3(0, 5, 0), math32.Vec3(0, 1, 0)) sc.SaveCamera("1") sc.SaveCamera("default") @@ -112,6 +130,9 @@ func main() { go func() { isRunning = true for range n { + for range subSteps { + wl.StepGet() // don't get anything + } wl.Step() wr.Update() if se.IsVisible() { diff --git a/physics/shaders/StepBodyContacts.wgsl b/physics/shaders/StepBodyContacts.wgsl index 2c999fdf..453dde89 100644 --- a/physics/shaders/StepBodyContacts.wgsl +++ b/physics/shaders/StepBodyContacts.wgsl @@ -187,8 +187,9 @@ let params = Params[0];; var ci = i32(i); ; var thick = thickA + thickB; ; var nnorm = ContactNorm(ci); ; var norm = Negate3(nnorm); -; var d = Dot3(norm, ctBw-(ctAw)) - thick; -; if (d >= 0.0) { // now separated +; +var d = Dot3(norm, ctBw-(ctAw)) - thick; +; if (d >= 0.0) { // todo: should this be margin or not? Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(ci), u32(ContactWeight))] = 0.0; var z = vec3(0, 0, 0); SetContactADelta(ci, z); diff --git a/physics/step.go b/physics/step.go index 319825aa..2dc29c7e 100644 --- a/physics/step.go +++ b/physics/step.go @@ -48,7 +48,15 @@ func OneIfNonzero(f float32) float32 { //gosl:end +// Step runs one physics step and gets the dynamics vars back +// from the GPU. func (wl *World) Step() { + wl.StepGet(DynamicsVar) +} + +// StepGet runs one physics step and gets the given vars back +// from the GPU. +func (wl *World) StepGet(vars ...GPUVars) { params := GetParams(0) if params.Cur == 0 { params.Cur = 1 @@ -66,7 +74,7 @@ func (wl *World) Step() { wl.StepSolveJoints() wl.StepBodyContacts() } - RunDone(DynamicsVar) + RunDone(vars...) } func (wl *World) StepCollision() { @@ -75,7 +83,8 @@ func (wl *World) StepCollision() { RunCollisionBroad(int(params.BodyCollidePairsN)) // note: time getting BroadContactsN back down and using that vs. running full RunCollisionNarrow(int(params.ContactsMax)) - RunDone(ContactsNVar) // we do multiple iterations so useful to have this + // note: too slow to get this back, so just using ContactsMax always. + // RunDone(ContactsNVar) // we do multiple iterations so useful to have this // fmt.Println("contacts:", ContactsN.Value(0), "max:", params.ContactsMax) } @@ -98,9 +107,14 @@ func (wl *World) StepSolveJoints() { func (wl *World) StepBodyContacts() { params := GetParams(0) - cmax := int(ContactsN.Values[0]) - if cmax > 0 { - RunStepBodyContacts(cmax) + if !wl.GPU { + cmax := int(ContactsN.Values[0]) + if cmax > 0 { + RunStepBodyContacts(cmax) + RunDeltasFromContacts(int(params.DynamicsN)) + } + } else { + RunStepBodyContacts(int(params.ContactsMax)) // just do max and let the routines bail RunDeltasFromContacts(int(params.DynamicsN)) } } diff --git a/physics/step.goal b/physics/step.goal index 7257363e..74673a3f 100644 --- a/physics/step.goal +++ b/physics/step.goal @@ -46,7 +46,15 @@ func OneIfNonzero(f float32) float32 { //gosl:end +// Step runs one physics step and gets the dynamics vars back +// from the GPU. func (wl *World) Step() { + wl.StepGet(DynamicsVar) +} + +// StepGet runs one physics step and gets the given vars back +// from the GPU. +func (wl *World) StepGet(vars ...GPUVars) { params := GetParams(0) if params.Cur == 0 { params.Cur = 1 @@ -64,7 +72,7 @@ func (wl *World) Step() { wl.StepSolveJoints() wl.StepBodyContacts() } - RunDone(DynamicsVar) + RunDone(vars...) } func (wl *World) StepCollision() { @@ -73,7 +81,8 @@ func (wl *World) StepCollision() { RunCollisionBroad(int(params.BodyCollidePairsN)) // note: time getting BroadContactsN back down and using that vs. running full RunCollisionNarrow(int(params.ContactsMax)) - RunDone(ContactsNVar) // we do multiple iterations so useful to have this + // note: too slow to get this back, so just using ContactsMax always. + // RunDone(ContactsNVar) // we do multiple iterations so useful to have this // fmt.Println("contacts:", ContactsN.Value(0), "max:", params.ContactsMax) } @@ -96,9 +105,14 @@ func (wl *World) StepSolveJoints() { func (wl *World) StepBodyContacts() { params := GetParams(0) - cmax := int(ContactsN.Values[0]) - if cmax > 0 { - RunStepBodyContacts(cmax) + if !wl.GPU { + cmax := int(ContactsN.Values[0]) + if cmax > 0 { + RunStepBodyContacts(cmax) + RunDeltasFromContacts(int(params.DynamicsN)) + } + } else { + RunStepBodyContacts(int(params.ContactsMax)) // just do max and let the routines bail RunDeltasFromContacts(int(params.DynamicsN)) } } From bbe56d5084a81c06b8b85ca096032286de8589fe Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Tue, 23 Dec 2025 06:38:55 +0100 Subject: [PATCH 45/97] physics: much improved balls interface with all params avail to tweak; substeps as part of main code, iterations = 1, other param updates --- physics/body.go | 22 ++ physics/body.goal | 22 ++ physics/contact.go | 15 +- physics/contact.goal | 15 +- physics/examples/balls/balls.go | 186 +++++++++----- physics/gosl.go | 94 +++---- physics/params.go | 66 +++-- physics/shaders/CollisionBroad.wgsl | 17 +- physics/shaders/CollisionInit.wgsl | 309 ----------------------- physics/shaders/CollisionNarrow.wgsl | 17 +- physics/shaders/DeltasFromContacts.wgsl | 17 +- physics/shaders/DeltasFromJoints.wgsl | 17 +- physics/shaders/DynamicsCurToNext.wgsl | 17 +- physics/shaders/ForcesFromJoints.wgsl | 17 +- physics/shaders/InitDynamics.wgsl | 17 +- physics/shaders/StepBodyContacts.wgsl | 16 +- physics/shaders/StepBodyDeltas.wgsl | 17 +- physics/shaders/StepIntegrateBodies.wgsl | 17 +- physics/shaders/StepJointForces.wgsl | 17 +- physics/shaders/StepSolveJoints.wgsl | 17 +- physics/step.go | 43 +++- physics/step.goal | 43 +++- physics/typegen.go | 2 +- physics/vars.go | 1 - physics/world.go | 8 +- physics/world.goal | 8 +- physics/world/view.go | 25 +- physics/world/world.go | 8 + 28 files changed, 482 insertions(+), 588 deletions(-) delete mode 100644 physics/shaders/CollisionInit.wgsl diff --git a/physics/body.go b/physics/body.go index adbee44a..8f5be162 100644 --- a/physics/body.go +++ b/physics/body.go @@ -235,4 +235,26 @@ func SetBodyInvInertia(idx int32, invInertia math32.Matrix3) { } } +// SetBodyBounce specifies the COR or coefficient of restitution (0..1), +// which determines how elastic the collision is, +// i.e., final velocity / initial velocity. +func SetBodyBounce(idx int32, val float32) { + Bodies.Set(val, int(idx), int(BodyBounce)) +} + +// SetBodyFriction is the standard coefficient for linear friction (mu). +func SetBodyFriction(idx int32, val float32) { + Bodies.Set(val, int(idx), int(BodyFriction)) +} + +// SetBodyFrictionTortion is resistance to spinning at the contact point. +func SetBodyFrictionTortion(idx int32, val float32) { + Bodies.Set(val, int(idx), int(BodyFrictionTortion)) +} + +// SetBodyFrictionRolling is resistance to rolling motion at contact. +func SetBodyFrictionRolling(idx int32, val float32) { + Bodies.Set(val, int(idx), int(BodyFrictionRolling)) +} + //gosl:end diff --git a/physics/body.goal b/physics/body.goal index 39b92fa5..fa83a0f0 100644 --- a/physics/body.goal +++ b/physics/body.goal @@ -233,4 +233,26 @@ func SetBodyInvInertia(idx int32, invInertia math32.Matrix3) { } } +// SetBodyBounce specifies the COR or coefficient of restitution (0..1), +// which determines how elastic the collision is, +// i.e., final velocity / initial velocity. +func SetBodyBounce(idx int32, val float32) { + Bodies[idx, BodyBounce] = val +} + +// SetBodyFriction is the standard coefficient for linear friction (mu). +func SetBodyFriction(idx int32, val float32) { + Bodies[idx, BodyFriction] = val +} + +// SetBodyFrictionTortion is resistance to spinning at the contact point. +func SetBodyFrictionTortion(idx int32, val float32) { + Bodies[idx, BodyFrictionTortion] = val +} + +// SetBodyFrictionRolling is resistance to rolling motion at contact. +func SetBodyFrictionRolling(idx int32, val float32) { + Bodies[idx, BodyFrictionRolling] = val +} + //gosl:end diff --git a/physics/contact.go b/physics/contact.go index 6e399633..80074037 100644 --- a/physics/contact.go +++ b/physics/contact.go @@ -7,7 +7,7 @@ package physics import ( - "fmt" + // "fmt" "math" "sync/atomic" @@ -248,15 +248,6 @@ func GroupsCollide(ga, gb int32) bool { return false } -// CollisionInit performs initialization at start of collision (i=1) -func CollisionInit(i uint32) { //gosl:kernel - if i > 0 { - return - } - BroadContactsN.Values[0] = 0 - ContactsN.Values[0] = 0 -} - // newton: geometry/kernels.py: broadphase_collision_pairs // CollisionBroad performs broad-phase collision detection, generating Contacts. @@ -786,7 +777,7 @@ func (wl *World) ConfigBodyCollidePairs() { pt.SetShapeSizes(np, 2) wl.BodyCollidePairs = pt BodyCollidePairs = pt - fmt.Println("body pairs over alloc", nalc-np, "total:", np) + // fmt.Println("body pairs over alloc", nalc - np, "total:", np) } // newton: geometry/kernels.py: count_contact_points @@ -818,7 +809,7 @@ func (wl *World) SetMaxContacts() { // todo: this is a massive over-estimate, b/c there is no way everyone could be // colliding at once. Except.. if it is a very small model. if params.BodyCollidePairsN > 1000 { - n = n / 2 // todo: could do more of this as N gets larger + n = int32(math32.Sqrt(float32(n))) } params.ContactsMax = n wl.BroadContacts.SetShapeSizes(int(n), int(BroadContactVarsN)) diff --git a/physics/contact.goal b/physics/contact.goal index 79fd4bfa..0852e0bc 100644 --- a/physics/contact.goal +++ b/physics/contact.goal @@ -5,7 +5,7 @@ package physics import ( - "fmt" + // "fmt" "math" "sync/atomic" @@ -246,15 +246,6 @@ func GroupsCollide(ga, gb int32) bool { return false } -// CollisionInit performs initialization at start of collision (i=1) -func CollisionInit(i uint32) { //gosl:kernel - if i > 0 { - return - } - BroadContactsN.Values[0] = 0 - ContactsN.Values[0] = 0 -} - // newton: geometry/kernels.py: broadphase_collision_pairs // CollisionBroad performs broad-phase collision detection, generating Contacts. @@ -785,7 +776,7 @@ func (wl *World) ConfigBodyCollidePairs() { pt.SetShapeSizes(np, 2) wl.BodyCollidePairs = pt BodyCollidePairs = pt - fmt.Println("body pairs over alloc", nalc - np, "total:", np) + // fmt.Println("body pairs over alloc", nalc - np, "total:", np) } // newton: geometry/kernels.py: count_contact_points @@ -817,7 +808,7 @@ func (wl *World) SetMaxContacts() { // todo: this is a massive over-estimate, b/c there is no way everyone could be // colliding at once. Except.. if it is a very small model. if params.BodyCollidePairsN > 1000 { - n = n/2 // todo: could do more of this as N gets larger + n = int32(math32.Sqrt(float32(n))) } params.ContactsMax = n wl.BroadContacts.SetShapeSizes(int(n), int(BroadContactVarsN)) diff --git a/physics/examples/balls/balls.go b/physics/examples/balls/balls.go index f084640e..e612e3e1 100644 --- a/physics/examples/balls/balls.go +++ b/physics/examples/balls/balls.go @@ -4,7 +4,7 @@ package main -//go:generate core generate +//go:generate core generate -add-types import ( "fmt" @@ -20,18 +20,71 @@ import ( "cogentcore.org/core/tree" "cogentcore.org/core/xyz" "cogentcore.org/core/xyz/xyzcore" + _ "cogentcore.org/lab/gosl/slbool/slboolcore" // include to get gui views "cogentcore.org/lab/physics" "cogentcore.org/lab/physics/world" ) +// Balls has sim params +type Balls struct { + + // Number of balls: if collide, then run out of memory above 1000 or so + NBalls int + + // Collide is whether the balls collide with each other + Collide bool + + // Size of each ball (m) + Size float32 + + // Mass of each ball (kg) + Mass float32 + + // size of the box (m) + Width float32 + Depth float32 + Height float32 + Thick float32 + + Bounce float32 + Friction float32 + FrictionTortion float32 + FrictionRolling float32 +} + +func (b *Balls) Defaults() { + b.NBalls = 1000 + b.Collide = true + b.Size = 0.2 + b.Mass = 0.1 + + b.Width = 50 + b.Depth = 50 + b.Height = 20 + b.Thick = .1 + + b.Bounce = 0.5 + b.Friction = 0 + b.FrictionTortion = 0 + b.FrictionRolling = 0 +} + func main() { // gpu.Debug = true b := core.NewBody("test1").SetTitle("Physics Balls") split := core.NewSplits(b) - // tv := core.NewTree(core.NewFrame(split)) - fv := core.NewForm(split) + fpanel := core.NewFrame(split) + fpanel.Styler(func(s *styles.Style) { + s.Direction = styles.Column + s.Grow.Set(1, 1) + }) + + bpf := core.NewForm(fpanel) + wpf := core.NewForm(fpanel) + tbvw := core.NewTabs(split) scfr, _ := tbvw.NewTab("3D View") + split.SetSplits(0.2, 0.8) se := xyzcore.NewSceneEditor(scfr) se.UpdateWidget() @@ -45,62 +98,74 @@ func main() { wr := world.NewWorld(sc) + bs := &Balls{} + bs.Defaults() + wl := physics.NewWorld() wl.GPU = true - fv.SetStruct(wl) - - split.SetSplits(0.2, 0.8) - - rot := math32.NewQuat(0, 0, 0, 1) - wr.NewBody(wl, "floor", physics.Plane, "#D0D0D080", math32.Vec3(0, 0, 0), - math32.Vec3(0, 0, 0), rot) - - width := float32(50) - depth := float32(50) - height := float32(20) - thick := float32(.1) - hw := width / 2 - hd := depth / 2 - hh := height / 2 - ht := thick / 2 - wr.NewBody(wl, "back-wall", physics.Box, "#0000FFA0", math32.Vec3(hw, hh, ht), - math32.Vec3(0, hh, -hd), rot) - wr.NewBody(wl, "left-wall", physics.Box, "#FF0000A0", math32.Vec3(ht, hh, hd), - math32.Vec3(-hw, hh, 0), rot) - wr.NewBody(wl, "right-wall", physics.Box, "#00FF00A0", math32.Vec3(ht, hh, hd), - math32.Vec3(hw, hh, 0), rot) - wr.NewBody(wl, "front-wall", physics.Box, "#FFFF00A0", math32.Vec3(hw, hh, ht), - math32.Vec3(0, hh, hd), rot) - - nballs := 1000 - size := float32(0.2) - bounce := float32(0.5) - box := width * .9 - // height := float32(20) - for i := range nballs { - _ = i - ht := rand.Float32() * height - x := rand.Float32()*box - 0.5*box - z := rand.Float32()*box - 0.5*box - clr := colors.Names[i%len(colors.Names)] - b1 := wr.NewDynamic(wl, "body", physics.Sphere, clr, 1.0, math32.Vec3(size, size, size), - math32.Vec3(x, size+ht, z), rot) - // todo: helper methods on view to set this stuff: - physics.Bodies.Set(bounce, int(b1.Index), int(physics.BodyBounce)) - // physics.SetBodyGroup(b1.Index, int32(i)) // no self collisions - } - wr.Init(wl) params := physics.GetParams(0) - params.Dt = 0.0001 // leaks balls > 0.0005 - subSteps := 100 // major speedup by inner-stepping + params.Dt = 0.0001 // leaks balls > 0.0005 + params.SubSteps = 100 // major speedup by inner-stepping // params.Gravity.Y = 0 params.ContactRelax = 0.1 // 0.1 seems most physical -- 0.2 getting a bit more ke? params.Restitution.SetBool(false) // not working! params.ContactMargin = 0.1 // 0.1 better than .01 -- leaks a few - wl.Config() - wr.Update() + bpf.SetStruct(bs) + wpf.SetStruct(params) + + config := func() { + wr.Reset() + wl.Reset() + rot := math32.NewQuat(0, 0, 0, 1) + wr.NewBody(wl, "floor", physics.Plane, "#D0D0D080", math32.Vec3(0, 0, 0), + math32.Vec3(0, 0, 0), rot) + + hw := bs.Width / 2 + hd := bs.Depth / 2 + hh := bs.Height / 2 + ht := bs.Thick / 2 + wr.NewBody(wl, "back-wall", physics.Box, "#0000FFA0", math32.Vec3(hw, hh, ht), + math32.Vec3(0, hh, -hd), rot) + wr.NewBody(wl, "left-wall", physics.Box, "#FF0000A0", math32.Vec3(ht, hh, hd), + math32.Vec3(-hw, hh, 0), rot) + wr.NewBody(wl, "right-wall", physics.Box, "#00FF00A0", math32.Vec3(ht, hh, hd), + math32.Vec3(hw, hh, 0), rot) + wr.NewBody(wl, "front-wall", physics.Box, "#FFFF00A0", math32.Vec3(hw, hh, ht), + math32.Vec3(0, hh, hd), rot) + + box := bs.Width * .9 + size := bs.Size + for i := range bs.NBalls { + _ = i + ht := rand.Float32() * bs.Height + x := rand.Float32()*box - 0.5*box + z := rand.Float32()*box - 0.5*box + clr := colors.Names[i%len(colors.Names)] + bl := wr.NewDynamic(wl, "body", physics.Sphere, clr, bs.Mass, math32.Vec3(size, size, size), + math32.Vec3(x, size+ht, z), rot) + if !bs.Collide { + physics.SetBodyGroup(bl.Index, int32(i)) // only collide within same group + } + bl.SetBodyBounce(bs.Bounce) + bl.SetBodyFriction(bs.Friction) + bl.SetBodyFrictionTortion(bs.FrictionTortion) + bl.SetBodyFrictionRolling(bs.FrictionRolling) + } + wr.Init(wl) + wr.Update() + } + + config() + + updateView := func() { + bpf.Update() + wpf.Update() + if se.IsVisible() { + se.NeedsRender() + } + } sc.Camera.Pose.Pos = math32.Vec3(0, 40, 3.5) sc.Camera.LookAt(math32.Vec3(0, 5, 0), math32.Vec3(0, 1, 0)) @@ -130,9 +195,6 @@ func main() { go func() { isRunning = true for range n { - for range subSteps { - wl.StepGet() // don't get anything - } wl.Step() wr.Update() if se.IsVisible() { @@ -159,13 +221,11 @@ func main() { core.NewToolbar(bar).Maker(func(p *tree.Plan) { tree.Add(p, func(w *core.Button) { w.SetText("Init").SetIcon(icons.Reset). - SetTooltip("Reset state"). + SetTooltip("Reset physics state back to starting."). OnClick(func(e events.Event) { wl.InitState() wr.Update() - if se.IsVisible() { - se.NeedsRender() - } + updateView() }) }) tree.Add(p, func(w *core.Button) { @@ -175,11 +235,23 @@ func main() { stop = true }) }) + tree.Add(p, func(w *core.Separator) {}) + stepNButton(p, 1) stepNButton(p, 10) stepNButton(p, 100) stepNButton(p, 1000) stepNButton(p, 10000) + tree.Add(p, func(w *core.Separator) {}) + + tree.Add(p, func(w *core.Button) { + w.SetText("Rebuild").SetIcon(icons.Reset). + SetTooltip("Rebuild the environment, when you change parameters"). + OnClick(func(e events.Event) { + config() + updateView() + }) + }) }) }) b.RunMainWindow() diff --git a/physics/gosl.go b/physics/gosl.go index 0c194792..4f6df58c 100644 --- a/physics/gosl.go +++ b/physics/gosl.go @@ -83,7 +83,6 @@ func GPUInit() { vr = sgp.Add("TensorStrides", gpu.Uint32, 1, gpu.ComputeShader) vr.ReadOnly = true vr = sgp.AddStruct("Params", int(unsafe.Sizeof(PhysParams{})), 1, gpu.ComputeShader) - vr.ReadOnly = true sgp.SetNValues(1) } { @@ -124,10 +123,6 @@ func GPUInit() { pl.AddVarUsed(2, "BroadContactsN") pl.AddVarUsed(2, "Dynamics") pl.AddVarUsed(0, "Params") - pl = gpu.NewComputePipelineShaderFS(shaders, "shaders/CollisionInit.wgsl", sy) - pl.AddVarUsed(0, "TensorStrides") - pl.AddVarUsed(2, "BroadContactsN") - pl.AddVarUsed(2, "ContactsN") pl = gpu.NewComputePipelineShaderFS(shaders, "shaders/CollisionNarrow.wgsl", sy) pl.AddVarUsed(0, "TensorStrides") pl.AddVarUsed(1, "Bodies") @@ -176,6 +171,11 @@ func GPUInit() { pl.AddVarUsed(1, "Bodies") pl.AddVarUsed(2, "Dynamics") pl.AddVarUsed(0, "Params") + pl = gpu.NewComputePipelineShaderFS(shaders, "shaders/StepInit.wgsl", sy) + pl.AddVarUsed(0, "TensorStrides") + pl.AddVarUsed(2, "BroadContactsN") + pl.AddVarUsed(2, "ContactsN") + pl.AddVarUsed(0, "Params") pl = gpu.NewComputePipelineShaderFS(shaders, "shaders/StepIntegrateBodies.wgsl", sy) pl.AddVarUsed(0, "TensorStrides") pl.AddVarUsed(1, "Bodies") @@ -258,48 +258,6 @@ func RunOneCollisionBroad(n int, syncVars ...GPUVars) { RunCollisionBroadCPU(n) } } -// RunCollisionInit runs the CollisionInit kernel with given number of elements, -// on either the CPU or GPU depending on the UseGPU variable. -// Can call multiple Run* kernels in a row, which are then all launched -// in the same command submission on the GPU, which is by far the most efficient. -// MUST call RunDone (with optional vars to sync) after all Run calls. -// Alternatively, a single-shot RunOneCollisionInit call does Run and Done for a -// single run-and-sync case. -func RunCollisionInit(n int) { - if UseGPU { - RunCollisionInitGPU(n) - } else { - RunCollisionInitCPU(n) - } -} - -// RunCollisionInitGPU runs the CollisionInit kernel on the GPU. See [RunCollisionInit] for more info. -func RunCollisionInitGPU(n int) { - sy := GPUSystem - pl := sy.ComputePipelines["CollisionInit"] - ce, _ := sy.BeginComputePass() - pl.Dispatch1D(ce, n, 64) -} - -// RunCollisionInitCPU runs the CollisionInit kernel on the CPU. -func RunCollisionInitCPU(n int) { - gpu.VectorizeFunc(0, n, CollisionInit) -} - -// RunOneCollisionInit runs the CollisionInit kernel with given number of elements, -// on either the CPU or GPU depending on the UseGPU variable. -// This version then calls RunDone with the given variables to sync -// after the Run, for a single-shot Run-and-Done call. If multiple kernels -// can be run in sequence, it is much more efficient to do multiple Run* -// calls followed by a RunDone call. -func RunOneCollisionInit(n int, syncVars ...GPUVars) { - if UseGPU { - RunCollisionInitGPU(n) - RunDone(syncVars...) - } else { - RunCollisionInitCPU(n) - } -} // RunCollisionNarrow runs the CollisionNarrow kernel with given number of elements, // on either the CPU or GPU depending on the UseGPU variable. // Can call multiple Run* kernels in a row, which are then all launched @@ -636,6 +594,48 @@ func RunOneStepBodyDeltas(n int, syncVars ...GPUVars) { RunStepBodyDeltasCPU(n) } } +// RunStepInit runs the StepInit kernel with given number of elements, +// on either the CPU or GPU depending on the UseGPU variable. +// Can call multiple Run* kernels in a row, which are then all launched +// in the same command submission on the GPU, which is by far the most efficient. +// MUST call RunDone (with optional vars to sync) after all Run calls. +// Alternatively, a single-shot RunOneStepInit call does Run and Done for a +// single run-and-sync case. +func RunStepInit(n int) { + if UseGPU { + RunStepInitGPU(n) + } else { + RunStepInitCPU(n) + } +} + +// RunStepInitGPU runs the StepInit kernel on the GPU. See [RunStepInit] for more info. +func RunStepInitGPU(n int) { + sy := GPUSystem + pl := sy.ComputePipelines["StepInit"] + ce, _ := sy.BeginComputePass() + pl.Dispatch1D(ce, n, 64) +} + +// RunStepInitCPU runs the StepInit kernel on the CPU. +func RunStepInitCPU(n int) { + gpu.VectorizeFunc(0, n, StepInit) +} + +// RunOneStepInit runs the StepInit kernel with given number of elements, +// on either the CPU or GPU depending on the UseGPU variable. +// This version then calls RunDone with the given variables to sync +// after the Run, for a single-shot Run-and-Done call. If multiple kernels +// can be run in sequence, it is much more efficient to do multiple Run* +// calls followed by a RunDone call. +func RunOneStepInit(n int, syncVars ...GPUVars) { + if UseGPU { + RunStepInitGPU(n) + RunDone(syncVars...) + } else { + RunStepInitCPU(n) + } +} // RunStepIntegrateBodies runs the StepIntegrateBodies kernel with given number of elements, // on either the CPU or GPU depending on the UseGPU variable. // Can call multiple Run* kernels in a row, which are then all launched diff --git a/physics/params.go b/physics/params.go index 6271f2bb..93f67739 100644 --- a/physics/params.go +++ b/physics/params.go @@ -13,14 +13,34 @@ import ( // PhysParams are the physics parameters type PhysParams struct { - // Iterations is the number of iterations to perform. - Iterations int32 `default:"2"` + // Iterations is the number of integration iterations to perform + // within each solver step. Muller et al (2020) report that 1 is best. + Iterations int32 `default:"1"` // Dt is the integration stepsize. - Dt float32 `default:"0.01"` + // For highly kinetic situations (e.g., rapidly moving bouncing balls) + // 0.0001 is needed to ensure contact registration. Use SubSteps to + // accomplish a target effective read-out step size. + Dt float32 `default:"0.0001"` - // SoftRelax is soft-body relaxation constant. - SoftRelax float32 `default:"0.9"` + // SubSteps is the number of integration steps to take per Step() + // function call. These sub steps are taken without any sync to/from + // the GPU and are therefore much faster. + SubSteps int32 `default:"10" min:"1"` + + // Contact margin is the extra distance for broadphase collision + // around rigid bodies. + ContactMargin float32 `defautl:"0.1"` + + // ContactRelax is rigid contact relaxation constant. + // Higher values cause errros + ContactRelax float32 `default:"0.1"` + + // Contact weighting: balances contact forces? + ContactWeighting slbool.Bool `default:"true"` + + // Restitution takes into account bounciness of objects. + Restitution slbool.Bool `default:"true"` // JointLinearRelax is joint linear relaxation constant. JointLinearRelax float32 `default:"0.7"` @@ -34,28 +54,18 @@ type PhysParams struct { // JointAngularComply is joint angular compliance constant. JointAngularComply float32 `default:"0"` - // ContactRelax is rigid contact relaxation constant. - ContactRelax float32 `default:"0.8"` - // AngularDamping is damping of angular motion. AngularDamping float32 `default:"0"` - // Contact weighting: balances contact forces? - ContactWeighting slbool.Bool `default:"true"` - - // Restitution takes into account bounciness of objects. - Restitution slbool.Bool `default:"true"` + // SoftRelax is soft-body relaxation constant. + SoftRelax float32 `default:"0.9"` // MaxGeomIter is number of iterations to perform in shape-based // geometry collision computations MaxGeomIter int32 `default:"10"` - // Contact margin is the extra distance for broadphase collision - // around rigid bodies. - ContactMargin float32 - // Maximum number of contacts to process at any given point. - ContactsMax int32 + ContactsMax int32 `edit:"-"` // Index for the current state (0 or 1, alternates with Next). Cur int32 `edit:"-"` @@ -82,27 +92,31 @@ type PhysParams struct { // to examine. BodyCollidePairsN int32 `edit:"-"` - pad, pad1 int32 + pad int32 // Gravity is the gravity acceleration function Gravity slvec.Vector3 } func (pr *PhysParams) Defaults() { - pr.Iterations = 2 - pr.Dt = 0.01 + pr.Iterations = 1 + pr.Dt = 0.0001 + pr.SubSteps = 10 pr.Gravity.Set(0, -9.81, 0) - pr.SoftRelax = 0.9 + + pr.ContactMargin = 0.1 + pr.ContactRelax = 0.1 + pr.ContactWeighting.SetBool(true) + pr.Restitution.SetBool(false) + pr.JointLinearRelax = 0.7 pr.JointAngularRelax = 0.4 pr.JointLinearComply = 0 pr.JointAngularComply = 0 - pr.ContactRelax = 0.8 + pr.AngularDamping = 0 + pr.SoftRelax = 0.9 pr.MaxGeomIter = 10 - pr.ContactWeighting.SetBool(true) - pr.Restitution.SetBool(true) - pr.ContactMargin = 0.1 } //gosl:end diff --git a/physics/shaders/CollisionBroad.wgsl b/physics/shaders/CollisionBroad.wgsl index a6e2da56..a56ef21a 100644 --- a/physics/shaders/CollisionBroad.wgsl +++ b/physics/shaders/CollisionBroad.wgsl @@ -5,7 +5,7 @@ @group(0) @binding(0) var TensorStrides: array; @group(0) @binding(1) -var Params: array; +var Params: array; // // Bodies are the rigid body elements (dynamic and static), // specifying the constant, non-dynamic properties, // which is initial state for dynamics. // [body][BodyVarsN] @group(1) @binding(0) var Bodies: array; @@ -162,7 +162,7 @@ fn SetBroadContactPointIdx(idx: i32,ptIdx: i32) { BroadContacts[Index2D(TensorStrides[70], TensorStrides[71], u32(idx), u32(ContactPointIdx))] = bitcast(u32(ptIdx)); } fn CollisionBroad(i: u32) { //gosl:kernel - let params = Params[0]; + var params = Params[0]; var ci = i32(i); if (ci >= params.BodyCollidePairsN) { return; @@ -205,6 +205,7 @@ fn CollisionBroad(i: u32) { //gosl:kernel return; } AddBroadContacts(biA, biB, nci, ncA, ncB); + Params[0] = params; } fn AddBroadContacts(biA: i32,biB: i32,nci: i32,ncA: i32,ncB: i32) { for (var i=0; i, } diff --git a/physics/shaders/CollisionInit.wgsl b/physics/shaders/CollisionInit.wgsl deleted file mode 100644 index e5453a00..00000000 --- a/physics/shaders/CollisionInit.wgsl +++ /dev/null @@ -1,309 +0,0 @@ -// Code generated by "gosl"; DO NOT EDIT -// kernel: CollisionInit - -// // Params are global parameters. -@group(0) @binding(0) -var TensorStrides: array; -// // Bodies are the rigid body elements (dynamic and static), // specifying the constant, non-dynamic properties, // which is initial state for dynamics. // [body][BodyVarsN] -// // Dynamics are the dynamic rigid body elements: these actually move. // Two alternating states are used: Params.Cur and Params.Next. // [dyn body][cur/next][DynamicVarsN] -@group(2) @binding(1) -var BroadContactsN: array; -@group(2) @binding(3) -var ContactsN: array; -// // JointControls are dynamic joint control inputs, per joint DoF // (in correspondence with [JointDoFs]). This can be uploaded to the // GPU at every step. // [dof][JointControlVarsN] - -alias GPUVars = i32; - -@compute @workgroup_size(64, 1, 1) -fn main(@builtin(workgroup_id) wgid: vec3, @builtin(num_workgroups) nwg: vec3, @builtin(local_invocation_index) loci: u32) { - let idx = loci + (wgid.x + wgid.y * nwg.x + wgid.z * nwg.x * nwg.y) * 64; - CollisionInit(idx); -} - -fn Index1D(s0: u32, i0: u32) -> u32 { - return s0 * i0; -} - - -//////// import: "vars.go" - -//////// import: "body.go" -alias BodyVars = i32; //enums:enum -const BodyShape: BodyVars = 0; -const BodyDynamic: BodyVars = 1; -const BodyWorld: BodyVars = 2; -const BodyGroup: BodyVars = 3; -const BodyHSizeX: BodyVars = 4; -const BodyHSizeY: BodyVars = 5; -const BodyHSizeZ: BodyVars = 6; -const BodyThick: BodyVars = 7; -const BodyMass: BodyVars = 8; -const BodyInvMass: BodyVars = 9; -const BodyBounce: BodyVars = 10; -const BodyFriction: BodyVars = 11; -const BodyFrictionTortion: BodyVars = 12; -const BodyFrictionRolling: BodyVars = 13; -const BodyPosX: BodyVars = 14; -const BodyPosY: BodyVars = 15; -const BodyPosZ: BodyVars = 16; -const BodyQuatX: BodyVars = 17; -const BodyQuatY: BodyVars = 18; -const BodyQuatZ: BodyVars = 19; -const BodyQuatW: BodyVars = 20; -const BodyComX: BodyVars = 21; -const BodyComY: BodyVars = 22; -const BodyComZ: BodyVars = 23; -const BodyInertiaXX: BodyVars = 24; -const BodyInertiaYX: BodyVars = 25; -const BodyInertiaZX: BodyVars = 26; -const BodyInertiaXY: BodyVars = 27; -const BodyInertiaYY: BodyVars = 28; -const BodyInertiaZY: BodyVars = 29; -const BodyInertiaXZ: BodyVars = 30; -const BodyInertiaYZ: BodyVars = 31; -const BodyInertiaZZ: BodyVars = 32; -const BodyInvInertiaXX: BodyVars = 33; -const BodyInvInertiaYX: BodyVars = 34; -const BodyInvInertiaZX: BodyVars = 35; -const BodyInvInertiaXY: BodyVars = 36; -const BodyInvInertiaYY: BodyVars = 37; -const BodyInvInertiaZY: BodyVars = 38; -const BodyInvInertiaXZ: BodyVars = 39; -const BodyInvInertiaYZ: BodyVars = 40; -const BodyInvInertiaZZ: BodyVars = 41; -const BodyRadius: BodyVars = 42; - -//////// import: "contact.go" -alias ContactVars = i32; //enums:enum -const ContactA: ContactVars = 0; -const ContactB: ContactVars = 1; -const ContactPointIdx: ContactVars = 2; -const ContactAPointX: ContactVars = 3; -const ContactAPointY: ContactVars = 4; -const ContactAPointZ: ContactVars = 5; -const ContactBPointX: ContactVars = 6; -const ContactBPointY: ContactVars = 7; -const ContactBPointZ: ContactVars = 8; -const ContactAOffX: ContactVars = 9; -const ContactAOffY: ContactVars = 10; -const ContactAOffZ: ContactVars = 11; -const ContactBOffX: ContactVars = 12; -const ContactBOffY: ContactVars = 13; -const ContactBOffZ: ContactVars = 14; -const ContactAThick: ContactVars = 15; -const ContactBThick: ContactVars = 16; -const ContactNormX: ContactVars = 17; -const ContactNormY: ContactVars = 18; -const ContactNormZ: ContactVars = 19; -const ContactWeight: ContactVars = 20; -const ContactADeltaX: ContactVars = 21; -const ContactADeltaY: ContactVars = 22; -const ContactADeltaZ: ContactVars = 23; -const ContactAAngDeltaX: ContactVars = 24; -const ContactAAngDeltaY: ContactVars = 25; -const ContactAAngDeltaZ: ContactVars = 26; -const ContactBDeltaX: ContactVars = 27; -const ContactBDeltaY: ContactVars = 28; -const ContactBDeltaZ: ContactVars = 29; -const ContactBAngDeltaX: ContactVars = 30; -const ContactBAngDeltaY: ContactVars = 31; -const ContactBAngDeltaZ: ContactVars = 32; -const BroadContactVarsN = ContactAPointX; -fn CollisionInit(i: u32) { //gosl:kernel - if (i > 0) { - return; - } - BroadContactsN[0] = 0; - ContactsN[0] = 0; -} - -//////// import: "control.go" -alias JointControlVars = i32; //enums:enum -const JointControlForce: JointControlVars = 0; -const JointTargetPos: JointControlVars = 1; -const JointTargetVel: JointControlVars = 2; - -//////// import: "dynamics.go" -alias DynamicVars = i32; //enums:enum -const DynBody: DynamicVars = 0; -const DynPosX: DynamicVars = 1; -const DynPosY: DynamicVars = 2; -const DynPosZ: DynamicVars = 3; -const DynQuatX: DynamicVars = 4; -const DynQuatY: DynamicVars = 5; -const DynQuatZ: DynamicVars = 6; -const DynQuatW: DynamicVars = 7; -const DynVelX: DynamicVars = 8; -const DynVelY: DynamicVars = 9; -const DynVelZ: DynamicVars = 10; -const DynAngVelX: DynamicVars = 11; -const DynAngVelY: DynamicVars = 12; -const DynAngVelZ: DynamicVars = 13; -const DynAccX: DynamicVars = 14; -const DynAccY: DynamicVars = 15; -const DynAccZ: DynamicVars = 16; -const DynAngAccX: DynamicVars = 17; -const DynAngAccY: DynamicVars = 18; -const DynAngAccZ: DynamicVars = 19; -const DynForceX: DynamicVars = 20; -const DynForceY: DynamicVars = 21; -const DynForceZ: DynamicVars = 22; -const DynTorqueX: DynamicVars = 23; -const DynTorqueY: DynamicVars = 24; -const DynTorqueZ: DynamicVars = 25; -const DynDeltaX: DynamicVars = 26; -const DynDeltaY: DynamicVars = 27; -const DynDeltaZ: DynamicVars = 28; -const DynAngDeltaX: DynamicVars = 29; -const DynAngDeltaY: DynamicVars = 30; -const DynAngDeltaZ: DynamicVars = 31; -const DynContactWeight: DynamicVars = 32; - -//////// import: "enumgen.go" -const BodyVarsN: BodyVars = 43; -const ContactVarsN: ContactVars = 33; -const JointControlVarsN: JointControlVars = 3; -const DynamicVarsN: DynamicVars = 33; -const GPUVarsN: GPUVars = 12; -const JointTypesN: JointTypes = 7; -const JointVarsN: JointVars = 50; -const JointDoFVarsN: JointDoFVars = 7; -const ShapesN: Shapes = 6; - -//////// import: "joint.go" -const JointLimitUnlimited = 1e10; -alias JointTypes = i32; //enums:enum -const Prismatic: JointTypes = 0; -const Revolute: JointTypes = 1; -const Ball: JointTypes = 2; -const Fixed: JointTypes = 3; -const Free: JointTypes = 4; -const Distance: JointTypes = 5; -const D6: JointTypes = 6; -alias JointVars = i32; //enums:enum -const JointType: JointVars = 0; -const JointEnabled: JointVars = 1; -const JointParent: JointVars = 2; -const JointChild: JointVars = 3; -const JointPPosX: JointVars = 4; -const JointPPosY: JointVars = 5; -const JointPPosZ: JointVars = 6; -const JointPQuatX: JointVars = 7; -const JointPQuatY: JointVars = 8; -const JointPQuatZ: JointVars = 9; -const JointPQuatW: JointVars = 10; -const JointCPosX: JointVars = 11; -const JointCPosY: JointVars = 12; -const JointCPosZ: JointVars = 13; -const JointCQuatX: JointVars = 14; -const JointCQuatY: JointVars = 15; -const JointCQuatZ: JointVars = 16; -const JointCQuatW: JointVars = 17; -const JointLinearDoFN: JointVars = 18; -const JointAngularDoFN: JointVars = 19; -const JointDoF1: JointVars = 20; -const JointDoF2: JointVars = 21; -const JointDoF3: JointVars = 22; -const JointDoF4: JointVars = 23; -const JointDoF5: JointVars = 24; -const JointDoF6: JointVars = 25; -const JointPForceX: JointVars = 26; -const JointPForceY: JointVars = 27; -const JointPForceZ: JointVars = 28; -const JointPTorqueX: JointVars = 29; -const JointPTorqueY: JointVars = 30; -const JointPTorqueZ: JointVars = 31; -const JointCForceX: JointVars = 32; -const JointCForceY: JointVars = 33; -const JointCForceZ: JointVars = 34; -const JointCTorqueX: JointVars = 35; -const JointCTorqueY: JointVars = 36; -const JointCTorqueZ: JointVars = 37; -const JointPDeltaX: JointVars = 38; -const JointPDeltaY: JointVars = 39; -const JointPDeltaZ: JointVars = 40; -const JointPAngDeltaX: JointVars = 41; -const JointPAngDeltaY: JointVars = 42; -const JointPAngDeltaZ: JointVars = 43; -const JointCDeltaX: JointVars = 44; -const JointCDeltaY: JointVars = 45; -const JointCDeltaZ: JointVars = 46; -const JointCAngDeltaX: JointVars = 47; -const JointCAngDeltaY: JointVars = 48; -const JointCAngDeltaZ: JointVars = 49; -alias JointDoFVars = i32; //enums:enum -const JointAxisX: JointDoFVars = 0; -const JointAxisY: JointDoFVars = 1; -const JointAxisZ: JointDoFVars = 2; -const JointLimitLower: JointDoFVars = 3; -const JointLimitUpper: JointDoFVars = 4; -const JointStiff: JointDoFVars = 5; -const JointDamp: JointDoFVars = 6; - -//////// import: "params.go" -struct PhysParams { - Iterations: i32, - Dt: f32, - SoftRelax: f32, - JointLinearRelax: f32, - JointAngularRelax: f32, - JointLinearComply: f32, - JointAngularComply: f32, - ContactRelax: f32, - AngularDamping: f32, - ContactWeighting: i32, - Restitution: i32, - MaxGeomIter: i32, - ContactMargin: f32, - ContactsMax: i32, - Cur: i32, - Next: i32, - BodiesN: i32, - DynamicsN: i32, - JointsN: i32, - JointDoFsN: i32, - BodyJointsMax: i32, - BodyCollidePairsN: i32, - pad: i32, - pad1: i32, - Gravity: vec4, -} - -//////// import: "shapecollide.go" -struct GeomData { - BodyIdx: i32, - Shape: Shapes, - MinSize: f32, - Thick: f32, - Radius: f32, - Size: vec3, - WbR: vec3, - WbQ: vec4, - BwR: vec3, - BwQ: vec4, -} - -//////// import: "shapegeom.go" - -//////// import: "shapes.go" -alias Shapes = i32; //enums:enum -const Plane: Shapes = 0; -const Sphere: Shapes = 1; -const Capsule: Shapes = 2; -const Cylinder: Shapes = 3; -const Box: Shapes = 4; -const Cone: Shapes = 5; - -//////// import: "slmath-matrix3.go" - -//////// import: "slmath-quaternion.go" - -//////// import: "slmath-vector2.go" - -//////// import: "slmath-vector3.go" - -//////// import: "step.go" - -//////// import: "step_body.go" - -//////// import: "step_joint.go" \ No newline at end of file diff --git a/physics/shaders/CollisionNarrow.wgsl b/physics/shaders/CollisionNarrow.wgsl index 9e39ac0e..32beef82 100644 --- a/physics/shaders/CollisionNarrow.wgsl +++ b/physics/shaders/CollisionNarrow.wgsl @@ -5,7 +5,7 @@ @group(0) @binding(0) var TensorStrides: array; @group(0) @binding(1) -var Params: array; +var Params: array; // // Bodies are the rigid body elements (dynamic and static), // specifying the constant, non-dynamic properties, // which is initial state for dynamics. // [body][BodyVarsN] @group(1) @binding(0) var Bodies: array; @@ -199,7 +199,7 @@ fn SetContactNorm(idx: i32, pos: vec3) { Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactNormZ))] = pos.z; } fn CollisionNarrow(i: u32) { //gosl:kernel - let params = Params[0]; + var params = Params[0]; var ci = i32(i); var cmax = BroadContactsN[0]; if (ci >= cmax) { @@ -306,6 +306,7 @@ fn CollisionNarrow(i: u32) { //gosl:kernel SetContactNorm(nci, norm); Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(nci), u32(ContactAThick))] = offMagA; Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(nci), u32(ContactBThick))] = offMagB; + Params[0] = params; } //////// import: "control.go" @@ -441,17 +442,18 @@ const JointDamp: JointDoFVars = 6; struct PhysParams { Iterations: i32, Dt: f32, - SoftRelax: f32, + SubSteps: i32, + ContactMargin: f32, + ContactRelax: f32, + ContactWeighting: i32, + Restitution: i32, JointLinearRelax: f32, JointAngularRelax: f32, JointLinearComply: f32, JointAngularComply: f32, - ContactRelax: f32, AngularDamping: f32, - ContactWeighting: i32, - Restitution: i32, + SoftRelax: f32, MaxGeomIter: i32, - ContactMargin: f32, ContactsMax: i32, Cur: i32, Next: i32, @@ -462,7 +464,6 @@ struct PhysParams { BodyJointsMax: i32, BodyCollidePairsN: i32, pad: i32, - pad1: i32, Gravity: vec4, } diff --git a/physics/shaders/DeltasFromContacts.wgsl b/physics/shaders/DeltasFromContacts.wgsl index d7415820..cf62a8b6 100644 --- a/physics/shaders/DeltasFromContacts.wgsl +++ b/physics/shaders/DeltasFromContacts.wgsl @@ -5,7 +5,7 @@ @group(0) @binding(0) var TensorStrides: array; @group(0) @binding(1) -var Params: array; +var Params: array; // // Bodies are the rigid body elements (dynamic and static), // specifying the constant, non-dynamic properties, // which is initial state for dynamics. // [body][BodyVarsN] // // Dynamics are the dynamic rigid body elements: these actually move. // Two alternating states are used: Params.Cur and Params.Next. // [dyn body][cur/next][DynamicVarsN] @group(2) @binding(0) @@ -136,7 +136,7 @@ fn ContactBAngDelta(idx: i32) -> vec3 { return vec3(Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactBAngDeltaX))], Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactBAngDeltaY))], Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactBAngDeltaZ))]); } fn DeltasFromContacts(i: u32) { //gosl:kernel - let params = Params[0]; + var params = Params[0]; var di = i32(i); if (di >= params.DynamicsN) { return; @@ -173,6 +173,7 @@ fn DeltasFromContacts(i: u32) { //gosl:kernel SetDynamicDelta(di, params.Next, td+(v0)); SetDynamicAngDelta(di, params.Next, ta+(w0)); Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(di), u32(params.Next), u32(DynContactWeight))] = tw; + Params[0] = params; } //////// import: "control.go" @@ -319,17 +320,18 @@ const JointDamp: JointDoFVars = 6; struct PhysParams { Iterations: i32, Dt: f32, - SoftRelax: f32, + SubSteps: i32, + ContactMargin: f32, + ContactRelax: f32, + ContactWeighting: i32, + Restitution: i32, JointLinearRelax: f32, JointAngularRelax: f32, JointLinearComply: f32, JointAngularComply: f32, - ContactRelax: f32, AngularDamping: f32, - ContactWeighting: i32, - Restitution: i32, + SoftRelax: f32, MaxGeomIter: i32, - ContactMargin: f32, ContactsMax: i32, Cur: i32, Next: i32, @@ -340,7 +342,6 @@ struct PhysParams { BodyJointsMax: i32, BodyCollidePairsN: i32, pad: i32, - pad1: i32, Gravity: vec4, } diff --git a/physics/shaders/DeltasFromJoints.wgsl b/physics/shaders/DeltasFromJoints.wgsl index 06551fc7..2e637431 100644 --- a/physics/shaders/DeltasFromJoints.wgsl +++ b/physics/shaders/DeltasFromJoints.wgsl @@ -5,7 +5,7 @@ @group(0) @binding(0) var TensorStrides: array; @group(0) @binding(1) -var Params: array; +var Params: array; // // Bodies are the rigid body elements (dynamic and static), // specifying the constant, non-dynamic properties, // which is initial state for dynamics. // [body][BodyVarsN] @group(1) @binding(1) var Joints: array; @@ -273,17 +273,18 @@ const JointDamp: JointDoFVars = 6; struct PhysParams { Iterations: i32, Dt: f32, - SoftRelax: f32, + SubSteps: i32, + ContactMargin: f32, + ContactRelax: f32, + ContactWeighting: i32, + Restitution: i32, JointLinearRelax: f32, JointAngularRelax: f32, JointLinearComply: f32, JointAngularComply: f32, - ContactRelax: f32, AngularDamping: f32, - ContactWeighting: i32, - Restitution: i32, + SoftRelax: f32, MaxGeomIter: i32, - ContactMargin: f32, ContactsMax: i32, Cur: i32, Next: i32, @@ -294,7 +295,6 @@ struct PhysParams { BodyJointsMax: i32, BodyCollidePairsN: i32, pad: i32, - pad1: i32, Gravity: vec4, } @@ -335,7 +335,7 @@ const Cone: Shapes = 5; //////// import: "step_body.go" fn DeltasFromJoints(i: u32) { //gosl:kernel - let params = Params[0]; + var params = Params[0]; var di = i32(i); if (di >= params.DynamicsN) { return; @@ -364,6 +364,7 @@ fn DeltasFromJoints(i: u32) { //gosl:kernel var w0 = DynamicAngDelta(di, params.Next); SetDynamicDelta(di, params.Next, td+(v0)); SetDynamicAngDelta(di, params.Next, ta+(w0)); + Params[0] = params; } //////// import: "step_joint.go" \ No newline at end of file diff --git a/physics/shaders/DynamicsCurToNext.wgsl b/physics/shaders/DynamicsCurToNext.wgsl index 261e27e2..d6063185 100644 --- a/physics/shaders/DynamicsCurToNext.wgsl +++ b/physics/shaders/DynamicsCurToNext.wgsl @@ -5,7 +5,7 @@ @group(0) @binding(0) var TensorStrides: array; @group(0) @binding(1) -var Params: array; +var Params: array; // // Bodies are the rigid body elements (dynamic and static), // specifying the constant, non-dynamic properties, // which is initial state for dynamics. // [body][BodyVarsN] // // Dynamics are the dynamic rigid body elements: these actually move. // Two alternating states are used: Params.Cur and Params.Next. // [dyn body][cur/next][DynamicVarsN] @group(2) @binding(0) @@ -237,17 +237,18 @@ const JointDamp: JointDoFVars = 6; struct PhysParams { Iterations: i32, Dt: f32, - SoftRelax: f32, + SubSteps: i32, + ContactMargin: f32, + ContactRelax: f32, + ContactWeighting: i32, + Restitution: i32, JointLinearRelax: f32, JointAngularRelax: f32, JointLinearComply: f32, JointAngularComply: f32, - ContactRelax: f32, AngularDamping: f32, - ContactWeighting: i32, - Restitution: i32, + SoftRelax: f32, MaxGeomIter: i32, - ContactMargin: f32, ContactsMax: i32, Cur: i32, Next: i32, @@ -258,7 +259,6 @@ struct PhysParams { BodyJointsMax: i32, BodyCollidePairsN: i32, pad: i32, - pad1: i32, Gravity: vec4, } @@ -299,7 +299,7 @@ const Cone: Shapes = 5; //////// import: "step_body.go" fn DynamicsCurToNext(i: u32) { //gosl:kernel - let params = Params[0]; + var params = Params[0]; var ii = i32(i); if (ii >= params.DynamicsN) { return; @@ -308,6 +308,7 @@ fn DynamicsCurToNext(i: u32) { //gosl:kernel di < DynamicVarsN; di++) { Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(ii), u32(params.Next), u32(di))] = Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(ii), u32(params.Cur), u32(di))]; } + Params[0] = params; } //////// import: "step_joint.go" \ No newline at end of file diff --git a/physics/shaders/ForcesFromJoints.wgsl b/physics/shaders/ForcesFromJoints.wgsl index 33029c24..041a0c0f 100644 --- a/physics/shaders/ForcesFromJoints.wgsl +++ b/physics/shaders/ForcesFromJoints.wgsl @@ -5,7 +5,7 @@ @group(0) @binding(0) var TensorStrides: array; @group(0) @binding(1) -var Params: array; +var Params: array; // // Bodies are the rigid body elements (dynamic and static), // specifying the constant, non-dynamic properties, // which is initial state for dynamics. // [body][BodyVarsN] @group(1) @binding(1) var Joints: array; @@ -267,17 +267,18 @@ const JointDamp: JointDoFVars = 6; struct PhysParams { Iterations: i32, Dt: f32, - SoftRelax: f32, + SubSteps: i32, + ContactMargin: f32, + ContactRelax: f32, + ContactWeighting: i32, + Restitution: i32, JointLinearRelax: f32, JointAngularRelax: f32, JointLinearComply: f32, JointAngularComply: f32, - ContactRelax: f32, AngularDamping: f32, - ContactWeighting: i32, - Restitution: i32, + SoftRelax: f32, MaxGeomIter: i32, - ContactMargin: f32, ContactsMax: i32, Cur: i32, Next: i32, @@ -288,7 +289,6 @@ struct PhysParams { BodyJointsMax: i32, BodyCollidePairsN: i32, pad: i32, - pad1: i32, Gravity: vec4, } @@ -329,7 +329,7 @@ const Cone: Shapes = 5; //////// import: "step_body.go" fn ForcesFromJoints(i: u32) { //gosl:kernel - let params = Params[0]; + var params = Params[0]; var di = i32(i); if (di >= params.DynamicsN) { return; @@ -356,6 +356,7 @@ fn ForcesFromJoints(i: u32) { //gosl:kernel } SetDynamicForce(di, params.Next, tf); SetDynamicTorque(di, params.Next, tt); + Params[0] = params; } //////// import: "step_joint.go" \ No newline at end of file diff --git a/physics/shaders/InitDynamics.wgsl b/physics/shaders/InitDynamics.wgsl index 118c578b..fdcabf7e 100644 --- a/physics/shaders/InitDynamics.wgsl +++ b/physics/shaders/InitDynamics.wgsl @@ -5,7 +5,7 @@ @group(0) @binding(0) var TensorStrides: array; @group(0) @binding(1) -var Params: array; +var Params: array; // // Bodies are the rigid body elements (dynamic and static), // specifying the constant, non-dynamic properties, // which is initial state for dynamics. // [body][BodyVarsN] @group(1) @binding(0) var Bodies: array; @@ -244,17 +244,18 @@ const JointDamp: JointDoFVars = 6; struct PhysParams { Iterations: i32, Dt: f32, - SoftRelax: f32, + SubSteps: i32, + ContactMargin: f32, + ContactRelax: f32, + ContactWeighting: i32, + Restitution: i32, JointLinearRelax: f32, JointAngularRelax: f32, JointLinearComply: f32, JointAngularComply: f32, - ContactRelax: f32, AngularDamping: f32, - ContactWeighting: i32, - Restitution: i32, + SoftRelax: f32, MaxGeomIter: i32, - ContactMargin: f32, ContactsMax: i32, Cur: i32, Next: i32, @@ -265,7 +266,6 @@ struct PhysParams { BodyJointsMax: i32, BodyCollidePairsN: i32, pad: i32, - pad1: i32, Gravity: vec4, } @@ -306,7 +306,7 @@ const Cone: Shapes = 5; //////// import: "step_body.go" fn InitDynamics(i: u32) { //gosl:kernel - let params = Params[0]; + var params = Params[0]; var ii = i32(i); if (ii >= params.DynamicsN) { return; @@ -326,6 +326,7 @@ fn InitDynamics(i: u32) { //gosl:kernel u32(ii), u32(cni), u32(v))] = 0.0; } } + Params[0] = params; } //////// import: "step_joint.go" \ No newline at end of file diff --git a/physics/shaders/StepBodyContacts.wgsl b/physics/shaders/StepBodyContacts.wgsl index 453dde89..4499b6d5 100644 --- a/physics/shaders/StepBodyContacts.wgsl +++ b/physics/shaders/StepBodyContacts.wgsl @@ -5,7 +5,7 @@ @group(0) @binding(0) var TensorStrides: array; @group(0) @binding(1) -var Params: array; +var Params: array; // // Bodies are the rigid body elements (dynamic and static), // specifying the constant, non-dynamic properties, // which is initial state for dynamics. // [body][BodyVarsN] @group(1) @binding(0) var Bodies: array; @@ -164,7 +164,7 @@ fn SetContactAAngDelta(idx: i32, pos: vec3) { Contacts[Index2D(TensorStride fn SetContactBDelta(idx: i32, pos: vec3) { Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactBDeltaX))] = pos.x;; Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactBDeltaY))] = pos.y;; Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactBDeltaZ))] = pos.z; } fn SetContactBAngDelta(idx: i32, pos: vec3) { Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactBAngDeltaX))] = pos.x;; Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactBAngDeltaY))] = pos.y;; Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactBAngDeltaZ))] = pos.z; } fn StepBodyContacts(i: u32) { //gosl:kernel -let params = Params[0];; var ci = i32(i); +var params = Params[0];; var ci = i32(i); ; var cmax = ContactsN[0]; ; if (ci >= cmax) { return; @@ -468,17 +468,18 @@ const JointDamp: JointDoFVars = 6; struct PhysParams { Iterations: i32, Dt: f32, - SoftRelax: f32, + SubSteps: i32, + ContactMargin: f32, + ContactRelax: f32, + ContactWeighting: i32, + Restitution: i32, JointLinearRelax: f32, JointAngularRelax: f32, JointLinearComply: f32, JointAngularComply: f32, - ContactRelax: f32, AngularDamping: f32, - ContactWeighting: i32, - Restitution: i32, + SoftRelax: f32, MaxGeomIter: i32, - ContactMargin: f32, ContactsMax: i32, Cur: i32, Next: i32, @@ -489,7 +490,6 @@ struct PhysParams { BodyJointsMax: i32, BodyCollidePairsN: i32, pad: i32, - pad1: i32, Gravity: vec4, } diff --git a/physics/shaders/StepBodyDeltas.wgsl b/physics/shaders/StepBodyDeltas.wgsl index 74f7bd68..e02f4272 100644 --- a/physics/shaders/StepBodyDeltas.wgsl +++ b/physics/shaders/StepBodyDeltas.wgsl @@ -5,7 +5,7 @@ @group(0) @binding(0) var TensorStrides: array; @group(0) @binding(1) -var Params: array; +var Params: array; // // Bodies are the rigid body elements (dynamic and static), // specifying the constant, non-dynamic properties, // which is initial state for dynamics. // [body][BodyVarsN] @group(1) @binding(0) var Bodies: array; @@ -277,17 +277,18 @@ const JointDamp: JointDoFVars = 6; struct PhysParams { Iterations: i32, Dt: f32, - SoftRelax: f32, + SubSteps: i32, + ContactMargin: f32, + ContactRelax: f32, + ContactWeighting: i32, + Restitution: i32, JointLinearRelax: f32, JointAngularRelax: f32, JointLinearComply: f32, JointAngularComply: f32, - ContactRelax: f32, AngularDamping: f32, - ContactWeighting: i32, - Restitution: i32, + SoftRelax: f32, MaxGeomIter: i32, - ContactMargin: f32, ContactsMax: i32, Cur: i32, Next: i32, @@ -298,7 +299,6 @@ struct PhysParams { BodyJointsMax: i32, BodyCollidePairsN: i32, pad: i32, - pad1: i32, Gravity: vec4, } @@ -382,7 +382,7 @@ fn Cross3(v: vec3,o: vec3) -> vec3 { //////// import: "step_body.go" fn StepBodyDeltas(i: u32) { //gosl:kernel - let params = Params[0]; + var params = Params[0]; var di = i32(i); if (di >= params.DynamicsN) { return; @@ -429,6 +429,7 @@ fn StepBodyDeltas(i: u32) { //gosl:kernel SetDynamicQuat(di, params.Next, q1); SetDynamicDelta(di, params.Next, v1); SetDynamicAngDelta(di, params.Next, w1); + Params[0] = params; } //////// import: "step_joint.go" \ No newline at end of file diff --git a/physics/shaders/StepIntegrateBodies.wgsl b/physics/shaders/StepIntegrateBodies.wgsl index 859c618f..6432de8f 100644 --- a/physics/shaders/StepIntegrateBodies.wgsl +++ b/physics/shaders/StepIntegrateBodies.wgsl @@ -5,7 +5,7 @@ @group(0) @binding(0) var TensorStrides: array; @group(0) @binding(1) -var Params: array; +var Params: array; // // Bodies are the rigid body elements (dynamic and static), // specifying the constant, non-dynamic properties, // which is initial state for dynamics. // [body][BodyVarsN] @group(1) @binding(0) var Bodies: array; @@ -267,17 +267,18 @@ const JointDamp: JointDoFVars = 6; struct PhysParams { Iterations: i32, Dt: f32, - SoftRelax: f32, + SubSteps: i32, + ContactMargin: f32, + ContactRelax: f32, + ContactWeighting: i32, + Restitution: i32, JointLinearRelax: f32, JointAngularRelax: f32, JointLinearComply: f32, JointAngularComply: f32, - ContactRelax: f32, AngularDamping: f32, - ContactWeighting: i32, - Restitution: i32, + SoftRelax: f32, MaxGeomIter: i32, - ContactMargin: f32, ContactsMax: i32, Cur: i32, Next: i32, @@ -288,7 +289,6 @@ struct PhysParams { BodyJointsMax: i32, BodyCollidePairsN: i32, pad: i32, - pad1: i32, Gravity: vec4, } @@ -382,7 +382,7 @@ fn OneIfNonzero(f: f32) -> f32 { //////// import: "step_body.go" fn StepIntegrateBodies(i: u32) { //gosl:kernel - let params = Params[0]; + var params = Params[0]; var di = i32(i); if (di >= params.DynamicsN) { return; @@ -414,6 +414,7 @@ fn StepIntegrateBodies(i: u32) { //gosl:kernel SetDynamicQuat(di, params.Next, q1); SetDynamicDelta(di, params.Next, v1); SetDynamicAngDelta(di, params.Next, w1); + Params[0] = params; } //////// import: "step_joint.go" \ No newline at end of file diff --git a/physics/shaders/StepJointForces.wgsl b/physics/shaders/StepJointForces.wgsl index 004ae2ff..68993b78 100644 --- a/physics/shaders/StepJointForces.wgsl +++ b/physics/shaders/StepJointForces.wgsl @@ -5,7 +5,7 @@ @group(0) @binding(0) var TensorStrides: array; @group(0) @binding(1) -var Params: array; +var Params: array; // // Bodies are the rigid body elements (dynamic and static), // specifying the constant, non-dynamic properties, // which is initial state for dynamics. // [body][BodyVarsN] @group(1) @binding(0) var Bodies: array; @@ -312,17 +312,18 @@ fn JointAxis(idx: i32,dof: i32) -> vec3 { struct PhysParams { Iterations: i32, Dt: f32, - SoftRelax: f32, + SubSteps: i32, + ContactMargin: f32, + ContactRelax: f32, + ContactWeighting: i32, + Restitution: i32, JointLinearRelax: f32, JointAngularRelax: f32, JointLinearComply: f32, JointAngularComply: f32, - ContactRelax: f32, AngularDamping: f32, - ContactWeighting: i32, - Restitution: i32, + SoftRelax: f32, MaxGeomIter: i32, - ContactMargin: f32, ContactsMax: i32, Cur: i32, Next: i32, @@ -333,7 +334,6 @@ struct PhysParams { BodyJointsMax: i32, BodyCollidePairsN: i32, pad: i32, - pad1: i32, Gravity: vec4, } @@ -400,7 +400,7 @@ fn Cross3(v: vec3,o: vec3) -> vec3 { //////// import: "step_joint.go" fn StepJointForces(i: u32) { //gosl:kernel - let params = Params[0]; + var params = Params[0]; var ji = i32(i); if (ji >= params.JointsN) { return; @@ -470,4 +470,5 @@ fn StepJointForces(i: u32) { //gosl:kernel SetJointCForce(ji, f); SetJointPTorque(ji, t+(Cross3(dP, f))); SetJointCTorque(ji, t+(Cross3(dC, f))); + Params[0] = params; } \ No newline at end of file diff --git a/physics/shaders/StepSolveJoints.wgsl b/physics/shaders/StepSolveJoints.wgsl index 44f976d9..3258fe83 100644 --- a/physics/shaders/StepSolveJoints.wgsl +++ b/physics/shaders/StepSolveJoints.wgsl @@ -5,7 +5,7 @@ @group(0) @binding(0) var TensorStrides: array; @group(0) @binding(1) -var Params: array; +var Params: array; // // Bodies are the rigid body elements (dynamic and static), // specifying the constant, non-dynamic properties, // which is initial state for dynamics. // [body][BodyVarsN] @group(1) @binding(0) var Bodies: array; @@ -332,17 +332,18 @@ fn JointDoF(idx: i32,dof: i32, vr: JointDoFVars) -> f32 { struct PhysParams { Iterations: i32, Dt: f32, - SoftRelax: f32, + SubSteps: i32, + ContactMargin: f32, + ContactRelax: f32, + ContactWeighting: i32, + Restitution: i32, JointLinearRelax: f32, JointAngularRelax: f32, JointLinearComply: f32, JointAngularComply: f32, - ContactRelax: f32, AngularDamping: f32, - ContactWeighting: i32, - Restitution: i32, + SoftRelax: f32, MaxGeomIter: i32, - ContactMargin: f32, ContactsMax: i32, Cur: i32, Next: i32, @@ -353,7 +354,6 @@ struct PhysParams { BodyJointsMax: i32, BodyCollidePairsN: i32, pad: i32, - pad1: i32, Gravity: vec4, } @@ -523,7 +523,7 @@ fn SetDim3(v: vec3, dim: i32, val: f32) -> vec3 { //////// import: "step_joint.go" fn StepSolveJoints(i: u32) { //gosl:kernel - let params = Params[0]; + var params = Params[0]; var ji = i32(i); if (ji >= params.JointsN) { return; @@ -811,6 +811,7 @@ fn StepSolveJoints(i: u32) { //gosl:kernel SetJointPAngDelta(ji, angDeltaP); SetJointCDelta(ji, linDeltaC); SetJointCAngDelta(ji, angDeltaC); + Params[0] = params; } fn JointAxisTarget(axis: vec3, targ: f32,weight: f32, axisTargets: ptr>,axisWeights: ptr>) { var weightedAxis = axis*(weight); diff --git a/physics/step.go b/physics/step.go index 2dc29c7e..91e51b0d 100644 --- a/physics/step.go +++ b/physics/step.go @@ -19,6 +19,23 @@ func OneIfNonzero(f float32) float32 { return 0.0 } +// StepInit performs initialization at start of Step. +func StepInit(i uint32) { //gosl:kernel read-write:Params + if i > 0 { + return + } + params := GetParams(0) + BroadContactsN.Values[0] = 0 + ContactsN.Values[0] = 0 + if params.Cur == 0 { + params.Cur = 1 + params.Next = 0 + } else { + params.Cur = 0 + params.Next = 1 + } +} + // step does the following: // if self.compute_body_velocity_from_position_delta or self.enable_restitution: // // save initial state: @@ -48,24 +65,27 @@ func OneIfNonzero(f float32) float32 { //gosl:end -// Step runs one physics step and gets the dynamics vars back -// from the GPU. +// Step runs one physics step, sending Params and JointControls +// to the GPU, and getting the Dynamics state vars back. +// Each step has SubSteps integration sub-steps. func (wl *World) Step() { - wl.StepGet(DynamicsVar) + params := GetParams(0) + ToGPU(ParamsVar, JointControlsVar) + if params.SubSteps > 1 { + for range params.SubSteps - 1 { + wl.StepGet() + } + } + wl.StepGet(ParamsVar, DynamicsVar) + // wl.StepGet(ParamsVar, DynamicsVar, ContactsNVar) + // fmt.Println("contacts:", ContactsN.Value(0), "max:", params.ContactsMax) } // StepGet runs one physics step and gets the given vars back // from the GPU. func (wl *World) StepGet(vars ...GPUVars) { params := GetParams(0) - if params.Cur == 0 { - params.Cur = 1 - params.Next = 0 - } else { - params.Cur = 0 - params.Next = 1 - } - ToGPU(ParamsVar, JointControlsVar) + RunStepInit(1) wl.StepCollision() wl.StepJointForces() wl.StepIntegrateBodies() @@ -79,7 +99,6 @@ func (wl *World) StepGet(vars ...GPUVars) { func (wl *World) StepCollision() { params := GetParams(0) - RunCollisionInit(1) // could also just copy up RunCollisionBroad(int(params.BodyCollidePairsN)) // note: time getting BroadContactsN back down and using that vs. running full RunCollisionNarrow(int(params.ContactsMax)) diff --git a/physics/step.goal b/physics/step.goal index 74673a3f..1af562ab 100644 --- a/physics/step.goal +++ b/physics/step.goal @@ -17,6 +17,23 @@ func OneIfNonzero(f float32) float32 { return 0.0 } +// StepInit performs initialization at start of Step. +func StepInit(i uint32) { //gosl:kernel read-write:Params + if i > 0 { + return + } + params := GetParams(0) + BroadContactsN.Values[0] = 0 + ContactsN.Values[0] = 0 + if params.Cur == 0 { + params.Cur = 1 + params.Next = 0 + } else { + params.Cur = 0 + params.Next = 1 + } +} + // step does the following: // if self.compute_body_velocity_from_position_delta or self.enable_restitution: // // save initial state: @@ -46,24 +63,27 @@ func OneIfNonzero(f float32) float32 { //gosl:end -// Step runs one physics step and gets the dynamics vars back -// from the GPU. +// Step runs one physics step, sending Params and JointControls +// to the GPU, and getting the Dynamics state vars back. +// Each step has SubSteps integration sub-steps. func (wl *World) Step() { - wl.StepGet(DynamicsVar) + params := GetParams(0) + ToGPU(ParamsVar, JointControlsVar) + if params.SubSteps > 1 { + for range params.SubSteps - 1 { + wl.StepGet() + } + } + wl.StepGet(ParamsVar, DynamicsVar) + // wl.StepGet(ParamsVar, DynamicsVar, ContactsNVar) + // fmt.Println("contacts:", ContactsN.Value(0), "max:", params.ContactsMax) } // StepGet runs one physics step and gets the given vars back // from the GPU. func (wl *World) StepGet(vars ...GPUVars) { params := GetParams(0) - if params.Cur == 0 { - params.Cur = 1 - params.Next = 0 - } else { - params.Cur = 0 - params.Next = 1 - } - ToGPU(ParamsVar, JointControlsVar) + RunStepInit(1) wl.StepCollision() wl.StepJointForces() wl.StepIntegrateBodies() @@ -77,7 +97,6 @@ func (wl *World) StepGet(vars ...GPUVars) { func (wl *World) StepCollision() { params := GetParams(0) - RunCollisionInit(1) // could also just copy up RunCollisionBroad(int(params.BodyCollidePairsN)) // note: time getting BroadContactsN back down and using that vs. running full RunCollisionNarrow(int(params.ContactsMax)) diff --git a/physics/typegen.go b/physics/typegen.go index 82e7cdff..ae0b9953 100644 --- a/physics/typegen.go +++ b/physics/typegen.go @@ -24,7 +24,7 @@ var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.JointVars", var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.JointDoFVars", IDName: "joint-do-f-vars", Doc: "JointDoFVars are joint DoF state variables stored in tensor.Float32,\none for each DoF."}) -var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.PhysParams", IDName: "phys-params", Doc: "PhysParams are the physics parameters", Directives: []types.Directive{{Tool: "gosl", Directive: "start"}}, Fields: []types.Field{{Name: "Iterations", Doc: "Iterations is the number of iterations to perform."}, {Name: "Dt", Doc: "Dt is the integration stepsize."}, {Name: "SoftRelax", Doc: "SoftRelax is soft-body relaxation constant."}, {Name: "JointLinearRelax", Doc: "JointLinearRelax is joint linear relaxation constant."}, {Name: "JointAngularRelax", Doc: "JointAngularRelax is joint angular relaxation constant."}, {Name: "JointLinearComply", Doc: "JointLinearComply is joint linear compliance constant."}, {Name: "JointAngularComply", Doc: "JointAngularComply is joint angular compliance constant."}, {Name: "ContactRelax", Doc: "ContactRelax is rigid contact relaxation constant."}, {Name: "AngularDamping", Doc: "AngularDamping is damping of angular motion."}, {Name: "ContactWeighting", Doc: "Contact weighting: balances contact forces?"}, {Name: "Restitution", Doc: "Restitution takes into account bounciness of objects."}, {Name: "MaxGeomIter", Doc: "MaxGeomIter is number of iterations to perform in shape-based\ngeometry collision computations"}, {Name: "ContactMargin", Doc: "Contact margin is the extra distance for broadphase collision\naround rigid bodies."}, {Name: "ContactsMax", Doc: "Maximum number of contacts to process at any given point."}, {Name: "Cur", Doc: "Index for the current state (0 or 1, alternates with Next)."}, {Name: "Next", Doc: "Index for the next state (1 or 0, alternates with Cur)."}, {Name: "BodiesN", Doc: "BodiesN is number of rigid bodies."}, {Name: "DynamicsN", Doc: "DynamicsN is number of dynamics bodies."}, {Name: "JointsN", Doc: "JointsN is number of joints."}, {Name: "JointDoFsN", Doc: "JointDoFsN is number of joint DoFs."}, {Name: "BodyJointsMax", Doc: "BodyJointsMax is max number of joints per body + 1 for actual n."}, {Name: "BodyCollidePairsN", Doc: "BodyCollidePairsN is the total number of pre-compiled collision pairs\nto examine."}, {Name: "pad"}, {Name: "pad1"}, {Name: "Gravity", Doc: "Gravity is the gravity acceleration function"}}}) +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.PhysParams", IDName: "phys-params", Doc: "PhysParams are the physics parameters", Directives: []types.Directive{{Tool: "gosl", Directive: "start"}}, Fields: []types.Field{{Name: "Iterations", Doc: "Iterations is the number of integration iterations to perform\nwithin each solver step. Muller et al (2020) report that 1 is best."}, {Name: "Dt", Doc: "Dt is the integration stepsize.\nFor highly kinetic situations (e.g., rapidly moving bouncing balls)\n0.0001 is needed to ensure contact registration. Use SubSteps to\naccomplish a target effective read-out step size."}, {Name: "SubSteps", Doc: "SubSteps is the number of integration steps to take per Step()\nfunction call. These sub steps are taken without any sync to/from\nthe GPU and are therefore much faster."}, {Name: "ContactMargin", Doc: "Contact margin is the extra distance for broadphase collision\naround rigid bodies."}, {Name: "ContactRelax", Doc: "ContactRelax is rigid contact relaxation constant.\nHigher values cause errros"}, {Name: "ContactWeighting", Doc: "Contact weighting: balances contact forces?"}, {Name: "Restitution", Doc: "Restitution takes into account bounciness of objects."}, {Name: "JointLinearRelax", Doc: "JointLinearRelax is joint linear relaxation constant."}, {Name: "JointAngularRelax", Doc: "JointAngularRelax is joint angular relaxation constant."}, {Name: "JointLinearComply", Doc: "JointLinearComply is joint linear compliance constant."}, {Name: "JointAngularComply", Doc: "JointAngularComply is joint angular compliance constant."}, {Name: "AngularDamping", Doc: "AngularDamping is damping of angular motion."}, {Name: "SoftRelax", Doc: "SoftRelax is soft-body relaxation constant."}, {Name: "MaxGeomIter", Doc: "MaxGeomIter is number of iterations to perform in shape-based\ngeometry collision computations"}, {Name: "ContactsMax", Doc: "Maximum number of contacts to process at any given point."}, {Name: "Cur", Doc: "Index for the current state (0 or 1, alternates with Next)."}, {Name: "Next", Doc: "Index for the next state (1 or 0, alternates with Cur)."}, {Name: "BodiesN", Doc: "BodiesN is number of rigid bodies."}, {Name: "DynamicsN", Doc: "DynamicsN is number of dynamics bodies."}, {Name: "JointsN", Doc: "JointsN is number of joints."}, {Name: "JointDoFsN", Doc: "JointDoFsN is number of joint DoFs."}, {Name: "BodyJointsMax", Doc: "BodyJointsMax is max number of joints per body + 1 for actual n."}, {Name: "BodyCollidePairsN", Doc: "BodyCollidePairsN is the total number of pre-compiled collision pairs\nto examine."}, {Name: "pad"}, {Name: "Gravity", Doc: "Gravity is the gravity acceleration function"}}}) var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.GeomData", IDName: "geom-data", Doc: "GeomData contains all geometric data for narrow-phase collision.", Directives: []types.Directive{{Tool: "gosl", Directive: "start"}}, Fields: []types.Field{{Name: "BodyIdx"}, {Name: "Shape"}, {Name: "MinSize", Doc: "MinSize is the min of the Size dimensions."}, {Name: "Thick", Doc: "Thickness of shape."}, {Name: "Radius", Doc: "Radius is the effective radius for sphere-like elements (Sphere, Capsule, Cone)"}, {Name: "Size"}, {Name: "WbR", Doc: "World-to-Body transform\nPosition (R) (i.e., BodyPos)"}, {Name: "WbQ", Doc: "Quaternion (Q) (i.e., BodyQuat)"}, {Name: "BwR", Doc: "Body-to-World transform (inverse)\nPosition (R)"}, {Name: "BwQ", Doc: "Quaternion (Q)"}}}) diff --git a/physics/vars.go b/physics/vars.go index 390d0d9a..7f984246 100644 --- a/physics/vars.go +++ b/physics/vars.go @@ -17,7 +17,6 @@ import "cogentcore.org/lab/tensor" var ( // Params are global parameters. //gosl:group Params - //gosl:read-only Params []PhysParams // Bodies are the rigid body elements (dynamic and static), diff --git a/physics/world.go b/physics/world.go index fcafa30e..19569b52 100644 --- a/physics/world.go +++ b/physics/world.go @@ -81,11 +81,17 @@ func NewWorld() *World { return wl } -// Init makes initial vars. +// Init makes initial vars. Called in NewWorld. +// Must call Config once configured. func (wl *World) Init() { wl.GPU = true wl.Params = make([]PhysParams, 1) wl.Params[0].Defaults() + wl.Reset() +} + +// Reset resets all data to empty: starting over. +func (wl *World) Reset() { wl.Bodies = tensor.NewFloat32(0, int(BodyVarsN)) wl.Joints = tensor.NewFloat32(0, int(JointVarsN)) wl.JointDoFs = tensor.NewFloat32(0, int(JointDoFVarsN)) diff --git a/physics/world.goal b/physics/world.goal index acf31f53..0bd61b84 100644 --- a/physics/world.goal +++ b/physics/world.goal @@ -79,11 +79,17 @@ func NewWorld() *World { return wl } -// Init makes initial vars. +// Init makes initial vars. Called in NewWorld. +// Must call Config once configured. func (wl *World) Init() { wl.GPU = true wl.Params = make([]PhysParams, 1) wl.Params[0].Defaults() + wl.Reset() +} + +// Reset resets all data to empty: starting over. +func (wl *World) Reset() { wl.Bodies = tensor.NewFloat32(0, int(BodyVarsN)) wl.Joints = tensor.NewFloat32(0, int(JointVarsN)) wl.JointDoFs = tensor.NewFloat32(0, int(JointDoFVarsN)) diff --git a/physics/world/view.go b/physics/world/view.go index f61df6a2..baa313d5 100644 --- a/physics/world/view.go +++ b/physics/world/view.go @@ -153,7 +153,8 @@ func (vw *View) BoxInit(sld *xyz.Solid) { func (vw *View) PlaneInit(sld *xyz.Solid) { mnm := "physics.Plane" if ms, _ := sld.Scene.MeshByName(mnm); ms == nil { - xyz.NewPlane(sld.Scene, mnm, 1, 1) + pl := xyz.NewPlane(sld.Scene, mnm, 1, 1) + pl.Segs.Set(4, 4) } sld.SetMeshName(mnm) if vw.Size.X == 0 { @@ -216,3 +217,25 @@ func (vw *View) SphereInit(sld *xyz.Solid) { vw.UpdatePose(sld) }) } + +// SetBodyBounce specifies the COR or coefficient of restitution (0..1), +// which determines how elastic the collision is, +// i.e., final velocity / initial velocity. +func (vw *View) SetBodyBounce(val float32) { + physics.Bodies.Set(val, int(vw.Index), int(physics.BodyBounce)) +} + +// SetBodyFriction is the standard coefficient for linear friction (mu). +func (vw *View) SetBodyFriction(val float32) { + physics.Bodies.Set(val, int(vw.Index), int(physics.BodyFriction)) +} + +// SetBodyFrictionTortion is resistance to spinning at the contact point. +func (vw *View) SetBodyFrictionTortion(val float32) { + physics.Bodies.Set(val, int(vw.Index), int(physics.BodyFrictionTortion)) +} + +// SetBodyFrictionRolling is resistance to rolling motion at contact. +func (vw *View) SetBodyFrictionRolling(val float32) { + physics.Bodies.Set(val, int(vw.Index), int(physics.BodyFrictionRolling)) +} diff --git a/physics/world/world.go b/physics/world/world.go index 53ca2e4a..10c7f64a 100644 --- a/physics/world/world.go +++ b/physics/world/world.go @@ -58,6 +58,14 @@ func (wr *World) Init(wl *physics.World) { }) } +// Reset resets any existing views, starting fresh for a new configuration. +func (wr *World) Reset() { + wr.Views = nil + if wr.Scene != nil { + wr.Scene.Update() + } +} + // Update updates the xyz scene from current physics node state. // (use physics.World.SetAsCurrent()). func (wr *World) Update() { From 98765e2e8fabbdb728e0b129bb3dc0116e9dcea4 Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Tue, 23 Dec 2025 07:06:51 +0100 Subject: [PATCH 46/97] physics: update to xyz fix. balls runs on the web! except.. chrome somehow has different physics :( balls are more bouncy. --- go.mod | 2 +- go.sum | 4 ++-- physics/contact.go | 1 + physics/contact.goal | 1 + 4 files changed, 5 insertions(+), 3 deletions(-) diff --git a/go.mod b/go.mod index 83765ed5..79c9c110 100644 --- a/go.mod +++ b/go.mod @@ -10,7 +10,7 @@ go 1.23.4 // https://github.com/googleapis/go-genproto/issues/1015 require ( - cogentcore.org/core v0.3.13-0.20251219074447-aad9d251c7e7 + cogentcore.org/core v0.3.13-0.20251223055455-950754144c98 github.com/alecthomas/assert/v2 v2.6.0 github.com/cogentcore/readline v0.1.3 github.com/cogentcore/yaegi v0.0.0-20250622201820-b7838bdd95eb diff --git a/go.sum b/go.sum index 4d022928..0850edc2 100644 --- a/go.sum +++ b/go.sum @@ -1,5 +1,5 @@ -cogentcore.org/core v0.3.13-0.20251219074447-aad9d251c7e7 h1:cDlSzZUdONqsphHfrNuyyJN6lC0AQ9kGY4Idv4/HO24= -cogentcore.org/core v0.3.13-0.20251219074447-aad9d251c7e7/go.mod h1:eDHnTCy1sBhAKN9NPsSCnBW3VAnwQBNA9nbGMo9r+Xs= +cogentcore.org/core v0.3.13-0.20251223055455-950754144c98 h1:EMClYWakND3PL5OnsJsUhVuyG9A0JU6IU9iQYAeJk4k= +cogentcore.org/core v0.3.13-0.20251223055455-950754144c98/go.mod h1:eDHnTCy1sBhAKN9NPsSCnBW3VAnwQBNA9nbGMo9r+Xs= github.com/Bios-Marcel/wastebasket/v2 v2.0.3 h1:TkoDPcSqluhLGE+EssHu7UGmLgUEkWg7kNyHyyJ3Q9g= github.com/Bios-Marcel/wastebasket/v2 v2.0.3/go.mod h1:769oPCv6eH7ugl90DYIsWwjZh4hgNmMS3Zuhe1bH6KU= github.com/BurntSushi/toml v0.3.1/go.mod h1:xHWCNGjB5oqiDr8zfno3MHue2Ht5sIBksp03qcyfWMU= diff --git a/physics/contact.go b/physics/contact.go index 80074037..a400bcb5 100644 --- a/physics/contact.go +++ b/physics/contact.go @@ -810,6 +810,7 @@ func (wl *World) SetMaxContacts() { // colliding at once. Except.. if it is a very small model. if params.BodyCollidePairsN > 1000 { n = int32(math32.Sqrt(float32(n))) + n = max(n, params.DynamicsN) } params.ContactsMax = n wl.BroadContacts.SetShapeSizes(int(n), int(BroadContactVarsN)) diff --git a/physics/contact.goal b/physics/contact.goal index 0852e0bc..fad78e35 100644 --- a/physics/contact.goal +++ b/physics/contact.goal @@ -809,6 +809,7 @@ func (wl *World) SetMaxContacts() { // colliding at once. Except.. if it is a very small model. if params.BodyCollidePairsN > 1000 { n = int32(math32.Sqrt(float32(n))) + n = max(n, params.DynamicsN) } params.ContactsMax = n wl.BroadContacts.SetShapeSizes(int(n), int(BroadContactVarsN)) From ac43dec4a5ce68e833e551b88b44688b5644b9b3 Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Tue, 23 Dec 2025 18:53:41 +0100 Subject: [PATCH 47/97] physics: major fixes on joints: pendula example working --- physics/config.go | 2 +- physics/config.goal | 2 +- physics/contact.go | 40 ++- physics/contact.goal | 40 ++- physics/control.go | 32 +- physics/control.goal | 32 +- physics/dynamics.go | 6 +- physics/dynamics.goal | 8 +- physics/enumgen.go | 22 +- physics/examples/balls/balls.go | 14 +- physics/examples/pendula/pendula.go | 273 ++++++++++++++ physics/examples/test1/test1.go | 35 +- physics/gosl.go | 179 ++++------ physics/joint.go | 19 +- physics/joint.goal | 19 +- physics/params.go | 18 +- physics/shaders/CollisionBroad.wgsl | 28 +- physics/shaders/CollisionNarrow.wgsl | 28 +- physics/shaders/DeltasFromContacts.wgsl | 385 -------------------- physics/shaders/DeltasFromJoints.wgsl | 370 ------------------- physics/shaders/DynamicsCurToNext.wgsl | 28 +- physics/shaders/ForcesFromJoints.wgsl | 28 +- physics/shaders/InitDynamics.wgsl | 28 +- physics/shaders/StepBodyContacts.wgsl | 28 +- physics/shaders/StepBodyDeltas.wgsl | 435 ----------------------- physics/shaders/StepIntegrateBodies.wgsl | 28 +- physics/shaders/StepJointForces.wgsl | 40 ++- physics/shaders/StepSolveJoints.wgsl | 51 +-- physics/step.go | 6 +- physics/step.goal | 6 +- physics/step_body.go | 81 ++--- physics/step_body.goal | 81 ++--- physics/step_joint.go | 38 +- physics/step_joint.goal | 38 +- 34 files changed, 798 insertions(+), 1670 deletions(-) create mode 100644 physics/examples/pendula/pendula.go delete mode 100644 physics/shaders/DeltasFromContacts.wgsl delete mode 100644 physics/shaders/DeltasFromJoints.wgsl delete mode 100644 physics/shaders/StepBodyDeltas.wgsl diff --git a/physics/config.go b/physics/config.go index 5cbc9756..8c691649 100644 --- a/physics/config.go +++ b/physics/config.go @@ -54,7 +54,7 @@ func (wl *World) ConfigJoints() { if maxi == 0 { maxi = 1 } - wl.BodyJoints.SetShapeSizes(int(nd), 2, maxi) + wl.BodyJoints.SetShapeSizes(int(nd), 2, maxi+1) for di := range nd { np := int32(len(bjp[di])) wl.BodyJoints.Set(np, int(di), int(0), int(0)) diff --git a/physics/config.goal b/physics/config.goal index 34609d31..e26cc841 100644 --- a/physics/config.goal +++ b/physics/config.goal @@ -52,7 +52,7 @@ func (wl *World) ConfigJoints() { if maxi == 0 { maxi = 1 } - wl.BodyJoints.SetShapeSizes(int(nd), 2, maxi) + wl.BodyJoints.SetShapeSizes(int(nd), 2, maxi+1) for di := range nd { np := int32(len(bjp[di])) wl.BodyJoints[di, 0, 0] = np diff --git a/physics/contact.go b/physics/contact.go index a400bcb5..b59343b7 100644 --- a/physics/contact.go +++ b/physics/contact.go @@ -638,23 +638,27 @@ func StepBodyContacts(i uint32) { //gosl:kernel SetContactBAngDelta(ci, angDeltaB) } -// DeltasFromContacts gathers deltas, angDeltas from contacts per dynamic. -func DeltasFromContacts(i uint32) { //gosl:kernel +// StepBodyContactDeltas gathers raw deltas, angDeltas from contacts per dynamic +// and computes updated deltas integrated via StepBodyDeltas. +func StepBodyContactDeltas(i uint32) { //gosl:kernel params := GetParams(0) di := int32(i) if di >= params.DynamicsN { return } - cmax := ContactsN.Values[0] - bi := DynamicBody(di) + invMass := Bodies.Value(int(bi), int(BodyInvMass)) + if invMass == 0 { + return // no updates + } + cmax := ContactsN.Values[0] - td := math32.Vec3(0, 0, 0) - ta := math32.Vec3(0, 0, 0) + linDel := math32.Vec3(0, 0, 0) + angDel := math32.Vec3(0, 0, 0) tw := float32(0) for ci := range cmax { wt := Contacts.Value(int(ci), int(ContactWeight)) - if wt == 0 { + if wt == 0 { // 0 = no actual; else 1 continue } biA := GetContactA(ci) @@ -662,25 +666,20 @@ func DeltasFromContacts(i uint32) { //gosl:kernel if biA == bi { tw += wt d := ContactADelta(ci) - td = td.Add(d) + linDel = linDel.Add(d) a := ContactAAngDelta(ci) - ta = ta.Add(a) + angDel = angDel.Add(a) } if biB == bi { tw += wt d := ContactBDelta(ci) - td = td.Add(d) + linDel = linDel.Add(d) a := ContactBAngDelta(ci) - ta = ta.Add(a) + angDel = angDel.Add(a) } } - v0 := DynamicDelta(di, params.Next) - w0 := DynamicAngDelta(di, params.Next) - // fmt.Println(params.Next, "contact:", v0, td) - - SetDynamicDelta(di, params.Next, td.Add(v0)) - SetDynamicAngDelta(di, params.Next, ta.Add(w0)) Dynamics.Set(tw, int(di), int(params.Next), int(DynContactWeight)) + StepBodyDeltas(di, bi, true, tw, linDel, angDel) } func ContactConstraint(err float32, q0A, q0B math32.Quat, mInvA, mInvB float32, iInvA, iInvB math32.Matrix3, linA, linB, angA, angB math32.Vector3, relaxation, dt float32) float32 { @@ -752,7 +751,7 @@ func (wl *World) ConfigBodyCollidePairs() { } dib := GetBodyDynamic(b) // now check joints (ConfigJoints must have been called first) - if wl.IsChildDynamic(dia, dib) { + if wl.IsChildDynamic(dia, dib) || wl.IsChildDynamic(dib, dia) { continue } if np >= nalc { @@ -774,6 +773,9 @@ func (wl *World) ConfigBodyCollidePairs() { } } params.BodyCollidePairsN = int32(np) + if np == 0 { + np = 1 + } pt.SetShapeSizes(np, 2) wl.BodyCollidePairs = pt BodyCollidePairs = pt @@ -810,8 +812,8 @@ func (wl *World) SetMaxContacts() { // colliding at once. Except.. if it is a very small model. if params.BodyCollidePairsN > 1000 { n = int32(math32.Sqrt(float32(n))) - n = max(n, params.DynamicsN) } + n = max(n, params.DynamicsN) params.ContactsMax = n wl.BroadContacts.SetShapeSizes(int(n), int(BroadContactVarsN)) wl.Contacts.SetShapeSizes(int(n), int(ContactVarsN)) diff --git a/physics/contact.goal b/physics/contact.goal index fad78e35..b7f6ff5f 100644 --- a/physics/contact.goal +++ b/physics/contact.goal @@ -637,23 +637,27 @@ func StepBodyContacts(i uint32) { //gosl:kernel SetContactBAngDelta(ci, angDeltaB) } -// DeltasFromContacts gathers deltas, angDeltas from contacts per dynamic. -func DeltasFromContacts(i uint32) { //gosl:kernel +// StepBodyContactDeltas gathers raw deltas, angDeltas from contacts per dynamic +// and computes updated deltas integrated via StepBodyDeltas. +func StepBodyContactDeltas(i uint32) { //gosl:kernel params := GetParams(0) di := int32(i) if di >= params.DynamicsN { return } + bi := DynamicBody(di) + invMass := Bodies[bi, BodyInvMass] + if invMass == 0 { + return // no updates + } cmax := ContactsN.Values[0] - bi := DynamicBody(di) - - td := math32.Vec3(0,0,0) - ta := math32.Vec3(0,0,0) + linDel := math32.Vec3(0,0,0) + angDel := math32.Vec3(0,0,0) tw := float32(0) for ci := range cmax { wt := Contacts[ci, ContactWeight] - if wt == 0 { + if wt == 0 { // 0 = no actual; else 1 continue } biA := GetContactA(ci) @@ -661,25 +665,20 @@ func DeltasFromContacts(i uint32) { //gosl:kernel if biA == bi { tw += wt d := ContactADelta(ci) - td = td.Add(d) + linDel = linDel.Add(d) a := ContactAAngDelta(ci) - ta = ta.Add(a) + angDel = angDel.Add(a) } if biB == bi { tw += wt d := ContactBDelta(ci) - td = td.Add(d) + linDel = linDel.Add(d) a := ContactBAngDelta(ci) - ta = ta.Add(a) + angDel = angDel.Add(a) } } - v0 := DynamicDelta(di, params.Next) - w0 := DynamicAngDelta(di, params.Next) - // fmt.Println(params.Next, "contact:", v0, td) - - SetDynamicDelta(di, params.Next, td.Add(v0)) - SetDynamicAngDelta(di, params.Next, ta.Add(w0)) Dynamics[di, params.Next, DynContactWeight] = tw + StepBodyDeltas(di, bi, true, tw, linDel, angDel) } func ContactConstraint(err float32, q0A, q0B math32.Quat, mInvA, mInvB float32, iInvA, iInvB math32.Matrix3, linA, linB, angA, angB math32.Vector3, relaxation, dt float32) float32 { @@ -751,7 +750,7 @@ func (wl *World) ConfigBodyCollidePairs() { } dib := GetBodyDynamic(b) // now check joints (ConfigJoints must have been called first) - if wl.IsChildDynamic(dia, dib) { + if wl.IsChildDynamic(dia, dib) || wl.IsChildDynamic(dib, dia) { continue } if np >= nalc { @@ -773,6 +772,9 @@ func (wl *World) ConfigBodyCollidePairs() { } } params.BodyCollidePairsN = int32(np) + if np == 0 { + np = 1 + } pt.SetShapeSizes(np, 2) wl.BodyCollidePairs = pt BodyCollidePairs = pt @@ -809,8 +811,8 @@ func (wl *World) SetMaxContacts() { // colliding at once. Except.. if it is a very small model. if params.BodyCollidePairsN > 1000 { n = int32(math32.Sqrt(float32(n))) - n = max(n, params.DynamicsN) } + n = max(n, params.DynamicsN) params.ContactsMax = n wl.BroadContacts.SetShapeSizes(int(n), int(BroadContactVarsN)) wl.Contacts.SetShapeSizes(int(n), int(ContactVarsN)) diff --git a/physics/control.go b/physics/control.go index 66246dfd..19426757 100644 --- a/physics/control.go +++ b/physics/control.go @@ -15,7 +15,19 @@ type JointControlVars int32 //enums:enum const ( // Joint force and torque inputs JointControlForce JointControlVars = iota + + // Joint target position settings: the stiffness parameter determines + // how strongly the target position target is enforced: + // 0 = not at all; larger = stronger (e.g., 1000 or higher). + // Set to 0 to allow the joint to be fully flexible. + JointTargetStiff JointTargetPos + + // Joint target velocity settings: the damping parameter determines + // how strongly the target velocity target is enforced: + // 0 = not at all; larger = stronger (e.g., 1 is reasonable). + // Set to 0 to allow the joint to be fully flexible. + JointTargetDamp JointTargetVel ) @@ -34,14 +46,22 @@ func SetJointControlForce(idx, dof int32, value float32) { SetJointControl(idx, dof, JointControlForce, value) } -// SetJointTargetPos sets the target position for given joint, dof to given value. -func SetJointTargetPos(idx, dof int32, value float32) { - SetJointControl(idx, dof, JointTargetPos, value) +// SetJointTargetPos sets the target position and stiffness +// for given joint, DoF to given values. Stiffness determines +// how strongly the joint constraint is enforced +// (0 = not at all; 1 = as strongly as possible). +func SetJointTargetPos(idx, dof int32, pos, stiff float32) { + SetJointControl(idx, dof, JointTargetPos, pos) + SetJointControl(idx, dof, JointTargetStiff, stiff) } -// SetJointTargetVel sets the target velocity for given joint, dof to given value. -func SetJointTargetVel(idx, dof int32, value float32) { - SetJointControl(idx, dof, JointTargetVel, value) +// SetJointTargetVel sets the target velocity and damping +// for given joint, DoF to given values. Damping determines +// how strongly the joint constraint is enforced +// (0 = not at all; 1 = as strongly as possible). +func SetJointTargetVel(idx, dof int32, vel, damp float32) { + SetJointControl(idx, dof, JointTargetVel, vel) + SetJointControl(idx, dof, JointTargetDamp, damp) } //gosl:end diff --git a/physics/control.goal b/physics/control.goal index 50f86c15..97920d11 100644 --- a/physics/control.goal +++ b/physics/control.goal @@ -13,7 +13,19 @@ type JointControlVars int32 //enums:enum const ( // Joint force and torque inputs JointControlForce JointControlVars = iota + + // Joint target position settings: the stiffness parameter determines + // how strongly the target position target is enforced: + // 0 = not at all; larger = stronger (e.g., 1000 or higher). + // Set to 0 to allow the joint to be fully flexible. + JointTargetStiff JointTargetPos + + // Joint target velocity settings: the damping parameter determines + // how strongly the target velocity target is enforced: + // 0 = not at all; larger = stronger (e.g., 1 is reasonable). + // Set to 0 to allow the joint to be fully flexible. + JointTargetDamp JointTargetVel ) @@ -32,14 +44,22 @@ func SetJointControlForce(idx, dof int32, value float32) { SetJointControl(idx, dof, JointControlForce, value) } -// SetJointTargetPos sets the target position for given joint, dof to given value. -func SetJointTargetPos(idx, dof int32, value float32) { - SetJointControl(idx, dof, JointTargetPos, value) +// SetJointTargetPos sets the target position and stiffness +// for given joint, DoF to given values. Stiffness determines +// how strongly the joint constraint is enforced +// (0 = not at all; 1 = as strongly as possible). +func SetJointTargetPos(idx, dof int32, pos, stiff float32) { + SetJointControl(idx, dof, JointTargetPos, pos) + SetJointControl(idx, dof, JointTargetStiff, stiff) } -// SetJointTargetVel sets the target velocity for given joint, dof to given value. -func SetJointTargetVel(idx, dof int32, value float32) { - SetJointControl(idx, dof, JointTargetVel, value) +// SetJointTargetVel sets the target velocity and damping +// for given joint, DoF to given values. Damping determines +// how strongly the joint constraint is enforced +// (0 = not at all; 1 = as strongly as possible). +func SetJointTargetVel(idx, dof int32, vel, damp float32) { + SetJointControl(idx, dof, JointTargetVel, vel) + SetJointControl(idx, dof, JointTargetDamp, damp) } //gosl:end diff --git a/physics/dynamics.go b/physics/dynamics.go index 5abfe9a9..0d77dfbb 100644 --- a/physics/dynamics.go +++ b/physics/dynamics.go @@ -62,12 +62,12 @@ const ( DynTorqueY DynTorqueZ - // Linear deltas. + // Linear deltas. These accumulate over time via StepBodyDeltas. DynDeltaX DynDeltaY DynDeltaZ - // Angular deltas. + // Angular deltas. These accumulate over time via StepBodyDeltas. DynAngDeltaX DynAngDeltaY DynAngDeltaZ @@ -169,6 +169,8 @@ func SetDynamicAngAcc(idx, cni int32, angAcc math32.Vector3) { Dynamics.Set(angAcc.Z, int(idx), int(cni), int(DynAngAccZ)) } +//////// Accumulating deltas + func DynamicDelta(idx, cni int32) math32.Vector3 { return math32.Vec3(Dynamics.Value(int(idx), int(cni), int(DynDeltaX)), Dynamics.Value(int(idx), int(cni), int(DynDeltaY)), Dynamics.Value(int(idx), int(cni), int(DynDeltaZ))) } diff --git a/physics/dynamics.goal b/physics/dynamics.goal index 4c4823f3..4faa4bf7 100644 --- a/physics/dynamics.goal +++ b/physics/dynamics.goal @@ -60,16 +60,16 @@ const ( DynTorqueY DynTorqueZ - // Linear deltas. + // Linear deltas. These accumulate over time via StepBodyDeltas. DynDeltaX DynDeltaY DynDeltaZ - // Angular deltas. + // Angular deltas. These accumulate over time via StepBodyDeltas. DynAngDeltaX DynAngDeltaY DynAngDeltaZ - + // integrated weight of all contacts DynContactWeight ) @@ -167,6 +167,8 @@ func SetDynamicAngAcc(idx, cni int32, angAcc math32.Vector3) { Dynamics[idx, cni, DynAngAccZ] = angAcc.Z } +//////// Accumulating deltas + func DynamicDelta(idx, cni int32) math32.Vector3 { return math32.Vec3(Dynamics[idx, cni, DynDeltaX], Dynamics[idx, cni, DynDeltaY], Dynamics[idx, cni, DynDeltaZ]) } diff --git a/physics/enumgen.go b/physics/enumgen.go index 8e733163..8dbd4d46 100644 --- a/physics/enumgen.go +++ b/physics/enumgen.go @@ -98,20 +98,20 @@ func (i *ContactVars) UnmarshalText(text []byte) error { return enums.UnmarshalText(i, text, "ContactVars") } -var _JointControlVarsValues = []JointControlVars{0, 1, 2} +var _JointControlVarsValues = []JointControlVars{0, 1, 2, 3, 4} // JointControlVarsN is the highest valid value for type JointControlVars, plus one. // //gosl:start -const JointControlVarsN JointControlVars = 3 +const JointControlVarsN JointControlVars = 5 //gosl:end -var _JointControlVarsValueMap = map[string]JointControlVars{`JointControlForce`: 0, `JointTargetPos`: 1, `JointTargetVel`: 2} +var _JointControlVarsValueMap = map[string]JointControlVars{`JointControlForce`: 0, `JointTargetStiff`: 1, `JointTargetPos`: 2, `JointTargetDamp`: 3, `JointTargetVel`: 4} -var _JointControlVarsDescMap = map[JointControlVars]string{0: `Joint force and torque inputs`, 1: ``, 2: ``} +var _JointControlVarsDescMap = map[JointControlVars]string{0: `Joint force and torque inputs`, 1: `Joint target position settings: the stiffness parameter determines how strongly the target position target is enforced: 0 = not at all; larger = stronger (e.g., 1000 or higher). Set to 0 to allow the joint to be fully flexible.`, 2: ``, 3: `Joint target velocity settings: the damping parameter determines how strongly the target velocity target is enforced: 0 = not at all; larger = stronger (e.g., 1 is reasonable). Set to 0 to allow the joint to be fully flexible.`, 4: ``} -var _JointControlVarsMap = map[JointControlVars]string{0: `JointControlForce`, 1: `JointTargetPos`, 2: `JointTargetVel`} +var _JointControlVarsMap = map[JointControlVars]string{0: `JointControlForce`, 1: `JointTargetStiff`, 2: `JointTargetPos`, 3: `JointTargetDamp`, 4: `JointTargetVel`} // String returns the string representation of this JointControlVars value. func (i JointControlVars) String() string { return enums.String(i, _JointControlVarsMap) } @@ -156,7 +156,7 @@ const DynamicVarsN DynamicVars = 33 var _DynamicVarsValueMap = map[string]DynamicVars{`DynBody`: 0, `DynPosX`: 1, `DynPosY`: 2, `DynPosZ`: 3, `DynQuatX`: 4, `DynQuatY`: 5, `DynQuatZ`: 6, `DynQuatW`: 7, `DynVelX`: 8, `DynVelY`: 9, `DynVelZ`: 10, `DynAngVelX`: 11, `DynAngVelY`: 12, `DynAngVelZ`: 13, `DynAccX`: 14, `DynAccY`: 15, `DynAccZ`: 16, `DynAngAccX`: 17, `DynAngAccY`: 18, `DynAngAccZ`: 19, `DynForceX`: 20, `DynForceY`: 21, `DynForceZ`: 22, `DynTorqueX`: 23, `DynTorqueY`: 24, `DynTorqueZ`: 25, `DynDeltaX`: 26, `DynDeltaY`: 27, `DynDeltaZ`: 28, `DynAngDeltaX`: 29, `DynAngDeltaY`: 30, `DynAngDeltaZ`: 31, `DynContactWeight`: 32} -var _DynamicVarsDescMap = map[DynamicVars]string{0: `Index of body in list of bodies.`, 1: `3D position of structural center.`, 2: ``, 3: ``, 4: `Quaternion rotation.`, 5: ``, 6: ``, 7: ``, 8: `Linear velocity.`, 9: ``, 10: ``, 11: `Angular velocity.`, 12: ``, 13: ``, 14: `Linear acceleration.`, 15: ``, 16: ``, 17: `Angular acceleration due to applied torques.`, 18: ``, 19: ``, 20: `Linear force driving linear acceleration (from joints, etc).`, 21: ``, 22: ``, 23: `Torque driving angular acceleration (from joints, etc).`, 24: ``, 25: ``, 26: `Linear deltas.`, 27: ``, 28: ``, 29: `Angular deltas.`, 30: ``, 31: ``, 32: `integrated weight of all contacts`} +var _DynamicVarsDescMap = map[DynamicVars]string{0: `Index of body in list of bodies.`, 1: `3D position of structural center.`, 2: ``, 3: ``, 4: `Quaternion rotation.`, 5: ``, 6: ``, 7: ``, 8: `Linear velocity.`, 9: ``, 10: ``, 11: `Angular velocity.`, 12: ``, 13: ``, 14: `Linear acceleration.`, 15: ``, 16: ``, 17: `Angular acceleration due to applied torques.`, 18: ``, 19: ``, 20: `Linear force driving linear acceleration (from joints, etc).`, 21: ``, 22: ``, 23: `Torque driving angular acceleration (from joints, etc).`, 24: ``, 25: ``, 26: `Linear deltas. These accumulate over time via StepBodyDeltas.`, 27: ``, 28: ``, 29: `Angular deltas. These accumulate over time via StepBodyDeltas.`, 30: ``, 31: ``, 32: `integrated weight of all contacts`} var _DynamicVarsMap = map[DynamicVars]string{0: `DynBody`, 1: `DynPosX`, 2: `DynPosY`, 3: `DynPosZ`, 4: `DynQuatX`, 5: `DynQuatY`, 6: `DynQuatZ`, 7: `DynQuatW`, 8: `DynVelX`, 9: `DynVelY`, 10: `DynVelZ`, 11: `DynAngVelX`, 12: `DynAngVelY`, 13: `DynAngVelZ`, 14: `DynAccX`, 15: `DynAccY`, 16: `DynAccZ`, 17: `DynAngAccX`, 18: `DynAngAccY`, 19: `DynAngAccZ`, 20: `DynForceX`, 21: `DynForceY`, 22: `DynForceZ`, 23: `DynTorqueX`, 24: `DynTorqueY`, 25: `DynTorqueZ`, 26: `DynDeltaX`, 27: `DynDeltaY`, 28: `DynDeltaZ`, 29: `DynAngDeltaX`, 30: `DynAngDeltaY`, 31: `DynAngDeltaZ`, 32: `DynContactWeight`} @@ -331,20 +331,20 @@ func (i *JointVars) UnmarshalText(text []byte) error { return enums.UnmarshalText(i, text, "JointVars") } -var _JointDoFVarsValues = []JointDoFVars{0, 1, 2, 3, 4, 5, 6} +var _JointDoFVarsValues = []JointDoFVars{0, 1, 2, 3, 4} // JointDoFVarsN is the highest valid value for type JointDoFVars, plus one. // //gosl:start -const JointDoFVarsN JointDoFVars = 7 +const JointDoFVarsN JointDoFVars = 5 //gosl:end -var _JointDoFVarsValueMap = map[string]JointDoFVars{`JointAxisX`: 0, `JointAxisY`: 1, `JointAxisZ`: 2, `JointLimitLower`: 3, `JointLimitUpper`: 4, `JointStiff`: 5, `JointDamp`: 6} +var _JointDoFVarsValueMap = map[string]JointDoFVars{`JointAxisX`: 0, `JointAxisY`: 1, `JointAxisZ`: 2, `JointLimitLower`: 3, `JointLimitUpper`: 4} -var _JointDoFVarsDescMap = map[JointDoFVars]string{0: `axis of articulation for the DoF`, 1: ``, 2: ``, 3: `joint limits`, 4: ``, 5: `joint stiffness target (ke)`, 6: `joint damping target (kd)`} +var _JointDoFVarsDescMap = map[JointDoFVars]string{0: `axis of articulation for the DoF`, 1: ``, 2: ``, 3: `joint limits`, 4: ``} -var _JointDoFVarsMap = map[JointDoFVars]string{0: `JointAxisX`, 1: `JointAxisY`, 2: `JointAxisZ`, 3: `JointLimitLower`, 4: `JointLimitUpper`, 5: `JointStiff`, 6: `JointDamp`} +var _JointDoFVarsMap = map[JointDoFVars]string{0: `JointAxisX`, 1: `JointAxisY`, 2: `JointAxisZ`, 3: `JointLimitLower`, 4: `JointLimitUpper`} // String returns the string representation of this JointDoFVars value. func (i JointDoFVars) String() string { return enums.String(i, _JointDoFVarsMap) } diff --git a/physics/examples/balls/balls.go b/physics/examples/balls/balls.go index e612e3e1..e39df89e 100644 --- a/physics/examples/balls/balls.go +++ b/physics/examples/balls/balls.go @@ -138,15 +138,14 @@ func main() { box := bs.Width * .9 size := bs.Size for i := range bs.NBalls { - _ = i ht := rand.Float32() * bs.Height x := rand.Float32()*box - 0.5*box z := rand.Float32()*box - 0.5*box clr := colors.Names[i%len(colors.Names)] - bl := wr.NewDynamic(wl, "body", physics.Sphere, clr, bs.Mass, math32.Vec3(size, size, size), + bl := wr.NewDynamic(wl, "ball", physics.Sphere, clr, bs.Mass, math32.Vec3(size, size, size), math32.Vec3(x, size+ht, z), rot) if !bs.Collide { - physics.SetBodyGroup(bl.Index, int32(i)) // only collide within same group + physics.SetBodyGroup(bl.Index, int32(i+1)) // only collide within same group } bl.SetBodyBounce(bs.Bounce) bl.SetBodyFriction(bs.Friction) @@ -159,6 +158,8 @@ func main() { config() + cycle := 0 + updateView := func() { bpf.Update() wpf.Update() @@ -196,6 +197,7 @@ func main() { isRunning = true for range n { wl.Step() + cycle++ wr.Update() if se.IsVisible() { se.AsyncLock() @@ -223,6 +225,9 @@ func main() { w.SetText("Init").SetIcon(icons.Reset). SetTooltip("Reset physics state back to starting."). OnClick(func(e events.Event) { + if isRunning { + return + } wl.InitState() wr.Update() updateView() @@ -248,6 +253,9 @@ func main() { w.SetText("Rebuild").SetIcon(icons.Reset). SetTooltip("Rebuild the environment, when you change parameters"). OnClick(func(e events.Event) { + if isRunning { + return + } config() updateView() }) diff --git a/physics/examples/pendula/pendula.go b/physics/examples/pendula/pendula.go new file mode 100644 index 00000000..bc45cdd5 --- /dev/null +++ b/physics/examples/pendula/pendula.go @@ -0,0 +1,273 @@ +// Copyright (c) 2019, Cogent Core. All rights reserved. +// Use of this source code is governed by a BSD-style +// license that can be found in the LICENSE file. + +package main + +//go:generate core generate -add-types + +import ( + "fmt" + + "cogentcore.org/core/colors" + "cogentcore.org/core/core" + "cogentcore.org/core/events" + "cogentcore.org/core/icons" + "cogentcore.org/core/math32" + "cogentcore.org/core/styles" + "cogentcore.org/core/styles/abilities" + "cogentcore.org/core/tree" + "cogentcore.org/core/xyz" + "cogentcore.org/core/xyz/xyzcore" + _ "cogentcore.org/lab/gosl/slbool/slboolcore" // include to get gui views + "cogentcore.org/lab/physics" + "cogentcore.org/lab/physics/world" +) + +// Pendula has sim params +type Pendula struct { + + // Number of balls: if collide, then run out of memory above 1000 or so + NPendula int + + ForceOn int + ForceOff int + Force float32 + + HSize math32.Vector3 + + // Mass of each ball (kg) + Mass float32 + + // do the elements collide with each other? + Collide bool + + // Stiff is the strength of positional constraints + // when imposing them. + Stiff float32 + + // Damp is the strength of velocity constraints + // when imposing them. + Damp float32 +} + +func (b *Pendula) Defaults() { + b.NPendula = 2 + b.HSize.Set(0.05, .2, 0.05) + b.Mass = 0.1 + + b.ForceOn = 100 + b.ForceOff = 102 + b.Force = 0 + + b.Damp = 0.5 + b.Stiff = 1e4 +} + +func main() { + // gpu.Debug = true + b := core.NewBody("test1").SetTitle("Physics Pendula") + split := core.NewSplits(b) + fpanel := core.NewFrame(split) + fpanel.Styler(func(s *styles.Style) { + s.Direction = styles.Column + s.Grow.Set(1, 1) + }) + + bpf := core.NewForm(fpanel) + wpf := core.NewForm(fpanel) + + tbvw := core.NewTabs(split) + scfr, _ := tbvw.NewTab("3D View") + split.SetSplits(0.2, 0.8) + + se := xyzcore.NewSceneEditor(scfr) + se.UpdateWidget() + sc := se.SceneXYZ() + + sc.Background = colors.Scheme.Select.Container + xyz.NewAmbient(sc, "ambient", 0.3, xyz.DirectSun) + + dir := xyz.NewDirectional(sc, "dir", 1, xyz.DirectSun) + dir.Pos.Set(0, 2, 1) + + wr := world.NewWorld(sc) + + ps := &Pendula{} + ps.Defaults() + + wl := physics.NewWorld() + wl.GPU = true + + params := physics.GetParams(0) + params.Dt = 0.0001 // leaks balls > 0.0005 + params.SubSteps = 100 // major speedup by inner-stepping + // params.Gravity.Y = 0 + params.Restitution.SetBool(false) // not working! + params.ContactMargin = 0.1 // 0.1 better than .01 -- leaks a few + + bpf.SetStruct(ps) + wpf.SetStruct(params) + + var topJoint int32 + + config := func() { + wr.Reset() + wl.Reset() + rot := math32.NewQuat(0, 0, 0, 1) + _ = rot + rleft := math32.NewQuatAxisAngle(math32.Vec3(0, 0, 1), -math32.Pi/2) + _ = rleft + stY := 2*ps.HSize.Y*float32(ps.NPendula+1) + 1 + clr := colors.Names[0] + pb := wr.NewDynamic(wl, "top", physics.Box, clr, ps.Mass, ps.HSize, + math32.Vec3(-ps.HSize.Y, stY, 0), rleft) + if !ps.Collide { + physics.SetBodyGroup(pb.Index, int32(1)) + } + + ji := wl.NewJointRevolute(-1, pb.DynamicIndex, math32.Vec3(0, stY, 0), math32.Vec3(0, ps.HSize.Y, 0), math32.Vec3(0, 0, 1)) + // physics.SetJointTargetPos(ji, 0, math32.Pi/2, 1) // let it swing! + physics.SetJointTargetPos(ji, 0, 0, 0) // let it swing! + physics.SetJointTargetVel(ji, 0, 0, 0) // let it swing! + + topJoint = ji + + for i := 1; i < ps.NPendula; i++ { + clr := colors.Names[i%len(colors.Names)] + x := -float32(i)*ps.HSize.Y*2 - ps.HSize.Y + cb := wr.NewDynamic(wl, "child", physics.Box, clr, ps.Mass, ps.HSize, + math32.Vec3(x, stY, 0), rleft) + if !ps.Collide { + physics.SetBodyGroup(cb.Index, int32(1+i)) + } + ji = wl.NewJointRevolute(pb.DynamicIndex, cb.DynamicIndex, math32.Vec3(0, -ps.HSize.Y, 0), math32.Vec3(0, ps.HSize.Y, 0), math32.Vec3(0, 0, 1)) + physics.SetJointTargetPos(ji, 0, 0, 0) // let it swing! + physics.SetJointTargetVel(ji, 0, 0, 0) // let it swing! + pb = cb + } + wr.Init(wl) + wr.Update() + } + + config() + + cycle := 0 + + updateView := func() { + bpf.Update() + wpf.Update() + if se.IsVisible() { + se.NeedsRender() + } + } + + sc.Camera.Pose.Pos = math32.Vec3(0, 40, 3.5) + sc.Camera.LookAt(math32.Vec3(0, 5, 0), math32.Vec3(0, 1, 0)) + sc.SaveCamera("3") + + sc.Camera.Pose.Pos = math32.Vec3(-1.33, 2.24, 3.55) + sc.Camera.LookAt(math32.Vec3(0, .5, 0), math32.Vec3(0, 1, 0)) + sc.SaveCamera("2") + + sc.Camera.Pose.Pos = math32.Vec3(0, 6, 4.5) + sc.Camera.LookAt(math32.Vec3(0, 3, 0), math32.Vec3(0, 1, 0)) + sc.SaveCamera("1") + sc.SaveCamera("default") + + isRunning := false + stop := false + + stepNButton := func(p *tree.Plan, n int) { + nm := fmt.Sprintf("Step %d", n) + tree.AddAt(p, nm, func(w *core.Button) { + w.SetText(nm).SetIcon(icons.PlayArrow). + SetTooltip(fmt.Sprintf("Step state %d times", n)). + OnClick(func(e events.Event) { + if isRunning { + fmt.Println("still running...") + return + } + go func() { + isRunning = true + for range n { + wl.Step() + cycle++ + if cycle >= ps.ForceOn && cycle < ps.ForceOff { + fmt.Println(cycle, "\tforce on:", ps.Force) + physics.SetJointControlForce(topJoint, 0, ps.Force) + } else { + physics.SetJointControlForce(topJoint, 0, 0) + } + wr.Update() + if se.IsVisible() { + se.AsyncLock() + se.NeedsRender() + se.AsyncUnlock() + // time.Sleep(1 * time.Nanosecond) + } + if stop { + stop = false + break + } + } + isRunning = false + }() + }) + w.Styler(func(s *styles.Style) { + s.SetAbilities(true, abilities.RepeatClickable) + }) + }) + } + + b.AddTopBar(func(bar *core.Frame) { + core.NewToolbar(bar).Maker(func(p *tree.Plan) { + tree.Add(p, func(w *core.Button) { + w.SetText("Init").SetIcon(icons.Reset). + SetTooltip("Reset physics state back to starting."). + OnClick(func(e events.Event) { + if isRunning { + fmt.Println("still running...") + return + } + stop = false + cycle = 0 + wl.InitState() + wr.Update() + updateView() + }) + }) + tree.Add(p, func(w *core.Button) { + w.SetText("Stop").SetIcon(icons.Stop). + SetTooltip("Stop running"). + OnClick(func(e events.Event) { + stop = true + }) + }) + tree.Add(p, func(w *core.Separator) {}) + + stepNButton(p, 1) + stepNButton(p, 10) + stepNButton(p, 100) + stepNButton(p, 1000) + stepNButton(p, 10000) + tree.Add(p, func(w *core.Separator) {}) + + tree.Add(p, func(w *core.Button) { + w.SetText("Rebuild").SetIcon(icons.Reset). + SetTooltip("Rebuild the environment, when you change parameters"). + OnClick(func(e events.Event) { + if isRunning { + fmt.Println("still running...") + return + } + stop = false + cycle = 0 + config() + updateView() + }) + }) + }) + }) + b.RunMainWindow() +} diff --git a/physics/examples/test1/test1.go b/physics/examples/test1/test1.go index 67c11cf5..0b93627a 100644 --- a/physics/examples/test1/test1.go +++ b/physics/examples/test1/test1.go @@ -46,27 +46,33 @@ func main() { wr := world.NewWorld(sc) wl := physics.NewWorld() - wl.GPU = true - fv.SetStruct(wl) + wl.GPU = false + params := physics.GetParams(0) + params.Dt = 0.0001 + params.SubSteps = 1 + params.Gravity.Y = 0 + params.ContactRelax = 0.1 + params.Restitution.SetBool(false) + fv.SetStruct(params) split.SetSplits(0.2, 0.8) rot := math32.NewQuat(0, 0, 0, 1) // thick := float32(0.1) - wr.NewBody(wl, "floor", physics.Plane, "grey", math32.Vec3(10, 0, 10), + wr.NewBody(wl, "floor", physics.Plane, "#D0D0D080", math32.Vec3(10, 0, 10), math32.Vec3(0, 0, 0), rot) height := float32(.5) width := height * .4 depth := height * .15 _ = width - b1 := wr.NewDynamic(wl, "body", physics.Sphere, "purple", 1.0, math32.Vec3(height, height, depth), + b1 := wr.NewDynamic(wl, "body", physics.Box, "purple", 1.0, math32.Vec3(height, height, depth), math32.Vec3(0, height+2, 0), rot) _ = b1 // bj := wl.NewJointRevolute(-1, b1.DynamicIndex, math32.Vec3(0, 0, 0), math32.Vec3(0, -height, 0), math32.Vec3(0, 1, 0)) - // bj := wl.NewJointPrismatic(-1, b1.DynamicIndex, math32.Vec3(0, 0, 0), math32.Vec3(0, -height, 0), math32.Vec3(1, 0, 0)) - // physics.SetJointControlForce(bj, 0, 5) + bj := wl.NewJointPrismatic(-1, b1.DynamicIndex, math32.Vec3(0, 0, 0), math32.Vec3(0, -height, 0), math32.Vec3(1, 0, 0)) + physics.SetJointControlForce(bj, 0, .1) // physics.SetJointTargetPos(bj, 0, 1) // physics.SetJointDoF(bj, 0, physics.JointDamp, 0.01) // physics.SetJointDoF(bj, 0, physics.JointStiff, 1) // this makes a big difference @@ -75,27 +81,18 @@ func main() { wr.Init(wl) - params := physics.GetParams(0) - params.Dt = 0.01 - // params.Gravity.Y = 0 - params.ContactRelax = 0.1 - params.Restitution.SetBool(false) - fmt.Println(params.ContactRelax) - - wl.Config() - wr.Update() sc.Camera.Pose.Pos = math32.Vec3(0, 40, 3.5) sc.Camera.LookAt(math32.Vec3(0, 5, 0), math32.Vec3(0, 1, 0)) sc.SaveCamera("3") - sc.Camera.Pose.Pos = math32.Vec3(0, 20, 30) - sc.Camera.LookAt(math32.Vec3(0, 5, 0), math32.Vec3(0, 1, 0)) - sc.SaveCamera("2") - sc.Camera.Pose.Pos = math32.Vec3(-1.33, 2.24, 3.55) sc.Camera.LookAt(math32.Vec3(0, .5, 0), math32.Vec3(0, 1, 0)) + sc.SaveCamera("2") + + sc.Camera.Pose.Pos = math32.Vec3(0, 20, 30) + sc.Camera.LookAt(math32.Vec3(0, 5, 0), math32.Vec3(0, 1, 0)) sc.SaveCamera("1") sc.SaveCamera("default") diff --git a/physics/gosl.go b/physics/gosl.go index 4f6df58c..afde8063 100644 --- a/physics/gosl.go +++ b/physics/gosl.go @@ -132,18 +132,6 @@ func GPUInit() { pl.AddVarUsed(2, "ContactsN") pl.AddVarUsed(2, "Dynamics") pl.AddVarUsed(0, "Params") - pl = gpu.NewComputePipelineShaderFS(shaders, "shaders/DeltasFromContacts.wgsl", sy) - pl.AddVarUsed(0, "TensorStrides") - pl.AddVarUsed(2, "Contacts") - pl.AddVarUsed(2, "ContactsN") - pl.AddVarUsed(2, "Dynamics") - pl.AddVarUsed(0, "Params") - pl = gpu.NewComputePipelineShaderFS(shaders, "shaders/DeltasFromJoints.wgsl", sy) - pl.AddVarUsed(0, "TensorStrides") - pl.AddVarUsed(1, "BodyJoints") - pl.AddVarUsed(2, "Dynamics") - pl.AddVarUsed(1, "Joints") - pl.AddVarUsed(0, "Params") pl = gpu.NewComputePipelineShaderFS(shaders, "shaders/DynamicsCurToNext.wgsl", sy) pl.AddVarUsed(0, "TensorStrides") pl.AddVarUsed(2, "Dynamics") @@ -159,6 +147,13 @@ func GPUInit() { pl.AddVarUsed(1, "Bodies") pl.AddVarUsed(2, "Dynamics") pl.AddVarUsed(0, "Params") + pl = gpu.NewComputePipelineShaderFS(shaders, "shaders/StepBodyContactDeltas.wgsl", sy) + pl.AddVarUsed(0, "TensorStrides") + pl.AddVarUsed(1, "Bodies") + pl.AddVarUsed(2, "Contacts") + pl.AddVarUsed(2, "ContactsN") + pl.AddVarUsed(2, "Dynamics") + pl.AddVarUsed(0, "Params") pl = gpu.NewComputePipelineShaderFS(shaders, "shaders/StepBodyContacts.wgsl", sy) pl.AddVarUsed(0, "TensorStrides") pl.AddVarUsed(1, "Bodies") @@ -166,10 +161,12 @@ func GPUInit() { pl.AddVarUsed(2, "ContactsN") pl.AddVarUsed(2, "Dynamics") pl.AddVarUsed(0, "Params") - pl = gpu.NewComputePipelineShaderFS(shaders, "shaders/StepBodyDeltas.wgsl", sy) + pl = gpu.NewComputePipelineShaderFS(shaders, "shaders/StepBodyJointDeltas.wgsl", sy) pl.AddVarUsed(0, "TensorStrides") pl.AddVarUsed(1, "Bodies") + pl.AddVarUsed(1, "BodyJoints") pl.AddVarUsed(2, "Dynamics") + pl.AddVarUsed(1, "Joints") pl.AddVarUsed(0, "Params") pl = gpu.NewComputePipelineShaderFS(shaders, "shaders/StepInit.wgsl", sy) pl.AddVarUsed(0, "TensorStrides") @@ -300,90 +297,6 @@ func RunOneCollisionNarrow(n int, syncVars ...GPUVars) { RunCollisionNarrowCPU(n) } } -// RunDeltasFromContacts runs the DeltasFromContacts kernel with given number of elements, -// on either the CPU or GPU depending on the UseGPU variable. -// Can call multiple Run* kernels in a row, which are then all launched -// in the same command submission on the GPU, which is by far the most efficient. -// MUST call RunDone (with optional vars to sync) after all Run calls. -// Alternatively, a single-shot RunOneDeltasFromContacts call does Run and Done for a -// single run-and-sync case. -func RunDeltasFromContacts(n int) { - if UseGPU { - RunDeltasFromContactsGPU(n) - } else { - RunDeltasFromContactsCPU(n) - } -} - -// RunDeltasFromContactsGPU runs the DeltasFromContacts kernel on the GPU. See [RunDeltasFromContacts] for more info. -func RunDeltasFromContactsGPU(n int) { - sy := GPUSystem - pl := sy.ComputePipelines["DeltasFromContacts"] - ce, _ := sy.BeginComputePass() - pl.Dispatch1D(ce, n, 64) -} - -// RunDeltasFromContactsCPU runs the DeltasFromContacts kernel on the CPU. -func RunDeltasFromContactsCPU(n int) { - gpu.VectorizeFunc(0, n, DeltasFromContacts) -} - -// RunOneDeltasFromContacts runs the DeltasFromContacts kernel with given number of elements, -// on either the CPU or GPU depending on the UseGPU variable. -// This version then calls RunDone with the given variables to sync -// after the Run, for a single-shot Run-and-Done call. If multiple kernels -// can be run in sequence, it is much more efficient to do multiple Run* -// calls followed by a RunDone call. -func RunOneDeltasFromContacts(n int, syncVars ...GPUVars) { - if UseGPU { - RunDeltasFromContactsGPU(n) - RunDone(syncVars...) - } else { - RunDeltasFromContactsCPU(n) - } -} -// RunDeltasFromJoints runs the DeltasFromJoints kernel with given number of elements, -// on either the CPU or GPU depending on the UseGPU variable. -// Can call multiple Run* kernels in a row, which are then all launched -// in the same command submission on the GPU, which is by far the most efficient. -// MUST call RunDone (with optional vars to sync) after all Run calls. -// Alternatively, a single-shot RunOneDeltasFromJoints call does Run and Done for a -// single run-and-sync case. -func RunDeltasFromJoints(n int) { - if UseGPU { - RunDeltasFromJointsGPU(n) - } else { - RunDeltasFromJointsCPU(n) - } -} - -// RunDeltasFromJointsGPU runs the DeltasFromJoints kernel on the GPU. See [RunDeltasFromJoints] for more info. -func RunDeltasFromJointsGPU(n int) { - sy := GPUSystem - pl := sy.ComputePipelines["DeltasFromJoints"] - ce, _ := sy.BeginComputePass() - pl.Dispatch1D(ce, n, 64) -} - -// RunDeltasFromJointsCPU runs the DeltasFromJoints kernel on the CPU. -func RunDeltasFromJointsCPU(n int) { - gpu.VectorizeFunc(0, n, DeltasFromJoints) -} - -// RunOneDeltasFromJoints runs the DeltasFromJoints kernel with given number of elements, -// on either the CPU or GPU depending on the UseGPU variable. -// This version then calls RunDone with the given variables to sync -// after the Run, for a single-shot Run-and-Done call. If multiple kernels -// can be run in sequence, it is much more efficient to do multiple Run* -// calls followed by a RunDone call. -func RunOneDeltasFromJoints(n int, syncVars ...GPUVars) { - if UseGPU { - RunDeltasFromJointsGPU(n) - RunDone(syncVars...) - } else { - RunDeltasFromJointsCPU(n) - } -} // RunDynamicsCurToNext runs the DynamicsCurToNext kernel with given number of elements, // on either the CPU or GPU depending on the UseGPU variable. // Can call multiple Run* kernels in a row, which are then all launched @@ -510,6 +423,48 @@ func RunOneInitDynamics(n int, syncVars ...GPUVars) { RunInitDynamicsCPU(n) } } +// RunStepBodyContactDeltas runs the StepBodyContactDeltas kernel with given number of elements, +// on either the CPU or GPU depending on the UseGPU variable. +// Can call multiple Run* kernels in a row, which are then all launched +// in the same command submission on the GPU, which is by far the most efficient. +// MUST call RunDone (with optional vars to sync) after all Run calls. +// Alternatively, a single-shot RunOneStepBodyContactDeltas call does Run and Done for a +// single run-and-sync case. +func RunStepBodyContactDeltas(n int) { + if UseGPU { + RunStepBodyContactDeltasGPU(n) + } else { + RunStepBodyContactDeltasCPU(n) + } +} + +// RunStepBodyContactDeltasGPU runs the StepBodyContactDeltas kernel on the GPU. See [RunStepBodyContactDeltas] for more info. +func RunStepBodyContactDeltasGPU(n int) { + sy := GPUSystem + pl := sy.ComputePipelines["StepBodyContactDeltas"] + ce, _ := sy.BeginComputePass() + pl.Dispatch1D(ce, n, 64) +} + +// RunStepBodyContactDeltasCPU runs the StepBodyContactDeltas kernel on the CPU. +func RunStepBodyContactDeltasCPU(n int) { + gpu.VectorizeFunc(0, n, StepBodyContactDeltas) +} + +// RunOneStepBodyContactDeltas runs the StepBodyContactDeltas kernel with given number of elements, +// on either the CPU or GPU depending on the UseGPU variable. +// This version then calls RunDone with the given variables to sync +// after the Run, for a single-shot Run-and-Done call. If multiple kernels +// can be run in sequence, it is much more efficient to do multiple Run* +// calls followed by a RunDone call. +func RunOneStepBodyContactDeltas(n int, syncVars ...GPUVars) { + if UseGPU { + RunStepBodyContactDeltasGPU(n) + RunDone(syncVars...) + } else { + RunStepBodyContactDeltasCPU(n) + } +} // RunStepBodyContacts runs the StepBodyContacts kernel with given number of elements, // on either the CPU or GPU depending on the UseGPU variable. // Can call multiple Run* kernels in a row, which are then all launched @@ -552,46 +507,46 @@ func RunOneStepBodyContacts(n int, syncVars ...GPUVars) { RunStepBodyContactsCPU(n) } } -// RunStepBodyDeltas runs the StepBodyDeltas kernel with given number of elements, +// RunStepBodyJointDeltas runs the StepBodyJointDeltas kernel with given number of elements, // on either the CPU or GPU depending on the UseGPU variable. // Can call multiple Run* kernels in a row, which are then all launched // in the same command submission on the GPU, which is by far the most efficient. // MUST call RunDone (with optional vars to sync) after all Run calls. -// Alternatively, a single-shot RunOneStepBodyDeltas call does Run and Done for a +// Alternatively, a single-shot RunOneStepBodyJointDeltas call does Run and Done for a // single run-and-sync case. -func RunStepBodyDeltas(n int) { +func RunStepBodyJointDeltas(n int) { if UseGPU { - RunStepBodyDeltasGPU(n) + RunStepBodyJointDeltasGPU(n) } else { - RunStepBodyDeltasCPU(n) + RunStepBodyJointDeltasCPU(n) } } -// RunStepBodyDeltasGPU runs the StepBodyDeltas kernel on the GPU. See [RunStepBodyDeltas] for more info. -func RunStepBodyDeltasGPU(n int) { +// RunStepBodyJointDeltasGPU runs the StepBodyJointDeltas kernel on the GPU. See [RunStepBodyJointDeltas] for more info. +func RunStepBodyJointDeltasGPU(n int) { sy := GPUSystem - pl := sy.ComputePipelines["StepBodyDeltas"] + pl := sy.ComputePipelines["StepBodyJointDeltas"] ce, _ := sy.BeginComputePass() pl.Dispatch1D(ce, n, 64) } -// RunStepBodyDeltasCPU runs the StepBodyDeltas kernel on the CPU. -func RunStepBodyDeltasCPU(n int) { - gpu.VectorizeFunc(0, n, StepBodyDeltas) +// RunStepBodyJointDeltasCPU runs the StepBodyJointDeltas kernel on the CPU. +func RunStepBodyJointDeltasCPU(n int) { + gpu.VectorizeFunc(0, n, StepBodyJointDeltas) } -// RunOneStepBodyDeltas runs the StepBodyDeltas kernel with given number of elements, +// RunOneStepBodyJointDeltas runs the StepBodyJointDeltas kernel with given number of elements, // on either the CPU or GPU depending on the UseGPU variable. // This version then calls RunDone with the given variables to sync // after the Run, for a single-shot Run-and-Done call. If multiple kernels // can be run in sequence, it is much more efficient to do multiple Run* // calls followed by a RunDone call. -func RunOneStepBodyDeltas(n int, syncVars ...GPUVars) { +func RunOneStepBodyJointDeltas(n int, syncVars ...GPUVars) { if UseGPU { - RunStepBodyDeltasGPU(n) + RunStepBodyJointDeltasGPU(n) RunDone(syncVars...) } else { - RunStepBodyDeltasCPU(n) + RunStepBodyJointDeltasCPU(n) } } // RunStepInit runs the StepInit kernel with given number of elements, diff --git a/physics/joint.go b/physics/joint.go index 9693032d..c7d3f2ad 100644 --- a/physics/joint.go +++ b/physics/joint.go @@ -337,12 +337,6 @@ const ( // joint limits JointLimitLower JointLimitUpper - - // joint stiffness target (ke) - JointStiff - - // joint damping target (kd) - JointDamp ) func JointAxisDoF(didx int32) math32.Vector3 { @@ -382,8 +376,17 @@ func (wl *World) JointDefaults(idx int32) { func (wl *World) JointDoFDefaults(didx int32) { JointDoFs.Set(-JointLimitUnlimited, int(didx), int(JointLimitLower)) JointDoFs.Set(JointLimitUnlimited, int(didx), int(JointLimitUpper)) - JointDoFs.Set(1.0e4, int(didx), int(JointStiff)) - JointDoFs.Set(1.0e1, int(didx), int(JointDamp)) + JointControls.Set(1, int(didx), int(JointTargetDamp)) +} + +// NewJointFixed adds a new Fixed joint +// between parent and child dynamic object indexes. +// Use -1 for parent to add a world-anchored joint. +// ppos, cpos are the relative positions from the parent, child. +// Sets relative rotation matricies to identity by default. +func (wl *World) NewJointFixed(parent, child int32, ppos, cpos math32.Vector3) int32 { + idx := wl.newJoint(Fixed, parent, child, ppos, cpos) + return idx } // NewJointPrismatic adds a new Prismatic (slider) joint diff --git a/physics/joint.goal b/physics/joint.goal index 7d111e50..630059d5 100644 --- a/physics/joint.goal +++ b/physics/joint.goal @@ -335,12 +335,6 @@ const ( // joint limits JointLimitLower JointLimitUpper - - // joint stiffness target (ke) - JointStiff - - // joint damping target (kd) - JointDamp ) func JointAxisDoF(didx int32) math32.Vector3 { @@ -382,8 +376,17 @@ func (wl *World) JointDefaults(idx int32) { func (wl *World) JointDoFDefaults(didx int32) { JointDoFs[didx, JointLimitLower] = -JointLimitUnlimited JointDoFs[didx, JointLimitUpper] = JointLimitUnlimited - JointDoFs[didx, JointStiff] = 1.0e4 - JointDoFs[didx, JointDamp] = 1.0e1 + JointControls[didx, JointTargetDamp] = 1 +} + +// NewJointFixed adds a new Fixed joint +// between parent and child dynamic object indexes. +// Use -1 for parent to add a world-anchored joint. +// ppos, cpos are the relative positions from the parent, child. +// Sets relative rotation matricies to identity by default. +func (wl *World) NewJointFixed(parent, child int32, ppos, cpos math32.Vector3) int32 { + idx := wl.newJoint(Fixed, parent, child, ppos, cpos) + return idx } // NewJointPrismatic adds a new Prismatic (slider) joint diff --git a/physics/params.go b/physics/params.go index 93f67739..1007c0e7 100644 --- a/physics/params.go +++ b/physics/params.go @@ -34,28 +34,28 @@ type PhysParams struct { // ContactRelax is rigid contact relaxation constant. // Higher values cause errros - ContactRelax float32 `default:"0.1"` + ContactRelax float32 `default:"0.8"` // 0.8 def // Contact weighting: balances contact forces? - ContactWeighting slbool.Bool `default:"true"` + ContactWeighting slbool.Bool `default:"true"` // true // Restitution takes into account bounciness of objects. - Restitution slbool.Bool `default:"true"` + Restitution slbool.Bool `default:"true"` // false // JointLinearRelax is joint linear relaxation constant. - JointLinearRelax float32 `default:"0.7"` + JointLinearRelax float32 `default:"0.7"` // 0.7 def // JointAngularRelax is joint angular relaxation constant. - JointAngularRelax float32 `default:"0.4"` + JointAngularRelax float32 `default:"0.4"` // 0.4 def // JointLinearComply is joint linear compliance constant. - JointLinearComply float32 `default:"0"` + JointLinearComply float32 `default:"0"` // 0 def // JointAngularComply is joint angular compliance constant. - JointAngularComply float32 `default:"0"` + JointAngularComply float32 `default:"0"` // 0 def // AngularDamping is damping of angular motion. - AngularDamping float32 `default:"0"` + AngularDamping float32 `default:"0"` // 0 def // SoftRelax is soft-body relaxation constant. SoftRelax float32 `default:"0.9"` @@ -105,7 +105,7 @@ func (pr *PhysParams) Defaults() { pr.Gravity.Set(0, -9.81, 0) pr.ContactMargin = 0.1 - pr.ContactRelax = 0.1 + pr.ContactRelax = 0.8 pr.ContactWeighting.SetBool(true) pr.Restitution.SetBool(false) diff --git a/physics/shaders/CollisionBroad.wgsl b/physics/shaders/CollisionBroad.wgsl index a56ef21a..d9ee0666 100644 --- a/physics/shaders/CollisionBroad.wgsl +++ b/physics/shaders/CollisionBroad.wgsl @@ -223,8 +223,10 @@ fn AddBroadContacts(biA: i32,biB: i32,nci: i32,ncA: i32,ncB: i32) { //////// import: "control.go" alias JointControlVars = i32; //enums:enum const JointControlForce: JointControlVars = 0; -const JointTargetPos: JointControlVars = 1; -const JointTargetVel: JointControlVars = 2; +const JointTargetStiff: JointControlVars = 1; +const JointTargetPos: JointControlVars = 2; +const JointTargetDamp: JointControlVars = 3; +const JointTargetVel: JointControlVars = 4; //////// import: "dynamics.go" alias DynamicVars = i32; //enums:enum @@ -271,12 +273,12 @@ fn DynamicQuat(idx: i32,cni: i32) -> vec4 { //////// import: "enumgen.go" const BodyVarsN: BodyVars = 43; const ContactVarsN: ContactVars = 33; -const JointControlVarsN: JointControlVars = 3; +const JointControlVarsN: JointControlVars = 5; const DynamicVarsN: DynamicVars = 33; const GPUVarsN: GPUVars = 12; const JointTypesN: JointTypes = 7; const JointVarsN: JointVars = 50; -const JointDoFVarsN: JointDoFVars = 7; +const JointDoFVarsN: JointDoFVars = 5; const ShapesN: Shapes = 6; //////// import: "joint.go" @@ -346,8 +348,6 @@ const JointAxisY: JointDoFVars = 1; const JointAxisZ: JointDoFVars = 2; const JointLimitLower: JointDoFVars = 3; const JointLimitUpper: JointDoFVars = 4; -const JointStiff: JointDoFVars = 5; -const JointDamp: JointDoFVars = 6; //////// import: "params.go" struct PhysParams { @@ -355,14 +355,14 @@ struct PhysParams { Dt: f32, SubSteps: i32, ContactMargin: f32, - ContactRelax: f32, - ContactWeighting: i32, - Restitution: i32, - JointLinearRelax: f32, - JointAngularRelax: f32, - JointLinearComply: f32, - JointAngularComply: f32, - AngularDamping: f32, + ContactRelax: f32, // 0.8 def + ContactWeighting: i32, // true + Restitution: i32, // false + JointLinearRelax: f32, // 0.7 def + JointAngularRelax: f32, // 0.4 def + JointLinearComply: f32, // 0 def + JointAngularComply: f32, // 0 def + AngularDamping: f32, // 0 def SoftRelax: f32, MaxGeomIter: i32, ContactsMax: i32, diff --git a/physics/shaders/CollisionNarrow.wgsl b/physics/shaders/CollisionNarrow.wgsl index 32beef82..b7b2a0dd 100644 --- a/physics/shaders/CollisionNarrow.wgsl +++ b/physics/shaders/CollisionNarrow.wgsl @@ -312,8 +312,10 @@ fn CollisionNarrow(i: u32) { //gosl:kernel //////// import: "control.go" alias JointControlVars = i32; //enums:enum const JointControlForce: JointControlVars = 0; -const JointTargetPos: JointControlVars = 1; -const JointTargetVel: JointControlVars = 2; +const JointTargetStiff: JointControlVars = 1; +const JointTargetPos: JointControlVars = 2; +const JointTargetDamp: JointControlVars = 3; +const JointTargetVel: JointControlVars = 4; //////// import: "dynamics.go" alias DynamicVars = i32; //enums:enum @@ -360,12 +362,12 @@ fn DynamicQuat(idx: i32,cni: i32) -> vec4 { //////// import: "enumgen.go" const BodyVarsN: BodyVars = 43; const ContactVarsN: ContactVars = 33; -const JointControlVarsN: JointControlVars = 3; +const JointControlVarsN: JointControlVars = 5; const DynamicVarsN: DynamicVars = 33; const GPUVarsN: GPUVars = 12; const JointTypesN: JointTypes = 7; const JointVarsN: JointVars = 50; -const JointDoFVarsN: JointDoFVars = 7; +const JointDoFVarsN: JointDoFVars = 5; const ShapesN: Shapes = 6; //////// import: "joint.go" @@ -435,8 +437,6 @@ const JointAxisY: JointDoFVars = 1; const JointAxisZ: JointDoFVars = 2; const JointLimitLower: JointDoFVars = 3; const JointLimitUpper: JointDoFVars = 4; -const JointStiff: JointDoFVars = 5; -const JointDamp: JointDoFVars = 6; //////// import: "params.go" struct PhysParams { @@ -444,14 +444,14 @@ struct PhysParams { Dt: f32, SubSteps: i32, ContactMargin: f32, - ContactRelax: f32, - ContactWeighting: i32, - Restitution: i32, - JointLinearRelax: f32, - JointAngularRelax: f32, - JointLinearComply: f32, - JointAngularComply: f32, - AngularDamping: f32, + ContactRelax: f32, // 0.8 def + ContactWeighting: i32, // true + Restitution: i32, // false + JointLinearRelax: f32, // 0.7 def + JointAngularRelax: f32, // 0.4 def + JointLinearComply: f32, // 0 def + JointAngularComply: f32, // 0 def + AngularDamping: f32, // 0 def SoftRelax: f32, MaxGeomIter: i32, ContactsMax: i32, diff --git a/physics/shaders/DeltasFromContacts.wgsl b/physics/shaders/DeltasFromContacts.wgsl deleted file mode 100644 index cf62a8b6..00000000 --- a/physics/shaders/DeltasFromContacts.wgsl +++ /dev/null @@ -1,385 +0,0 @@ -// Code generated by "gosl"; DO NOT EDIT -// kernel: DeltasFromContacts - -// // Params are global parameters. -@group(0) @binding(0) -var TensorStrides: array; -@group(0) @binding(1) -var Params: array; -// // Bodies are the rigid body elements (dynamic and static), // specifying the constant, non-dynamic properties, // which is initial state for dynamics. // [body][BodyVarsN] -// // Dynamics are the dynamic rigid body elements: these actually move. // Two alternating states are used: Params.Cur and Params.Next. // [dyn body][cur/next][DynamicVarsN] -@group(2) @binding(0) -var Dynamics: array; -@group(2) @binding(3) -var ContactsN: array; -@group(2) @binding(4) -var Contacts: array; -// // JointControls are dynamic joint control inputs, per joint DoF // (in correspondence with [JointDoFs]). This can be uploaded to the // GPU at every step. // [dof][JointControlVarsN] - -alias GPUVars = i32; - -@compute @workgroup_size(64, 1, 1) -fn main(@builtin(workgroup_id) wgid: vec3, @builtin(num_workgroups) nwg: vec3, @builtin(local_invocation_index) loci: u32) { - let idx = loci + (wgid.x + wgid.y * nwg.x + wgid.z * nwg.x * nwg.y) * 64; - DeltasFromContacts(idx); -} - -fn Index3D(s0: u32, s1: u32, s2: u32, i0: u32, i1: u32, i2: u32) -> u32 { - return s0 * i0 + s1 * i1 + s2 * i2; -} - -fn Index1D(s0: u32, i0: u32) -> u32 { - return s0 * i0; -} - -fn Index2D(s0: u32, s1: u32, i0: u32, i1: u32) -> u32 { - return s0 * i0 + s1 * i1; -} - - -//////// import: "vars.go" - -//////// import: "body.go" -alias BodyVars = i32; //enums:enum -const BodyShape: BodyVars = 0; -const BodyDynamic: BodyVars = 1; -const BodyWorld: BodyVars = 2; -const BodyGroup: BodyVars = 3; -const BodyHSizeX: BodyVars = 4; -const BodyHSizeY: BodyVars = 5; -const BodyHSizeZ: BodyVars = 6; -const BodyThick: BodyVars = 7; -const BodyMass: BodyVars = 8; -const BodyInvMass: BodyVars = 9; -const BodyBounce: BodyVars = 10; -const BodyFriction: BodyVars = 11; -const BodyFrictionTortion: BodyVars = 12; -const BodyFrictionRolling: BodyVars = 13; -const BodyPosX: BodyVars = 14; -const BodyPosY: BodyVars = 15; -const BodyPosZ: BodyVars = 16; -const BodyQuatX: BodyVars = 17; -const BodyQuatY: BodyVars = 18; -const BodyQuatZ: BodyVars = 19; -const BodyQuatW: BodyVars = 20; -const BodyComX: BodyVars = 21; -const BodyComY: BodyVars = 22; -const BodyComZ: BodyVars = 23; -const BodyInertiaXX: BodyVars = 24; -const BodyInertiaYX: BodyVars = 25; -const BodyInertiaZX: BodyVars = 26; -const BodyInertiaXY: BodyVars = 27; -const BodyInertiaYY: BodyVars = 28; -const BodyInertiaZY: BodyVars = 29; -const BodyInertiaXZ: BodyVars = 30; -const BodyInertiaYZ: BodyVars = 31; -const BodyInertiaZZ: BodyVars = 32; -const BodyInvInertiaXX: BodyVars = 33; -const BodyInvInertiaYX: BodyVars = 34; -const BodyInvInertiaZX: BodyVars = 35; -const BodyInvInertiaXY: BodyVars = 36; -const BodyInvInertiaYY: BodyVars = 37; -const BodyInvInertiaZY: BodyVars = 38; -const BodyInvInertiaXZ: BodyVars = 39; -const BodyInvInertiaYZ: BodyVars = 40; -const BodyInvInertiaZZ: BodyVars = 41; -const BodyRadius: BodyVars = 42; - -//////// import: "contact.go" -alias ContactVars = i32; //enums:enum -const ContactA: ContactVars = 0; -const ContactB: ContactVars = 1; -const ContactPointIdx: ContactVars = 2; -const ContactAPointX: ContactVars = 3; -const ContactAPointY: ContactVars = 4; -const ContactAPointZ: ContactVars = 5; -const ContactBPointX: ContactVars = 6; -const ContactBPointY: ContactVars = 7; -const ContactBPointZ: ContactVars = 8; -const ContactAOffX: ContactVars = 9; -const ContactAOffY: ContactVars = 10; -const ContactAOffZ: ContactVars = 11; -const ContactBOffX: ContactVars = 12; -const ContactBOffY: ContactVars = 13; -const ContactBOffZ: ContactVars = 14; -const ContactAThick: ContactVars = 15; -const ContactBThick: ContactVars = 16; -const ContactNormX: ContactVars = 17; -const ContactNormY: ContactVars = 18; -const ContactNormZ: ContactVars = 19; -const ContactWeight: ContactVars = 20; -const ContactADeltaX: ContactVars = 21; -const ContactADeltaY: ContactVars = 22; -const ContactADeltaZ: ContactVars = 23; -const ContactAAngDeltaX: ContactVars = 24; -const ContactAAngDeltaY: ContactVars = 25; -const ContactAAngDeltaZ: ContactVars = 26; -const ContactBDeltaX: ContactVars = 27; -const ContactBDeltaY: ContactVars = 28; -const ContactBDeltaZ: ContactVars = 29; -const ContactBAngDeltaX: ContactVars = 30; -const ContactBAngDeltaY: ContactVars = 31; -const ContactBAngDeltaZ: ContactVars = 32; -const BroadContactVarsN = ContactAPointX; -fn GetContactA(idx: i32) -> i32 { return i32(bitcast(Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactA))])); } -fn GetContactB(idx: i32) -> i32 { return i32(bitcast(Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactB))])); } -fn ContactADelta(idx: i32) -> vec3 { - return vec3(Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactADeltaX))], Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactADeltaY))], Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactADeltaZ))]); -} -fn ContactAAngDelta(idx: i32) -> vec3 { - return vec3(Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactAAngDeltaX))], Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactAAngDeltaY))], Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactAAngDeltaZ))]); -} -fn ContactBDelta(idx: i32) -> vec3 { - return vec3(Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactBDeltaX))], Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactBDeltaY))], Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactBDeltaZ))]); -} -fn ContactBAngDelta(idx: i32) -> vec3 { - return vec3(Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactBAngDeltaX))], Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactBAngDeltaY))], Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactBAngDeltaZ))]); -} -fn DeltasFromContacts(i: u32) { //gosl:kernel - var params = Params[0]; - var di = i32(i); - if (di >= params.DynamicsN) { - return; - } - var cmax = ContactsN[0]; - var bi = DynamicBody(di); - var td = vec3(0, 0, 0); - var ta = vec3(0, 0, 0); - var tw = f32(0); - for (var ci=0; ci i32 { return i32(bitcast(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(0), u32(DynBody))])); } -fn DynamicDelta(idx: i32,cni: i32) -> vec3 { - return vec3(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaZ))]); -} -fn SetDynamicDelta(idx: i32,cni: i32, delta: vec3) { - Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaX))] = delta.x; - Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaY))] = delta.y; - Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaZ))] = delta.z; -} -fn DynamicAngDelta(idx: i32,cni: i32) -> vec3 { - return vec3(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaZ))]); -} -fn SetDynamicAngDelta(idx: i32,cni: i32, angDelta: vec3) { - Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaX))] = angDelta.x; - Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaY))] = angDelta.y; - Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaZ))] = angDelta.z; -} - -//////// import: "enumgen.go" -const BodyVarsN: BodyVars = 43; -const ContactVarsN: ContactVars = 33; -const JointControlVarsN: JointControlVars = 3; -const DynamicVarsN: DynamicVars = 33; -const GPUVarsN: GPUVars = 12; -const JointTypesN: JointTypes = 7; -const JointVarsN: JointVars = 50; -const JointDoFVarsN: JointDoFVars = 7; -const ShapesN: Shapes = 6; - -//////// import: "joint.go" -const JointLimitUnlimited = 1e10; -alias JointTypes = i32; //enums:enum -const Prismatic: JointTypes = 0; -const Revolute: JointTypes = 1; -const Ball: JointTypes = 2; -const Fixed: JointTypes = 3; -const Free: JointTypes = 4; -const Distance: JointTypes = 5; -const D6: JointTypes = 6; -alias JointVars = i32; //enums:enum -const JointType: JointVars = 0; -const JointEnabled: JointVars = 1; -const JointParent: JointVars = 2; -const JointChild: JointVars = 3; -const JointPPosX: JointVars = 4; -const JointPPosY: JointVars = 5; -const JointPPosZ: JointVars = 6; -const JointPQuatX: JointVars = 7; -const JointPQuatY: JointVars = 8; -const JointPQuatZ: JointVars = 9; -const JointPQuatW: JointVars = 10; -const JointCPosX: JointVars = 11; -const JointCPosY: JointVars = 12; -const JointCPosZ: JointVars = 13; -const JointCQuatX: JointVars = 14; -const JointCQuatY: JointVars = 15; -const JointCQuatZ: JointVars = 16; -const JointCQuatW: JointVars = 17; -const JointLinearDoFN: JointVars = 18; -const JointAngularDoFN: JointVars = 19; -const JointDoF1: JointVars = 20; -const JointDoF2: JointVars = 21; -const JointDoF3: JointVars = 22; -const JointDoF4: JointVars = 23; -const JointDoF5: JointVars = 24; -const JointDoF6: JointVars = 25; -const JointPForceX: JointVars = 26; -const JointPForceY: JointVars = 27; -const JointPForceZ: JointVars = 28; -const JointPTorqueX: JointVars = 29; -const JointPTorqueY: JointVars = 30; -const JointPTorqueZ: JointVars = 31; -const JointCForceX: JointVars = 32; -const JointCForceY: JointVars = 33; -const JointCForceZ: JointVars = 34; -const JointCTorqueX: JointVars = 35; -const JointCTorqueY: JointVars = 36; -const JointCTorqueZ: JointVars = 37; -const JointPDeltaX: JointVars = 38; -const JointPDeltaY: JointVars = 39; -const JointPDeltaZ: JointVars = 40; -const JointPAngDeltaX: JointVars = 41; -const JointPAngDeltaY: JointVars = 42; -const JointPAngDeltaZ: JointVars = 43; -const JointCDeltaX: JointVars = 44; -const JointCDeltaY: JointVars = 45; -const JointCDeltaZ: JointVars = 46; -const JointCAngDeltaX: JointVars = 47; -const JointCAngDeltaY: JointVars = 48; -const JointCAngDeltaZ: JointVars = 49; -alias JointDoFVars = i32; //enums:enum -const JointAxisX: JointDoFVars = 0; -const JointAxisY: JointDoFVars = 1; -const JointAxisZ: JointDoFVars = 2; -const JointLimitLower: JointDoFVars = 3; -const JointLimitUpper: JointDoFVars = 4; -const JointStiff: JointDoFVars = 5; -const JointDamp: JointDoFVars = 6; - -//////// import: "params.go" -struct PhysParams { - Iterations: i32, - Dt: f32, - SubSteps: i32, - ContactMargin: f32, - ContactRelax: f32, - ContactWeighting: i32, - Restitution: i32, - JointLinearRelax: f32, - JointAngularRelax: f32, - JointLinearComply: f32, - JointAngularComply: f32, - AngularDamping: f32, - SoftRelax: f32, - MaxGeomIter: i32, - ContactsMax: i32, - Cur: i32, - Next: i32, - BodiesN: i32, - DynamicsN: i32, - JointsN: i32, - JointDoFsN: i32, - BodyJointsMax: i32, - BodyCollidePairsN: i32, - pad: i32, - Gravity: vec4, -} - -//////// import: "shapecollide.go" -struct GeomData { - BodyIdx: i32, - Shape: Shapes, - MinSize: f32, - Thick: f32, - Radius: f32, - Size: vec3, - WbR: vec3, - WbQ: vec4, - BwR: vec3, - BwQ: vec4, -} - -//////// import: "shapegeom.go" - -//////// import: "shapes.go" -alias Shapes = i32; //enums:enum -const Plane: Shapes = 0; -const Sphere: Shapes = 1; -const Capsule: Shapes = 2; -const Cylinder: Shapes = 3; -const Box: Shapes = 4; -const Cone: Shapes = 5; - -//////// import: "slmath-matrix3.go" - -//////// import: "slmath-quaternion.go" - -//////// import: "slmath-vector2.go" - -//////// import: "slmath-vector3.go" - -//////// import: "step.go" - -//////// import: "step_body.go" - -//////// import: "step_joint.go" \ No newline at end of file diff --git a/physics/shaders/DeltasFromJoints.wgsl b/physics/shaders/DeltasFromJoints.wgsl deleted file mode 100644 index 2e637431..00000000 --- a/physics/shaders/DeltasFromJoints.wgsl +++ /dev/null @@ -1,370 +0,0 @@ -// Code generated by "gosl"; DO NOT EDIT -// kernel: DeltasFromJoints - -// // Params are global parameters. -@group(0) @binding(0) -var TensorStrides: array; -@group(0) @binding(1) -var Params: array; -// // Bodies are the rigid body elements (dynamic and static), // specifying the constant, non-dynamic properties, // which is initial state for dynamics. // [body][BodyVarsN] -@group(1) @binding(1) -var Joints: array; -@group(1) @binding(3) -var BodyJoints: array; -// // Dynamics are the dynamic rigid body elements: these actually move. // Two alternating states are used: Params.Cur and Params.Next. // [dyn body][cur/next][DynamicVarsN] -@group(2) @binding(0) -var Dynamics: array; -// // JointControls are dynamic joint control inputs, per joint DoF // (in correspondence with [JointDoFs]). This can be uploaded to the // GPU at every step. // [dof][JointControlVarsN] - -alias GPUVars = i32; - -@compute @workgroup_size(64, 1, 1) -fn main(@builtin(workgroup_id) wgid: vec3, @builtin(num_workgroups) nwg: vec3, @builtin(local_invocation_index) loci: u32) { - let idx = loci + (wgid.x + wgid.y * nwg.x + wgid.z * nwg.x * nwg.y) * 64; - DeltasFromJoints(idx); -} - -fn Index2D(s0: u32, s1: u32, i0: u32, i1: u32) -> u32 { - return s0 * i0 + s1 * i1; -} - -fn Index3D(s0: u32, s1: u32, s2: u32, i0: u32, i1: u32, i2: u32) -> u32 { - return s0 * i0 + s1 * i1 + s2 * i2; -} - - -//////// import: "vars.go" - -//////// import: "body.go" -alias BodyVars = i32; //enums:enum -const BodyShape: BodyVars = 0; -const BodyDynamic: BodyVars = 1; -const BodyWorld: BodyVars = 2; -const BodyGroup: BodyVars = 3; -const BodyHSizeX: BodyVars = 4; -const BodyHSizeY: BodyVars = 5; -const BodyHSizeZ: BodyVars = 6; -const BodyThick: BodyVars = 7; -const BodyMass: BodyVars = 8; -const BodyInvMass: BodyVars = 9; -const BodyBounce: BodyVars = 10; -const BodyFriction: BodyVars = 11; -const BodyFrictionTortion: BodyVars = 12; -const BodyFrictionRolling: BodyVars = 13; -const BodyPosX: BodyVars = 14; -const BodyPosY: BodyVars = 15; -const BodyPosZ: BodyVars = 16; -const BodyQuatX: BodyVars = 17; -const BodyQuatY: BodyVars = 18; -const BodyQuatZ: BodyVars = 19; -const BodyQuatW: BodyVars = 20; -const BodyComX: BodyVars = 21; -const BodyComY: BodyVars = 22; -const BodyComZ: BodyVars = 23; -const BodyInertiaXX: BodyVars = 24; -const BodyInertiaYX: BodyVars = 25; -const BodyInertiaZX: BodyVars = 26; -const BodyInertiaXY: BodyVars = 27; -const BodyInertiaYY: BodyVars = 28; -const BodyInertiaZY: BodyVars = 29; -const BodyInertiaXZ: BodyVars = 30; -const BodyInertiaYZ: BodyVars = 31; -const BodyInertiaZZ: BodyVars = 32; -const BodyInvInertiaXX: BodyVars = 33; -const BodyInvInertiaYX: BodyVars = 34; -const BodyInvInertiaZX: BodyVars = 35; -const BodyInvInertiaXY: BodyVars = 36; -const BodyInvInertiaYY: BodyVars = 37; -const BodyInvInertiaZY: BodyVars = 38; -const BodyInvInertiaXZ: BodyVars = 39; -const BodyInvInertiaYZ: BodyVars = 40; -const BodyInvInertiaZZ: BodyVars = 41; -const BodyRadius: BodyVars = 42; - -//////// import: "contact.go" -alias ContactVars = i32; //enums:enum -const ContactA: ContactVars = 0; -const ContactB: ContactVars = 1; -const ContactPointIdx: ContactVars = 2; -const ContactAPointX: ContactVars = 3; -const ContactAPointY: ContactVars = 4; -const ContactAPointZ: ContactVars = 5; -const ContactBPointX: ContactVars = 6; -const ContactBPointY: ContactVars = 7; -const ContactBPointZ: ContactVars = 8; -const ContactAOffX: ContactVars = 9; -const ContactAOffY: ContactVars = 10; -const ContactAOffZ: ContactVars = 11; -const ContactBOffX: ContactVars = 12; -const ContactBOffY: ContactVars = 13; -const ContactBOffZ: ContactVars = 14; -const ContactAThick: ContactVars = 15; -const ContactBThick: ContactVars = 16; -const ContactNormX: ContactVars = 17; -const ContactNormY: ContactVars = 18; -const ContactNormZ: ContactVars = 19; -const ContactWeight: ContactVars = 20; -const ContactADeltaX: ContactVars = 21; -const ContactADeltaY: ContactVars = 22; -const ContactADeltaZ: ContactVars = 23; -const ContactAAngDeltaX: ContactVars = 24; -const ContactAAngDeltaY: ContactVars = 25; -const ContactAAngDeltaZ: ContactVars = 26; -const ContactBDeltaX: ContactVars = 27; -const ContactBDeltaY: ContactVars = 28; -const ContactBDeltaZ: ContactVars = 29; -const ContactBAngDeltaX: ContactVars = 30; -const ContactBAngDeltaY: ContactVars = 31; -const ContactBAngDeltaZ: ContactVars = 32; -const BroadContactVarsN = ContactAPointX; - -//////// import: "control.go" -alias JointControlVars = i32; //enums:enum -const JointControlForce: JointControlVars = 0; -const JointTargetPos: JointControlVars = 1; -const JointTargetVel: JointControlVars = 2; - -//////// import: "dynamics.go" -alias DynamicVars = i32; //enums:enum -const DynBody: DynamicVars = 0; -const DynPosX: DynamicVars = 1; -const DynPosY: DynamicVars = 2; -const DynPosZ: DynamicVars = 3; -const DynQuatX: DynamicVars = 4; -const DynQuatY: DynamicVars = 5; -const DynQuatZ: DynamicVars = 6; -const DynQuatW: DynamicVars = 7; -const DynVelX: DynamicVars = 8; -const DynVelY: DynamicVars = 9; -const DynVelZ: DynamicVars = 10; -const DynAngVelX: DynamicVars = 11; -const DynAngVelY: DynamicVars = 12; -const DynAngVelZ: DynamicVars = 13; -const DynAccX: DynamicVars = 14; -const DynAccY: DynamicVars = 15; -const DynAccZ: DynamicVars = 16; -const DynAngAccX: DynamicVars = 17; -const DynAngAccY: DynamicVars = 18; -const DynAngAccZ: DynamicVars = 19; -const DynForceX: DynamicVars = 20; -const DynForceY: DynamicVars = 21; -const DynForceZ: DynamicVars = 22; -const DynTorqueX: DynamicVars = 23; -const DynTorqueY: DynamicVars = 24; -const DynTorqueZ: DynamicVars = 25; -const DynDeltaX: DynamicVars = 26; -const DynDeltaY: DynamicVars = 27; -const DynDeltaZ: DynamicVars = 28; -const DynAngDeltaX: DynamicVars = 29; -const DynAngDeltaY: DynamicVars = 30; -const DynAngDeltaZ: DynamicVars = 31; -const DynContactWeight: DynamicVars = 32; -fn DynamicDelta(idx: i32,cni: i32) -> vec3 { - return vec3(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaZ))]); -} -fn SetDynamicDelta(idx: i32,cni: i32, delta: vec3) { - Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaX))] = delta.x; - Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaY))] = delta.y; - Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaZ))] = delta.z; -} -fn DynamicAngDelta(idx: i32,cni: i32) -> vec3 { - return vec3(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaZ))]); -} -fn SetDynamicAngDelta(idx: i32,cni: i32, angDelta: vec3) { - Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaX))] = angDelta.x; - Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaY))] = angDelta.y; - Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaZ))] = angDelta.z; -} - -//////// import: "enumgen.go" -const BodyVarsN: BodyVars = 43; -const ContactVarsN: ContactVars = 33; -const JointControlVarsN: JointControlVars = 3; -const DynamicVarsN: DynamicVars = 33; -const GPUVarsN: GPUVars = 12; -const JointTypesN: JointTypes = 7; -const JointVarsN: JointVars = 50; -const JointDoFVarsN: JointDoFVars = 7; -const ShapesN: Shapes = 6; - -//////// import: "joint.go" -const JointLimitUnlimited = 1e10; -alias JointTypes = i32; //enums:enum -const Prismatic: JointTypes = 0; -const Revolute: JointTypes = 1; -const Ball: JointTypes = 2; -const Fixed: JointTypes = 3; -const Free: JointTypes = 4; -const Distance: JointTypes = 5; -const D6: JointTypes = 6; -alias JointVars = i32; //enums:enum -const JointType: JointVars = 0; -const JointEnabled: JointVars = 1; -const JointParent: JointVars = 2; -const JointChild: JointVars = 3; -const JointPPosX: JointVars = 4; -const JointPPosY: JointVars = 5; -const JointPPosZ: JointVars = 6; -const JointPQuatX: JointVars = 7; -const JointPQuatY: JointVars = 8; -const JointPQuatZ: JointVars = 9; -const JointPQuatW: JointVars = 10; -const JointCPosX: JointVars = 11; -const JointCPosY: JointVars = 12; -const JointCPosZ: JointVars = 13; -const JointCQuatX: JointVars = 14; -const JointCQuatY: JointVars = 15; -const JointCQuatZ: JointVars = 16; -const JointCQuatW: JointVars = 17; -const JointLinearDoFN: JointVars = 18; -const JointAngularDoFN: JointVars = 19; -const JointDoF1: JointVars = 20; -const JointDoF2: JointVars = 21; -const JointDoF3: JointVars = 22; -const JointDoF4: JointVars = 23; -const JointDoF5: JointVars = 24; -const JointDoF6: JointVars = 25; -const JointPForceX: JointVars = 26; -const JointPForceY: JointVars = 27; -const JointPForceZ: JointVars = 28; -const JointPTorqueX: JointVars = 29; -const JointPTorqueY: JointVars = 30; -const JointPTorqueZ: JointVars = 31; -const JointCForceX: JointVars = 32; -const JointCForceY: JointVars = 33; -const JointCForceZ: JointVars = 34; -const JointCTorqueX: JointVars = 35; -const JointCTorqueY: JointVars = 36; -const JointCTorqueZ: JointVars = 37; -const JointPDeltaX: JointVars = 38; -const JointPDeltaY: JointVars = 39; -const JointPDeltaZ: JointVars = 40; -const JointPAngDeltaX: JointVars = 41; -const JointPAngDeltaY: JointVars = 42; -const JointPAngDeltaZ: JointVars = 43; -const JointCDeltaX: JointVars = 44; -const JointCDeltaY: JointVars = 45; -const JointCDeltaZ: JointVars = 46; -const JointCAngDeltaX: JointVars = 47; -const JointCAngDeltaY: JointVars = 48; -const JointCAngDeltaZ: JointVars = 49; -fn JointPDelta(idx: i32) -> vec3 { - return vec3(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPDeltaX))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPDeltaY))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPDeltaZ))]); -} -fn JointPAngDelta(idx: i32) -> vec3 { - return vec3(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPAngDeltaX))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPAngDeltaY))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPAngDeltaZ))]); -} -fn JointCDelta(idx: i32) -> vec3 { - return vec3(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCDeltaX))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCDeltaY))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCDeltaZ))]); -} -fn JointCAngDelta(idx: i32) -> vec3 { - return vec3(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCAngDeltaX))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCAngDeltaY))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCAngDeltaZ))]); -} -alias JointDoFVars = i32; //enums:enum -const JointAxisX: JointDoFVars = 0; -const JointAxisY: JointDoFVars = 1; -const JointAxisZ: JointDoFVars = 2; -const JointLimitLower: JointDoFVars = 3; -const JointLimitUpper: JointDoFVars = 4; -const JointStiff: JointDoFVars = 5; -const JointDamp: JointDoFVars = 6; - -//////// import: "params.go" -struct PhysParams { - Iterations: i32, - Dt: f32, - SubSteps: i32, - ContactMargin: f32, - ContactRelax: f32, - ContactWeighting: i32, - Restitution: i32, - JointLinearRelax: f32, - JointAngularRelax: f32, - JointLinearComply: f32, - JointAngularComply: f32, - AngularDamping: f32, - SoftRelax: f32, - MaxGeomIter: i32, - ContactsMax: i32, - Cur: i32, - Next: i32, - BodiesN: i32, - DynamicsN: i32, - JointsN: i32, - JointDoFsN: i32, - BodyJointsMax: i32, - BodyCollidePairsN: i32, - pad: i32, - Gravity: vec4, -} - -//////// import: "shapecollide.go" -struct GeomData { - BodyIdx: i32, - Shape: Shapes, - MinSize: f32, - Thick: f32, - Radius: f32, - Size: vec3, - WbR: vec3, - WbQ: vec4, - BwR: vec3, - BwQ: vec4, -} - -//////// import: "shapegeom.go" - -//////// import: "shapes.go" -alias Shapes = i32; //enums:enum -const Plane: Shapes = 0; -const Sphere: Shapes = 1; -const Capsule: Shapes = 2; -const Cylinder: Shapes = 3; -const Box: Shapes = 4; -const Cone: Shapes = 5; - -//////// import: "slmath-matrix3.go" - -//////// import: "slmath-quaternion.go" - -//////// import: "slmath-vector2.go" - -//////// import: "slmath-vector3.go" - -//////// import: "step.go" - -//////// import: "step_body.go" -fn DeltasFromJoints(i: u32) { //gosl:kernel - var params = Params[0]; - var di = i32(i); - if (di >= params.DynamicsN) { - return; - } - var np = BodyJoints[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(di), u32(0), u32(0))]; - var nc = BodyJoints[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(di), u32(1), u32(0))]; - var td = vec3(0, 0, 0); - var ta = vec3(0, 0, 0); - for (var i = i32(1); - i <= np; i++) { - var ji = BodyJoints[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(di), u32(0), u32(i))]; - var d = JointPDelta(ji); - td = td+(d); - var a = JointPAngDelta(ji); - ta = ta+(a); - } - for (var i = i32(1); - i <= nc; i++) { - var ji = BodyJoints[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(di), u32(1), u32(i))]; - var d = JointCDelta(ji); - td = td+(d); - var a = JointCAngDelta(ji); - ta = ta+(a); - } - var v0 = DynamicDelta(di, params.Next); - var w0 = DynamicAngDelta(di, params.Next); - SetDynamicDelta(di, params.Next, td+(v0)); - SetDynamicAngDelta(di, params.Next, ta+(w0)); - Params[0] = params; -} - -//////// import: "step_joint.go" \ No newline at end of file diff --git a/physics/shaders/DynamicsCurToNext.wgsl b/physics/shaders/DynamicsCurToNext.wgsl index d6063185..85cb614e 100644 --- a/physics/shaders/DynamicsCurToNext.wgsl +++ b/physics/shaders/DynamicsCurToNext.wgsl @@ -113,8 +113,10 @@ const BroadContactVarsN = ContactAPointX; //////// import: "control.go" alias JointControlVars = i32; //enums:enum const JointControlForce: JointControlVars = 0; -const JointTargetPos: JointControlVars = 1; -const JointTargetVel: JointControlVars = 2; +const JointTargetStiff: JointControlVars = 1; +const JointTargetPos: JointControlVars = 2; +const JointTargetDamp: JointControlVars = 3; +const JointTargetVel: JointControlVars = 4; //////// import: "dynamics.go" alias DynamicVars = i32; //enums:enum @@ -155,12 +157,12 @@ const DynContactWeight: DynamicVars = 32; //////// import: "enumgen.go" const BodyVarsN: BodyVars = 43; const ContactVarsN: ContactVars = 33; -const JointControlVarsN: JointControlVars = 3; +const JointControlVarsN: JointControlVars = 5; const DynamicVarsN: DynamicVars = 33; const GPUVarsN: GPUVars = 12; const JointTypesN: JointTypes = 7; const JointVarsN: JointVars = 50; -const JointDoFVarsN: JointDoFVars = 7; +const JointDoFVarsN: JointDoFVars = 5; const ShapesN: Shapes = 6; //////// import: "joint.go" @@ -230,8 +232,6 @@ const JointAxisY: JointDoFVars = 1; const JointAxisZ: JointDoFVars = 2; const JointLimitLower: JointDoFVars = 3; const JointLimitUpper: JointDoFVars = 4; -const JointStiff: JointDoFVars = 5; -const JointDamp: JointDoFVars = 6; //////// import: "params.go" struct PhysParams { @@ -239,14 +239,14 @@ struct PhysParams { Dt: f32, SubSteps: i32, ContactMargin: f32, - ContactRelax: f32, - ContactWeighting: i32, - Restitution: i32, - JointLinearRelax: f32, - JointAngularRelax: f32, - JointLinearComply: f32, - JointAngularComply: f32, - AngularDamping: f32, + ContactRelax: f32, // 0.8 def + ContactWeighting: i32, // true + Restitution: i32, // false + JointLinearRelax: f32, // 0.7 def + JointAngularRelax: f32, // 0.4 def + JointLinearComply: f32, // 0 def + JointAngularComply: f32, // 0 def + AngularDamping: f32, // 0 def SoftRelax: f32, MaxGeomIter: i32, ContactsMax: i32, diff --git a/physics/shaders/ForcesFromJoints.wgsl b/physics/shaders/ForcesFromJoints.wgsl index 041a0c0f..575d3687 100644 --- a/physics/shaders/ForcesFromJoints.wgsl +++ b/physics/shaders/ForcesFromJoints.wgsl @@ -121,8 +121,10 @@ const BroadContactVarsN = ContactAPointX; //////// import: "control.go" alias JointControlVars = i32; //enums:enum const JointControlForce: JointControlVars = 0; -const JointTargetPos: JointControlVars = 1; -const JointTargetVel: JointControlVars = 2; +const JointTargetStiff: JointControlVars = 1; +const JointTargetPos: JointControlVars = 2; +const JointTargetDamp: JointControlVars = 3; +const JointTargetVel: JointControlVars = 4; //////// import: "dynamics.go" alias DynamicVars = i32; //enums:enum @@ -173,12 +175,12 @@ fn SetDynamicTorque(idx: i32,cni: i32, torque: vec3) { //////// import: "enumgen.go" const BodyVarsN: BodyVars = 43; const ContactVarsN: ContactVars = 33; -const JointControlVarsN: JointControlVars = 3; +const JointControlVarsN: JointControlVars = 5; const DynamicVarsN: DynamicVars = 33; const GPUVarsN: GPUVars = 12; const JointTypesN: JointTypes = 7; const JointVarsN: JointVars = 50; -const JointDoFVarsN: JointDoFVars = 7; +const JointDoFVarsN: JointDoFVars = 5; const ShapesN: Shapes = 6; //////// import: "joint.go" @@ -260,8 +262,6 @@ const JointAxisY: JointDoFVars = 1; const JointAxisZ: JointDoFVars = 2; const JointLimitLower: JointDoFVars = 3; const JointLimitUpper: JointDoFVars = 4; -const JointStiff: JointDoFVars = 5; -const JointDamp: JointDoFVars = 6; //////// import: "params.go" struct PhysParams { @@ -269,14 +269,14 @@ struct PhysParams { Dt: f32, SubSteps: i32, ContactMargin: f32, - ContactRelax: f32, - ContactWeighting: i32, - Restitution: i32, - JointLinearRelax: f32, - JointAngularRelax: f32, - JointLinearComply: f32, - JointAngularComply: f32, - AngularDamping: f32, + ContactRelax: f32, // 0.8 def + ContactWeighting: i32, // true + Restitution: i32, // false + JointLinearRelax: f32, // 0.7 def + JointAngularRelax: f32, // 0.4 def + JointLinearComply: f32, // 0 def + JointAngularComply: f32, // 0 def + AngularDamping: f32, // 0 def SoftRelax: f32, MaxGeomIter: i32, ContactsMax: i32, diff --git a/physics/shaders/InitDynamics.wgsl b/physics/shaders/InitDynamics.wgsl index fdcabf7e..3fdb41df 100644 --- a/physics/shaders/InitDynamics.wgsl +++ b/physics/shaders/InitDynamics.wgsl @@ -119,8 +119,10 @@ const BroadContactVarsN = ContactAPointX; //////// import: "control.go" alias JointControlVars = i32; //enums:enum const JointControlForce: JointControlVars = 0; -const JointTargetPos: JointControlVars = 1; -const JointTargetVel: JointControlVars = 2; +const JointTargetStiff: JointControlVars = 1; +const JointTargetPos: JointControlVars = 2; +const JointTargetDamp: JointControlVars = 3; +const JointTargetVel: JointControlVars = 4; //////// import: "dynamics.go" alias DynamicVars = i32; //enums:enum @@ -162,12 +164,12 @@ fn DynamicBody(idx: i32) -> i32 { return i32(bitcast(Dynamics[Index3D(Tenso //////// import: "enumgen.go" const BodyVarsN: BodyVars = 43; const ContactVarsN: ContactVars = 33; -const JointControlVarsN: JointControlVars = 3; +const JointControlVarsN: JointControlVars = 5; const DynamicVarsN: DynamicVars = 33; const GPUVarsN: GPUVars = 12; const JointTypesN: JointTypes = 7; const JointVarsN: JointVars = 50; -const JointDoFVarsN: JointDoFVars = 7; +const JointDoFVarsN: JointDoFVars = 5; const ShapesN: Shapes = 6; //////// import: "joint.go" @@ -237,8 +239,6 @@ const JointAxisY: JointDoFVars = 1; const JointAxisZ: JointDoFVars = 2; const JointLimitLower: JointDoFVars = 3; const JointLimitUpper: JointDoFVars = 4; -const JointStiff: JointDoFVars = 5; -const JointDamp: JointDoFVars = 6; //////// import: "params.go" struct PhysParams { @@ -246,14 +246,14 @@ struct PhysParams { Dt: f32, SubSteps: i32, ContactMargin: f32, - ContactRelax: f32, - ContactWeighting: i32, - Restitution: i32, - JointLinearRelax: f32, - JointAngularRelax: f32, - JointLinearComply: f32, - JointAngularComply: f32, - AngularDamping: f32, + ContactRelax: f32, // 0.8 def + ContactWeighting: i32, // true + Restitution: i32, // false + JointLinearRelax: f32, // 0.7 def + JointAngularRelax: f32, // 0.4 def + JointLinearComply: f32, // 0 def + JointAngularComply: f32, // 0 def + AngularDamping: f32, // 0 def SoftRelax: f32, MaxGeomIter: i32, ContactsMax: i32, diff --git a/physics/shaders/StepBodyContacts.wgsl b/physics/shaders/StepBodyContacts.wgsl index 4499b6d5..9e56f388 100644 --- a/physics/shaders/StepBodyContacts.wgsl +++ b/physics/shaders/StepBodyContacts.wgsl @@ -332,8 +332,10 @@ fn ContactConstraint(err: f32, q0A: vec4,q0B: vec4, mInvA: f32,mInvB: //////// import: "control.go" alias JointControlVars = i32; //enums:enum const JointControlForce: JointControlVars = 0; -const JointTargetPos: JointControlVars = 1; -const JointTargetVel: JointControlVars = 2; +const JointTargetStiff: JointControlVars = 1; +const JointTargetPos: JointControlVars = 2; +const JointTargetDamp: JointControlVars = 3; +const JointTargetVel: JointControlVars = 4; //////// import: "dynamics.go" alias DynamicVars = i32; //enums:enum @@ -386,12 +388,12 @@ fn DynamicAngDelta(idx: i32,cni: i32) -> vec3 { //////// import: "enumgen.go" const BodyVarsN: BodyVars = 43; const ContactVarsN: ContactVars = 33; -const JointControlVarsN: JointControlVars = 3; +const JointControlVarsN: JointControlVars = 5; const DynamicVarsN: DynamicVars = 33; const GPUVarsN: GPUVars = 12; const JointTypesN: JointTypes = 7; const JointVarsN: JointVars = 50; -const JointDoFVarsN: JointDoFVars = 7; +const JointDoFVarsN: JointDoFVars = 5; const ShapesN: Shapes = 6; //////// import: "joint.go" @@ -461,8 +463,6 @@ const JointAxisY: JointDoFVars = 1; const JointAxisZ: JointDoFVars = 2; const JointLimitLower: JointDoFVars = 3; const JointLimitUpper: JointDoFVars = 4; -const JointStiff: JointDoFVars = 5; -const JointDamp: JointDoFVars = 6; //////// import: "params.go" struct PhysParams { @@ -470,14 +470,14 @@ struct PhysParams { Dt: f32, SubSteps: i32, ContactMargin: f32, - ContactRelax: f32, - ContactWeighting: i32, - Restitution: i32, - JointLinearRelax: f32, - JointAngularRelax: f32, - JointLinearComply: f32, - JointAngularComply: f32, - AngularDamping: f32, + ContactRelax: f32, // 0.8 def + ContactWeighting: i32, // true + Restitution: i32, // false + JointLinearRelax: f32, // 0.7 def + JointAngularRelax: f32, // 0.4 def + JointLinearComply: f32, // 0 def + JointAngularComply: f32, // 0 def + AngularDamping: f32, // 0 def SoftRelax: f32, MaxGeomIter: i32, ContactsMax: i32, diff --git a/physics/shaders/StepBodyDeltas.wgsl b/physics/shaders/StepBodyDeltas.wgsl deleted file mode 100644 index e02f4272..00000000 --- a/physics/shaders/StepBodyDeltas.wgsl +++ /dev/null @@ -1,435 +0,0 @@ -// Code generated by "gosl"; DO NOT EDIT -// kernel: StepBodyDeltas - -// // Params are global parameters. -@group(0) @binding(0) -var TensorStrides: array; -@group(0) @binding(1) -var Params: array; -// // Bodies are the rigid body elements (dynamic and static), // specifying the constant, non-dynamic properties, // which is initial state for dynamics. // [body][BodyVarsN] -@group(1) @binding(0) -var Bodies: array; -// // Dynamics are the dynamic rigid body elements: these actually move. // Two alternating states are used: Params.Cur and Params.Next. // [dyn body][cur/next][DynamicVarsN] -@group(2) @binding(0) -var Dynamics: array; -// // JointControls are dynamic joint control inputs, per joint DoF // (in correspondence with [JointDoFs]). This can be uploaded to the // GPU at every step. // [dof][JointControlVarsN] - -alias GPUVars = i32; - -@compute @workgroup_size(64, 1, 1) -fn main(@builtin(workgroup_id) wgid: vec3, @builtin(num_workgroups) nwg: vec3, @builtin(local_invocation_index) loci: u32) { - let idx = loci + (wgid.x + wgid.y * nwg.x + wgid.z * nwg.x * nwg.y) * 64; - StepBodyDeltas(idx); -} - -fn Index2D(s0: u32, s1: u32, i0: u32, i1: u32) -> u32 { - return s0 * i0 + s1 * i1; -} - -fn Index3D(s0: u32, s1: u32, s2: u32, i0: u32, i1: u32, i2: u32) -> u32 { - return s0 * i0 + s1 * i1 + s2 * i2; -} - - -//////// import: "vars.go" - -//////// import: "body.go" -alias BodyVars = i32; //enums:enum -const BodyShape: BodyVars = 0; -const BodyDynamic: BodyVars = 1; -const BodyWorld: BodyVars = 2; -const BodyGroup: BodyVars = 3; -const BodyHSizeX: BodyVars = 4; -const BodyHSizeY: BodyVars = 5; -const BodyHSizeZ: BodyVars = 6; -const BodyThick: BodyVars = 7; -const BodyMass: BodyVars = 8; -const BodyInvMass: BodyVars = 9; -const BodyBounce: BodyVars = 10; -const BodyFriction: BodyVars = 11; -const BodyFrictionTortion: BodyVars = 12; -const BodyFrictionRolling: BodyVars = 13; -const BodyPosX: BodyVars = 14; -const BodyPosY: BodyVars = 15; -const BodyPosZ: BodyVars = 16; -const BodyQuatX: BodyVars = 17; -const BodyQuatY: BodyVars = 18; -const BodyQuatZ: BodyVars = 19; -const BodyQuatW: BodyVars = 20; -const BodyComX: BodyVars = 21; -const BodyComY: BodyVars = 22; -const BodyComZ: BodyVars = 23; -const BodyInertiaXX: BodyVars = 24; -const BodyInertiaYX: BodyVars = 25; -const BodyInertiaZX: BodyVars = 26; -const BodyInertiaXY: BodyVars = 27; -const BodyInertiaYY: BodyVars = 28; -const BodyInertiaZY: BodyVars = 29; -const BodyInertiaXZ: BodyVars = 30; -const BodyInertiaYZ: BodyVars = 31; -const BodyInertiaZZ: BodyVars = 32; -const BodyInvInertiaXX: BodyVars = 33; -const BodyInvInertiaYX: BodyVars = 34; -const BodyInvInertiaZX: BodyVars = 35; -const BodyInvInertiaXY: BodyVars = 36; -const BodyInvInertiaYY: BodyVars = 37; -const BodyInvInertiaZY: BodyVars = 38; -const BodyInvInertiaXZ: BodyVars = 39; -const BodyInvInertiaYZ: BodyVars = 40; -const BodyInvInertiaZZ: BodyVars = 41; -const BodyRadius: BodyVars = 42; -fn BodyCom(idx: i32) -> vec3 { - return vec3(Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComX))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComY))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComZ))]); -} -fn BodyInertia(idx: i32) -> mat3x3f { - return mat3x3f(Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInertiaXX))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInertiaYX))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInertiaZX))], - Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInertiaXY))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInertiaYY))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInertiaZY))], - Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInertiaXZ))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInertiaYZ))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInertiaZZ))]); -} -fn BodyInvInertia(idx: i32) -> mat3x3f { - return mat3x3f(Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInvInertiaXX))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInvInertiaYX))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInvInertiaZX))], - Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInvInertiaXY))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInvInertiaYY))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInvInertiaZY))], - Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInvInertiaXZ))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInvInertiaYZ))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInvInertiaZZ))]); -} - -//////// import: "contact.go" -alias ContactVars = i32; //enums:enum -const ContactA: ContactVars = 0; -const ContactB: ContactVars = 1; -const ContactPointIdx: ContactVars = 2; -const ContactAPointX: ContactVars = 3; -const ContactAPointY: ContactVars = 4; -const ContactAPointZ: ContactVars = 5; -const ContactBPointX: ContactVars = 6; -const ContactBPointY: ContactVars = 7; -const ContactBPointZ: ContactVars = 8; -const ContactAOffX: ContactVars = 9; -const ContactAOffY: ContactVars = 10; -const ContactAOffZ: ContactVars = 11; -const ContactBOffX: ContactVars = 12; -const ContactBOffY: ContactVars = 13; -const ContactBOffZ: ContactVars = 14; -const ContactAThick: ContactVars = 15; -const ContactBThick: ContactVars = 16; -const ContactNormX: ContactVars = 17; -const ContactNormY: ContactVars = 18; -const ContactNormZ: ContactVars = 19; -const ContactWeight: ContactVars = 20; -const ContactADeltaX: ContactVars = 21; -const ContactADeltaY: ContactVars = 22; -const ContactADeltaZ: ContactVars = 23; -const ContactAAngDeltaX: ContactVars = 24; -const ContactAAngDeltaY: ContactVars = 25; -const ContactAAngDeltaZ: ContactVars = 26; -const ContactBDeltaX: ContactVars = 27; -const ContactBDeltaY: ContactVars = 28; -const ContactBDeltaZ: ContactVars = 29; -const ContactBAngDeltaX: ContactVars = 30; -const ContactBAngDeltaY: ContactVars = 31; -const ContactBAngDeltaZ: ContactVars = 32; -const BroadContactVarsN = ContactAPointX; - -//////// import: "control.go" -alias JointControlVars = i32; //enums:enum -const JointControlForce: JointControlVars = 0; -const JointTargetPos: JointControlVars = 1; -const JointTargetVel: JointControlVars = 2; - -//////// import: "dynamics.go" -alias DynamicVars = i32; //enums:enum -const DynBody: DynamicVars = 0; -const DynPosX: DynamicVars = 1; -const DynPosY: DynamicVars = 2; -const DynPosZ: DynamicVars = 3; -const DynQuatX: DynamicVars = 4; -const DynQuatY: DynamicVars = 5; -const DynQuatZ: DynamicVars = 6; -const DynQuatW: DynamicVars = 7; -const DynVelX: DynamicVars = 8; -const DynVelY: DynamicVars = 9; -const DynVelZ: DynamicVars = 10; -const DynAngVelX: DynamicVars = 11; -const DynAngVelY: DynamicVars = 12; -const DynAngVelZ: DynamicVars = 13; -const DynAccX: DynamicVars = 14; -const DynAccY: DynamicVars = 15; -const DynAccZ: DynamicVars = 16; -const DynAngAccX: DynamicVars = 17; -const DynAngAccY: DynamicVars = 18; -const DynAngAccZ: DynamicVars = 19; -const DynForceX: DynamicVars = 20; -const DynForceY: DynamicVars = 21; -const DynForceZ: DynamicVars = 22; -const DynTorqueX: DynamicVars = 23; -const DynTorqueY: DynamicVars = 24; -const DynTorqueZ: DynamicVars = 25; -const DynDeltaX: DynamicVars = 26; -const DynDeltaY: DynamicVars = 27; -const DynDeltaZ: DynamicVars = 28; -const DynAngDeltaX: DynamicVars = 29; -const DynAngDeltaY: DynamicVars = 30; -const DynAngDeltaZ: DynamicVars = 31; -const DynContactWeight: DynamicVars = 32; -fn DynamicBody(idx: i32) -> i32 { return i32(bitcast(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(0), u32(DynBody))])); } -fn DynamicPos(idx: i32,cni: i32) -> vec3 { return vec3(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosZ))]); } -fn SetDynamicPos(idx: i32,cni: i32, pos: vec3) { Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosX))] = pos.x;; Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosY))] = pos.y;; Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosZ))] = pos.z; } -fn DynamicQuat(idx: i32,cni: i32) -> vec4 { return vec4(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatZ))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatW))]); } -fn SetDynamicQuat(idx: i32,cni: i32, rot: vec4) { Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatX))] = rot.x;; Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatY))] = rot.y;; Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatZ))] = rot.z;; Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatW))] = rot.w; } -fn DynamicDelta(idx: i32,cni: i32) -> vec3 { - return vec3(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaZ))]); -} -fn SetDynamicDelta(idx: i32,cni: i32, delta: vec3) { - Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaX))] = delta.x; - Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaY))] = delta.y; - Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaZ))] = delta.z; -} -fn DynamicAngDelta(idx: i32,cni: i32) -> vec3 { - return vec3(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaZ))]); -} -fn SetDynamicAngDelta(idx: i32,cni: i32, angDelta: vec3) { - Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaX))] = angDelta.x; - Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaY))] = angDelta.y; - Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaZ))] = angDelta.z; -} - -//////// import: "enumgen.go" -const BodyVarsN: BodyVars = 43; -const ContactVarsN: ContactVars = 33; -const JointControlVarsN: JointControlVars = 3; -const DynamicVarsN: DynamicVars = 33; -const GPUVarsN: GPUVars = 12; -const JointTypesN: JointTypes = 7; -const JointVarsN: JointVars = 50; -const JointDoFVarsN: JointDoFVars = 7; -const ShapesN: Shapes = 6; - -//////// import: "joint.go" -const JointLimitUnlimited = 1e10; -alias JointTypes = i32; //enums:enum -const Prismatic: JointTypes = 0; -const Revolute: JointTypes = 1; -const Ball: JointTypes = 2; -const Fixed: JointTypes = 3; -const Free: JointTypes = 4; -const Distance: JointTypes = 5; -const D6: JointTypes = 6; -alias JointVars = i32; //enums:enum -const JointType: JointVars = 0; -const JointEnabled: JointVars = 1; -const JointParent: JointVars = 2; -const JointChild: JointVars = 3; -const JointPPosX: JointVars = 4; -const JointPPosY: JointVars = 5; -const JointPPosZ: JointVars = 6; -const JointPQuatX: JointVars = 7; -const JointPQuatY: JointVars = 8; -const JointPQuatZ: JointVars = 9; -const JointPQuatW: JointVars = 10; -const JointCPosX: JointVars = 11; -const JointCPosY: JointVars = 12; -const JointCPosZ: JointVars = 13; -const JointCQuatX: JointVars = 14; -const JointCQuatY: JointVars = 15; -const JointCQuatZ: JointVars = 16; -const JointCQuatW: JointVars = 17; -const JointLinearDoFN: JointVars = 18; -const JointAngularDoFN: JointVars = 19; -const JointDoF1: JointVars = 20; -const JointDoF2: JointVars = 21; -const JointDoF3: JointVars = 22; -const JointDoF4: JointVars = 23; -const JointDoF5: JointVars = 24; -const JointDoF6: JointVars = 25; -const JointPForceX: JointVars = 26; -const JointPForceY: JointVars = 27; -const JointPForceZ: JointVars = 28; -const JointPTorqueX: JointVars = 29; -const JointPTorqueY: JointVars = 30; -const JointPTorqueZ: JointVars = 31; -const JointCForceX: JointVars = 32; -const JointCForceY: JointVars = 33; -const JointCForceZ: JointVars = 34; -const JointCTorqueX: JointVars = 35; -const JointCTorqueY: JointVars = 36; -const JointCTorqueZ: JointVars = 37; -const JointPDeltaX: JointVars = 38; -const JointPDeltaY: JointVars = 39; -const JointPDeltaZ: JointVars = 40; -const JointPAngDeltaX: JointVars = 41; -const JointPAngDeltaY: JointVars = 42; -const JointPAngDeltaZ: JointVars = 43; -const JointCDeltaX: JointVars = 44; -const JointCDeltaY: JointVars = 45; -const JointCDeltaZ: JointVars = 46; -const JointCAngDeltaX: JointVars = 47; -const JointCAngDeltaY: JointVars = 48; -const JointCAngDeltaZ: JointVars = 49; -alias JointDoFVars = i32; //enums:enum -const JointAxisX: JointDoFVars = 0; -const JointAxisY: JointDoFVars = 1; -const JointAxisZ: JointDoFVars = 2; -const JointLimitLower: JointDoFVars = 3; -const JointLimitUpper: JointDoFVars = 4; -const JointStiff: JointDoFVars = 5; -const JointDamp: JointDoFVars = 6; - -//////// import: "params.go" -struct PhysParams { - Iterations: i32, - Dt: f32, - SubSteps: i32, - ContactMargin: f32, - ContactRelax: f32, - ContactWeighting: i32, - Restitution: i32, - JointLinearRelax: f32, - JointAngularRelax: f32, - JointLinearComply: f32, - JointAngularComply: f32, - AngularDamping: f32, - SoftRelax: f32, - MaxGeomIter: i32, - ContactsMax: i32, - Cur: i32, - Next: i32, - BodiesN: i32, - DynamicsN: i32, - JointsN: i32, - JointDoFsN: i32, - BodyJointsMax: i32, - BodyCollidePairsN: i32, - pad: i32, - Gravity: vec4, -} - -//////// import: "shapecollide.go" -struct GeomData { - BodyIdx: i32, - Shape: Shapes, - MinSize: f32, - Thick: f32, - Radius: f32, - Size: vec3, - WbR: vec3, - WbQ: vec4, - BwR: vec3, - BwQ: vec4, -} - -//////// import: "shapegeom.go" - -//////// import: "shapes.go" -alias Shapes = i32; //enums:enum -const Plane: Shapes = 0; -const Sphere: Shapes = 1; -const Capsule: Shapes = 2; -const Cylinder: Shapes = 3; -const Box: Shapes = 4; -const Cone: Shapes = 5; - -//////// import: "slmath-matrix3.go" - -//////// import: "slmath-quaternion.go" -fn QuatLength(q: vec4) -> f32 { - return sqrt(q.x*q.x + q.y*q.y + q.z*q.z + q.w*q.w); -} -fn QuatNormalize(q: vec4) -> vec4 { - var nq = q; - var l = QuatLength(q); - if (l == 0) { - nq.x = f32(0); - nq.y = f32(0); - nq.z = f32(0); - nq.w = f32(1); - } else { - l = 1 / l; - nq.x *= l; - nq.y *= l; - nq.z *= l; - nq.w *= l; - }return nq; -} -fn MulQuatVector(q: vec4, v: vec3) -> vec3 { - var xyz = vec3(q.x, q.y, q.z); - var t = Cross3(xyz, v)*(2); -return v+(t*(q.w))+(Cross3(xyz, t)); -} -fn MulQuatVectorInverse(q: vec4, v: vec3) -> vec3 { - var xyz = vec3(q.x, q.y, q.z); - var t = Cross3(xyz, v)*(2); -return v-(t*(q.w))+(Cross3(xyz, t)); -} -fn MulQuats(a: vec4,b: vec4) -> vec4 { - var q: vec4; - q.x = a.x*b.w + a.w*b.x + a.y*b.z - a.z*b.y; - q.y = a.y*b.w + a.w*b.y + a.z*b.x - a.x*b.z; - q.z = a.z*b.w + a.w*b.z + a.x*b.y - a.y*b.x; - q.w = a.w*b.w - a.x*b.x - a.y*b.y - a.z*b.z; -return q; -} - -//////// import: "slmath-vector2.go" - -//////// import: "slmath-vector3.go" -fn Length3(v: vec3) -> f32 { - return sqrt(v.x*v.x + v.y*v.y + v.z*v.z); -} -fn Cross3(v: vec3,o: vec3) -> vec3 { - return vec3(v.y*o.z-v.z*o.y, v.z*o.x-v.x*o.z, v.x*o.y-v.y*o.x); -} - -//////// import: "step.go" - -//////// import: "step_body.go" -fn StepBodyDeltas(i: u32) { //gosl:kernel - var params = Params[0]; - var di = i32(i); - if (di >= params.DynamicsN) { - return; - } - var bi = DynamicBody(di); - var invMass = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyInvMass))]; - if (invMass == 0) { - return; // no updates - } - var inertia = BodyInertia(bi); - var invInertia = BodyInvInertia(bi); - var r0 = DynamicPos(di, params.Next); - var q0 = DynamicQuat(di, params.Next); - var v0 = DynamicDelta(di, params.Next); - var w0 = DynamicAngDelta(di, params.Next); - var weight = f32(1.0); - if (params.ContactWeighting == 1) { - var cWt = Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(di), u32(params.Next), u32(DynContactWeight))]; - if (cWt > 0) { - weight = 1.0 / cWt; - } - } - var dp = v0*(invMass * weight); - var dq = w0*(weight); - var wb = MulQuatVectorInverse(q0, w0); - var dwb = invInertia*(MulQuatVectorInverse(q0, dq)); - var tb = Cross3(dwb, inertia*(wb+(dwb)))+(Cross3(wb, inertia*(dwb))); - var dw1 = MulQuatVector(q0, dwb-(invInertia*(tb)*(params.Dt))); - var q1 = q0+(MulQuats(vec4(dw1.x, dw1.y, dw1.z, 0), q0)*(0.5 * params.Dt)); - q1 = QuatNormalize(q1); - var com = BodyCom(bi); - var pcom = MulQuatVector(q0, com)+(r0); - var p1 = pcom+(dp*(params.Dt)); - p1 = p1-(MulQuatVector(q1, com)); - var v1 = v0+(dp); - var w1 = w0+(dw1); - if (Length3(v1) < 1e-4) { - v1 = vec3(0, 0, 0); - } - if (Length3(w1) < 1e-4) { - w1 = vec3(0, 0, 0); - } - SetDynamicPos(di, params.Next, p1); - SetDynamicQuat(di, params.Next, q1); - SetDynamicDelta(di, params.Next, v1); - SetDynamicAngDelta(di, params.Next, w1); - Params[0] = params; -} - -//////// import: "step_joint.go" \ No newline at end of file diff --git a/physics/shaders/StepIntegrateBodies.wgsl b/physics/shaders/StepIntegrateBodies.wgsl index 6432de8f..f006940e 100644 --- a/physics/shaders/StepIntegrateBodies.wgsl +++ b/physics/shaders/StepIntegrateBodies.wgsl @@ -132,8 +132,10 @@ const BroadContactVarsN = ContactAPointX; //////// import: "control.go" alias JointControlVars = i32; //enums:enum const JointControlForce: JointControlVars = 0; -const JointTargetPos: JointControlVars = 1; -const JointTargetVel: JointControlVars = 2; +const JointTargetStiff: JointControlVars = 1; +const JointTargetPos: JointControlVars = 2; +const JointTargetDamp: JointControlVars = 3; +const JointTargetVel: JointControlVars = 4; //////// import: "dynamics.go" alias DynamicVars = i32; //enums:enum @@ -185,12 +187,12 @@ fn SetDynamicAngDelta(idx: i32,cni: i32, angDelta: vec3) { Dynamics[Index3D //////// import: "enumgen.go" const BodyVarsN: BodyVars = 43; const ContactVarsN: ContactVars = 33; -const JointControlVarsN: JointControlVars = 3; +const JointControlVarsN: JointControlVars = 5; const DynamicVarsN: DynamicVars = 33; const GPUVarsN: GPUVars = 12; const JointTypesN: JointTypes = 7; const JointVarsN: JointVars = 50; -const JointDoFVarsN: JointDoFVars = 7; +const JointDoFVarsN: JointDoFVars = 5; const ShapesN: Shapes = 6; //////// import: "joint.go" @@ -260,8 +262,6 @@ const JointAxisY: JointDoFVars = 1; const JointAxisZ: JointDoFVars = 2; const JointLimitLower: JointDoFVars = 3; const JointLimitUpper: JointDoFVars = 4; -const JointStiff: JointDoFVars = 5; -const JointDamp: JointDoFVars = 6; //////// import: "params.go" struct PhysParams { @@ -269,14 +269,14 @@ struct PhysParams { Dt: f32, SubSteps: i32, ContactMargin: f32, - ContactRelax: f32, - ContactWeighting: i32, - Restitution: i32, - JointLinearRelax: f32, - JointAngularRelax: f32, - JointLinearComply: f32, - JointAngularComply: f32, - AngularDamping: f32, + ContactRelax: f32, // 0.8 def + ContactWeighting: i32, // true + Restitution: i32, // false + JointLinearRelax: f32, // 0.7 def + JointAngularRelax: f32, // 0.4 def + JointLinearComply: f32, // 0 def + JointAngularComply: f32, // 0 def + AngularDamping: f32, // 0 def SoftRelax: f32, MaxGeomIter: i32, ContactsMax: i32, diff --git a/physics/shaders/StepJointForces.wgsl b/physics/shaders/StepJointForces.wgsl index 68993b78..4e9ab597 100644 --- a/physics/shaders/StepJointForces.wgsl +++ b/physics/shaders/StepJointForces.wgsl @@ -128,8 +128,10 @@ const BroadContactVarsN = ContactAPointX; //////// import: "control.go" alias JointControlVars = i32; //enums:enum const JointControlForce: JointControlVars = 0; -const JointTargetPos: JointControlVars = 1; -const JointTargetVel: JointControlVars = 2; +const JointTargetStiff: JointControlVars = 1; +const JointTargetPos: JointControlVars = 2; +const JointTargetDamp: JointControlVars = 3; +const JointTargetVel: JointControlVars = 4; fn JointControl(idx: i32,dof: i32, vr: JointControlVars) -> f32 { return JointControls[Index2D(TensorStrides[100], TensorStrides[101], u32(JointDoFIndex(idx, dof)), u32(vr))]; } @@ -176,12 +178,12 @@ fn DynamicQuat(idx: i32,cni: i32) -> vec4 { return vec4(Dynamics[Index //////// import: "enumgen.go" const BodyVarsN: BodyVars = 43; const ContactVarsN: ContactVars = 33; -const JointControlVarsN: JointControlVars = 3; +const JointControlVarsN: JointControlVars = 5; const DynamicVarsN: DynamicVars = 33; const GPUVarsN: GPUVars = 12; const JointTypesN: JointTypes = 7; const JointVarsN: JointVars = 50; -const JointDoFVarsN: JointDoFVars = 7; +const JointDoFVarsN: JointDoFVars = 5; const ShapesN: Shapes = 6; //////// import: "joint.go" @@ -299,8 +301,6 @@ const JointAxisY: JointDoFVars = 1; const JointAxisZ: JointDoFVars = 2; const JointLimitLower: JointDoFVars = 3; const JointLimitUpper: JointDoFVars = 4; -const JointStiff: JointDoFVars = 5; -const JointDamp: JointDoFVars = 6; fn JointAxisDoF(didx: i32) -> vec3 { return vec3(JointDoFs[Index2D(TensorStrides[20], TensorStrides[21], u32(didx), u32(JointAxisX))], JointDoFs[Index2D(TensorStrides[20], TensorStrides[21], u32(didx), u32(JointAxisY))], JointDoFs[Index2D(TensorStrides[20], TensorStrides[21], u32(didx), u32(JointAxisZ))]); } @@ -314,14 +314,14 @@ struct PhysParams { Dt: f32, SubSteps: i32, ContactMargin: f32, - ContactRelax: f32, - ContactWeighting: i32, - Restitution: i32, - JointLinearRelax: f32, - JointAngularRelax: f32, - JointLinearComply: f32, - JointAngularComply: f32, - AngularDamping: f32, + ContactRelax: f32, // 0.8 def + ContactWeighting: i32, // true + Restitution: i32, // false + JointLinearRelax: f32, // 0.7 def + JointAngularRelax: f32, // 0.4 def + JointLinearComply: f32, // 0 def + JointAngularComply: f32, // 0 def + AngularDamping: f32, // 0 def SoftRelax: f32, MaxGeomIter: i32, ContactsMax: i32, @@ -390,6 +390,9 @@ return dp+(xP); //////// import: "slmath-vector2.go" //////// import: "slmath-vector3.go" +fn Negate3(v: vec3) -> vec3 { + return vec3(-v.x, -v.y, -v.z); +} fn Cross3(v: vec3,o: vec3) -> vec3 { return vec3(v.y*o.z-v.z*o.y, v.z*o.x-v.x*o.z, v.x*o.y-v.y*o.x); } @@ -405,6 +408,11 @@ fn StepJointForces(i: u32) { //gosl:kernel if (ji >= params.JointsN) { return; } + var zv = vec3(0, 0, 0); + SetJointPForce(ji, zv); + SetJointCForce(ji, zv); + SetJointPTorque(ji, zv); + SetJointCTorque(ji, zv); var jt = GetJointType(ji); if (!GetJointEnabled(ji)) { return; @@ -466,9 +474,9 @@ fn StepJointForces(i: u32) { //gosl:kernel } } } - SetJointPForce(ji, f); + SetJointPForce(ji, Negate3(f)); SetJointCForce(ji, f); - SetJointPTorque(ji, t+(Cross3(dP, f))); + SetJointPTorque(ji, Negate3(t+(Cross3(dP, f)))); SetJointCTorque(ji, t+(Cross3(dC, f))); Params[0] = params; } \ No newline at end of file diff --git a/physics/shaders/StepSolveJoints.wgsl b/physics/shaders/StepSolveJoints.wgsl index 3258fe83..6967f1e4 100644 --- a/physics/shaders/StepSolveJoints.wgsl +++ b/physics/shaders/StepSolveJoints.wgsl @@ -133,8 +133,10 @@ const BroadContactVarsN = ContactAPointX; //////// import: "control.go" alias JointControlVars = i32; //enums:enum const JointControlForce: JointControlVars = 0; -const JointTargetPos: JointControlVars = 1; -const JointTargetVel: JointControlVars = 2; +const JointTargetStiff: JointControlVars = 1; +const JointTargetPos: JointControlVars = 2; +const JointTargetDamp: JointControlVars = 3; +const JointTargetVel: JointControlVars = 4; fn JointControl(idx: i32,dof: i32, vr: JointControlVars) -> f32 { return JointControls[Index2D(TensorStrides[100], TensorStrides[101], u32(JointDoFIndex(idx, dof)), u32(vr))]; } @@ -187,12 +189,12 @@ fn DynamicAngDelta(idx: i32,cni: i32) -> vec3 { //////// import: "enumgen.go" const BodyVarsN: BodyVars = 43; const ContactVarsN: ContactVars = 33; -const JointControlVarsN: JointControlVars = 3; +const JointControlVarsN: JointControlVars = 5; const DynamicVarsN: DynamicVars = 33; const GPUVarsN: GPUVars = 12; const JointTypesN: JointTypes = 7; const JointVarsN: JointVars = 50; -const JointDoFVarsN: JointDoFVars = 7; +const JointDoFVarsN: JointDoFVars = 5; const ShapesN: Shapes = 6; //////// import: "joint.go" @@ -316,8 +318,6 @@ const JointAxisY: JointDoFVars = 1; const JointAxisZ: JointDoFVars = 2; const JointLimitLower: JointDoFVars = 3; const JointLimitUpper: JointDoFVars = 4; -const JointStiff: JointDoFVars = 5; -const JointDamp: JointDoFVars = 6; fn JointAxisDoF(didx: i32) -> vec3 { return vec3(JointDoFs[Index2D(TensorStrides[20], TensorStrides[21], u32(didx), u32(JointAxisX))], JointDoFs[Index2D(TensorStrides[20], TensorStrides[21], u32(didx), u32(JointAxisY))], JointDoFs[Index2D(TensorStrides[20], TensorStrides[21], u32(didx), u32(JointAxisZ))]); } @@ -334,14 +334,14 @@ struct PhysParams { Dt: f32, SubSteps: i32, ContactMargin: f32, - ContactRelax: f32, - ContactWeighting: i32, - Restitution: i32, - JointLinearRelax: f32, - JointAngularRelax: f32, - JointLinearComply: f32, - JointAngularComply: f32, - AngularDamping: f32, + ContactRelax: f32, // 0.8 def + ContactWeighting: i32, // true + Restitution: i32, // false + JointLinearRelax: f32, // 0.7 def + JointAngularRelax: f32, // 0.4 def + JointLinearComply: f32, // 0 def + JointAngularComply: f32, // 0 def + AngularDamping: f32, // 0 def SoftRelax: f32, MaxGeomIter: i32, ContactsMax: i32, @@ -528,6 +528,11 @@ fn StepSolveJoints(i: u32) { //gosl:kernel if (ji >= params.JointsN) { return; } + var zv = vec3(0, 0, 0); + SetJointPDelta(ji, zv); + SetJointCDelta(ji, zv); + SetJointPAngDelta(ji, zv); + SetJointCAngDelta(ji, zv); var jt = GetJointType(ji); if (jt == Free || !GetJointEnabled(ji)) { return; @@ -613,8 +618,8 @@ fn StepSolveJoints(i: u32) { //gosl:kernel var derr = Dot3(linearP, vP) + Dot3(linearC, vC) + Dot3(angularP, wP) + Dot3(angularC, wC); var lambdaIn = f32(0.0); var compliance = params.JointLinearComply; - var ke = JointDoF(ji, i32(i32(0)), JointStiff); - var kd = JointDoF(ji, i32(i32(0)), JointDamp); + var ke = JointControl(ji, i32(i32(0)), JointTargetStiff); + var kd = JointControl(ji, i32(i32(0)), JointTargetDamp); if (ke > 0.0) { compliance = 1.0 / ke; } @@ -625,7 +630,7 @@ fn StepSolveJoints(i: u32) { //gosl:kernel linDeltaC = linDeltaC+(linearC*(dLambda * params.JointLinearRelax)); angDeltaC = angDeltaC+(angularC*(dLambda * params.JointAngularRelax)); } - } else if (jLinearN > 0) { // compute joint target, stiffness, damping + } else { // compute joint target, stiffness, damping var axisLimitsD: vec3; var axisLimitsA: vec3; var axisTargetPosKeD: vec3; @@ -635,8 +640,8 @@ fn StepSolveJoints(i: u32) { //gosl:kernel for (var dof=0; dof 0.0) { // has position control @@ -698,7 +703,7 @@ fn StepSolveJoints(i: u32) { //gosl:kernel } } } - if (jAngularN > 0) { // angular + if (jt == Fixed || jt == Prismatic || jt == Revolute || jt == D6) { var qP = xwPQ; var qC = xwCQ; if (QuatDot(qP, qC) < 0) { @@ -751,8 +756,8 @@ fn StepSolveJoints(i: u32) { //gosl:kernel var di = dof + jLinearN; var axis = JointAxis(ji, di); JointAxisLimitsUpdate(dof, axis, JointDoF(ji, di, JointLimitLower), JointDoF(ji, di, JointLimitUpper), &axisLimitsD, &axisLimitsA); - var ke = JointDoF(ji, di, JointStiff); - var kd = JointDoF(ji, di, JointDamp); + var ke = JointControl(ji, di, JointTargetStiff); + var kd = JointControl(ji, di, JointTargetDamp); var targetPos = JointControl(ji, di, JointTargetPos); var targetVel = JointControl(ji, di, JointTargetVel); if (ke > 0.0) { // has position control @@ -808,8 +813,8 @@ fn StepSolveJoints(i: u32) { //gosl:kernel } } SetJointPDelta(ji, linDeltaP); - SetJointPAngDelta(ji, angDeltaP); SetJointCDelta(ji, linDeltaC); + SetJointPAngDelta(ji, angDeltaP); SetJointCAngDelta(ji, angDeltaC); Params[0] = params; } diff --git a/physics/step.go b/physics/step.go index 91e51b0d..00914836 100644 --- a/physics/step.go +++ b/physics/step.go @@ -121,7 +121,7 @@ func (wl *World) StepIntegrateBodies() { func (wl *World) StepSolveJoints() { params := GetParams(0) RunStepSolveJoints(int(params.JointsN)) - RunDeltasFromJoints(int(params.DynamicsN)) + RunStepBodyJointDeltas(int(params.DynamicsN)) } func (wl *World) StepBodyContacts() { @@ -130,10 +130,10 @@ func (wl *World) StepBodyContacts() { cmax := int(ContactsN.Values[0]) if cmax > 0 { RunStepBodyContacts(cmax) - RunDeltasFromContacts(int(params.DynamicsN)) + RunStepBodyContactDeltas(int(params.DynamicsN)) } } else { RunStepBodyContacts(int(params.ContactsMax)) // just do max and let the routines bail - RunDeltasFromContacts(int(params.DynamicsN)) + RunStepBodyContactDeltas(int(params.DynamicsN)) } } diff --git a/physics/step.goal b/physics/step.goal index 1af562ab..8b7db9a4 100644 --- a/physics/step.goal +++ b/physics/step.goal @@ -119,7 +119,7 @@ func (wl *World) StepIntegrateBodies() { func (wl *World) StepSolveJoints() { params := GetParams(0) RunStepSolveJoints(int(params.JointsN)) - RunDeltasFromJoints(int(params.DynamicsN)) + RunStepBodyJointDeltas(int(params.DynamicsN)) } func (wl *World) StepBodyContacts() { @@ -128,10 +128,10 @@ func (wl *World) StepBodyContacts() { cmax := int(ContactsN.Values[0]) if cmax > 0 { RunStepBodyContacts(cmax) - RunDeltasFromContacts(int(params.DynamicsN)) + RunStepBodyContactDeltas(int(params.DynamicsN)) } } else { RunStepBodyContacts(int(params.ContactsMax)) // just do max and let the routines bail - RunDeltasFromContacts(int(params.DynamicsN)) + RunStepBodyContactDeltas(int(params.DynamicsN)) } } diff --git a/physics/step_body.go b/physics/step_body.go index 358df988..8919c21b 100644 --- a/physics/step_body.go +++ b/physics/step_body.go @@ -84,40 +84,6 @@ func ForcesFromJoints(i uint32) { //gosl:kernel SetDynamicTorque(di, params.Next, tt) } -// DeltasFromJoints gathers deltas, angDeltas from joints per dynamic -func DeltasFromJoints(i uint32) { //gosl:kernel - params := GetParams(0) - di := int32(i) - if di >= params.DynamicsN { - return - } - np := BodyJoints.Value(int(di), int(0), int(0)) - nc := BodyJoints.Value(int(di), int(1), int(0)) - - td := math32.Vec3(0, 0, 0) - ta := math32.Vec3(0, 0, 0) - for i := int32(1); i <= np; i++ { - ji := BodyJoints.Value(int(di), int(0), int(i)) - d := JointPDelta(ji) - td = td.Add(d) - a := JointPAngDelta(ji) - ta = ta.Add(a) - } - for i := int32(1); i <= nc; i++ { - ji := BodyJoints.Value(int(di), int(1), int(i)) - d := JointCDelta(ji) - td = td.Add(d) - a := JointCAngDelta(ji) - ta = ta.Add(a) - } - v0 := DynamicDelta(di, params.Next) - w0 := DynamicAngDelta(di, params.Next) - // fmt.Println(params.Next, "joint v:", v0, td) - - SetDynamicDelta(di, params.Next, td.Add(v0)) - SetDynamicAngDelta(di, params.Next, ta.Add(w0)) -} - // newton: solvers/solver.py: integrate_rigid_body // StepIntegrateBodies applies forces to update pos and deltas @@ -175,21 +141,50 @@ func StepIntegrateBodies(i uint32) { //gosl:kernel SetDynamicAngDelta(di, params.Next, w1) } -// newton: solvers/xpbd/kernels.py: apply_body_deltas - -// StepBodyDeltas updates Next position with deltas. -func StepBodyDeltas(i uint32) { //gosl:kernel +// StepBodyJointDeltas gathers raw deltas, angDeltas from joints per dynamic +// and computes updated deltas integrated via StepBodyDeltas. +func StepBodyJointDeltas(i uint32) { //gosl:kernel params := GetParams(0) di := int32(i) if di >= params.DynamicsN { return } bi := DynamicBody(di) - invMass := Bodies.Value(int(bi), int(BodyInvMass)) if invMass == 0 { return // no updates } + + np := BodyJoints.Value(int(di), int(0), int(0)) + nc := BodyJoints.Value(int(di), int(1), int(0)) + + linDel := math32.Vec3(0, 0, 0) + angDel := math32.Vec3(0, 0, 0) + for i := int32(1); i <= np; i++ { + ji := BodyJoints.Value(int(di), int(0), int(i)) + d := JointPDelta(ji) + linDel = linDel.Add(d) + a := JointPAngDelta(ji) + angDel = angDel.Add(a) + } + for i := int32(1); i <= nc; i++ { + ji := BodyJoints.Value(int(di), int(1), int(i)) + d := JointCDelta(ji) + linDel = linDel.Add(d) + a := JointCAngDelta(ji) + angDel = angDel.Add(a) + } + StepBodyDeltas(di, bi, false, 0, linDel, angDel) +} + +// newton: solvers/xpbd/kernels.py: apply_body_deltas + +// StepBodyDeltas updates Next position with deltas from joints +// or contacts (if contacts true). +func StepBodyDeltas(di, bi int32, contacts bool, cWt float32, linDel, angDel math32.Vector3) { + params := GetParams(0) + + invMass := Bodies.Value(int(bi), int(BodyInvMass)) inertia := BodyInertia(bi) invInertia := BodyInvInertia(bi) @@ -202,15 +197,15 @@ func StepBodyDeltas(i uint32) { //gosl:kernel w0 := DynamicAngDelta(di, params.Next) weight := float32(1.0) - if params.ContactWeighting.IsTrue() { - cWt := Dynamics.Value(int(di), int(params.Next), int(DynContactWeight)) + if contacts && params.ContactWeighting.IsTrue() { if cWt > 0 { weight = 1.0 / cWt } } - dp := v0.MulScalar(invMass * weight) - dq := w0.MulScalar(weight) + // weighted + dp := linDel.MulScalar(invMass * weight) + dq := angDel.MulScalar(weight) wb := slmath.MulQuatVectorInverse(q0, w0) dwb := invInertia.MulVector3(slmath.MulQuatVectorInverse(q0, dq)) diff --git a/physics/step_body.goal b/physics/step_body.goal index a5d4cb72..36c3567e 100644 --- a/physics/step_body.goal +++ b/physics/step_body.goal @@ -82,40 +82,6 @@ func ForcesFromJoints(i uint32) { //gosl:kernel SetDynamicTorque(di, params.Next, tt) } -// DeltasFromJoints gathers deltas, angDeltas from joints per dynamic -func DeltasFromJoints(i uint32) { //gosl:kernel - params := GetParams(0) - di := int32(i) - if di >= params.DynamicsN { - return - } - np := BodyJoints[di, 0, 0] - nc := BodyJoints[di, 1, 0] - - td := math32.Vec3(0,0,0) - ta := math32.Vec3(0,0,0) - for i := int32(1); i <= np; i++ { - ji := BodyJoints[di, 0, i] - d := JointPDelta(ji) - td = td.Add(d) - a := JointPAngDelta(ji) - ta = ta.Add(a) - } - for i := int32(1); i <= nc; i++ { - ji := BodyJoints[di, 1, i] - d := JointCDelta(ji) - td = td.Add(d) - a := JointCAngDelta(ji) - ta = ta.Add(a) - } - v0 := DynamicDelta(di, params.Next) - w0 := DynamicAngDelta(di, params.Next) - // fmt.Println(params.Next, "joint v:", v0, td) - - SetDynamicDelta(di, params.Next, td.Add(v0)) - SetDynamicAngDelta(di, params.Next, ta.Add(w0)) -} - // newton: solvers/solver.py: integrate_rigid_body // StepIntegrateBodies applies forces to update pos and deltas @@ -173,21 +139,50 @@ func StepIntegrateBodies(i uint32) { //gosl:kernel SetDynamicAngDelta(di, params.Next, w1) } -// newton: solvers/xpbd/kernels.py: apply_body_deltas - -// StepBodyDeltas updates Next position with deltas. -func StepBodyDeltas(i uint32) { //gosl:kernel +// StepBodyJointDeltas gathers raw deltas, angDeltas from joints per dynamic +// and computes updated deltas integrated via StepBodyDeltas. +func StepBodyJointDeltas(i uint32) { //gosl:kernel params := GetParams(0) di := int32(i) if di >= params.DynamicsN { return } bi := DynamicBody(di) - invMass := Bodies[bi, BodyInvMass] if invMass == 0 { return // no updates } + + np := BodyJoints[di, 0, 0] + nc := BodyJoints[di, 1, 0] + + linDel := math32.Vec3(0,0,0) + angDel := math32.Vec3(0,0,0) + for i := int32(1); i <= np; i++ { + ji := BodyJoints[di, 0, i] + d := JointPDelta(ji) + linDel = linDel.Add(d) + a := JointPAngDelta(ji) + angDel = angDel.Add(a) + } + for i := int32(1); i <= nc; i++ { + ji := BodyJoints[di, 1, i] + d := JointCDelta(ji) + linDel = linDel.Add(d) + a := JointCAngDelta(ji) + angDel = angDel.Add(a) + } + StepBodyDeltas(di, bi, false, 0, linDel, angDel) +} + +// newton: solvers/xpbd/kernels.py: apply_body_deltas + +// StepBodyDeltas updates Next position with deltas from joints +// or contacts (if contacts true). +func StepBodyDeltas(di, bi int32, contacts bool, cWt float32, linDel, angDel math32.Vector3) { + params := GetParams(0) + + invMass := Bodies[bi, BodyInvMass] inertia := BodyInertia(bi) invInertia := BodyInvInertia(bi) @@ -200,15 +195,15 @@ func StepBodyDeltas(i uint32) { //gosl:kernel w0 := DynamicAngDelta(di, params.Next) weight := float32(1.0) - if params.ContactWeighting.IsTrue() { - cWt := Dynamics[di, params.Next, DynContactWeight] + if contacts && params.ContactWeighting.IsTrue() { if cWt > 0 { weight = 1.0 / cWt } } - dp := v0.MulScalar(invMass * weight) - dq := w0.MulScalar(weight) + // weighted + dp := linDel.MulScalar(invMass * weight) + dq := angDel.MulScalar(weight) wb := slmath.MulQuatVectorInverse(q0, w0) dwb := invInertia.MulVector3(slmath.MulQuatVectorInverse(q0, dq)) diff --git a/physics/step_joint.go b/physics/step_joint.go index e3197704..98e58ff9 100644 --- a/physics/step_joint.go +++ b/physics/step_joint.go @@ -36,6 +36,12 @@ func StepJointForces(i uint32) { //gosl:kernel if ji >= params.JointsN { return } + zv := math32.Vec3(0, 0, 0) + SetJointPForce(ji, zv) + SetJointCForce(ji, zv) + SetJointPTorque(ji, zv) + SetJointCTorque(ji, zv) + jt := GetJointType(ji) if !GetJointEnabled(ji) { return @@ -104,21 +110,28 @@ func StepJointForces(i uint32) { //gosl:kernel } } // These are unique to joint: aggregate into dynamics Next in [ForcesFromJoints] - SetJointPForce(ji, f) + SetJointPForce(ji, slmath.Negate3(f)) SetJointCForce(ji, f) - SetJointPTorque(ji, t.Add(slmath.Cross3(dP, f))) + SetJointPTorque(ji, slmath.Negate3(t.Add(slmath.Cross3(dP, f)))) SetJointCTorque(ji, t.Add(slmath.Cross3(dC, f))) } // newton: solvers/xpbd/kernels.py: solve_body_joints -// StepSolveJoints fixes joints after updating bodies. +// StepSolveJoints applies target positions to joints. func StepSolveJoints(i uint32) { //gosl:kernel params := GetParams(0) ji := int32(i) if ji >= params.JointsN { return } + + zv := math32.Vec3(0, 0, 0) + SetJointPDelta(ji, zv) + SetJointCDelta(ji, zv) + SetJointPAngDelta(ji, zv) + SetJointCAngDelta(ji, zv) + jt := GetJointType(ji) if jt == Free || !GetJointEnabled(ji) { return @@ -216,8 +229,8 @@ func StepSolveJoints(i uint32) { //gosl:kernel derr := slmath.Dot3(linearP, vP) + slmath.Dot3(linearC, vC) + slmath.Dot3(angularP, wP) + slmath.Dot3(angularC, wC) lambdaIn := float32(0.0) compliance := params.JointLinearComply - ke := JointDoF(ji, 0, JointStiff) - kd := JointDoF(ji, 0, JointDamp) + ke := JointControl(ji, 0, JointTargetStiff) + kd := JointControl(ji, 0, JointTargetDamp) if ke > 0.0 { compliance = 1.0 / ke } @@ -228,7 +241,8 @@ func StepSolveJoints(i uint32) { //gosl:kernel linDeltaC = linDeltaC.Add(linearC.MulScalar(dLambda * params.JointLinearRelax)) angDeltaC = angDeltaC.Add(angularC.MulScalar(dLambda * params.JointAngularRelax)) } - } else if jLinearN > 0 { // compute joint target, stiffness, damping + } else { // compute joint target, stiffness, damping + // all joints impose linear constraints! var axisLimitsD, axisLimitsA math32.Vector3 var axisTargetPosKeD, axisTargetPosKeA math32.Vector3 var axisTargetVelKdD, axisTargetVelKdA math32.Vector3 @@ -236,8 +250,8 @@ func StepSolveJoints(i uint32) { //gosl:kernel for dof := range jLinearN { axis := JointAxis(ji, dof) JointAxisLimitsUpdate(dof, axis, JointDoF(ji, dof, JointLimitLower), JointDoF(ji, dof, JointLimitUpper), &axisLimitsD, &axisLimitsA) - ke := JointDoF(ji, dof, JointStiff) - kd := JointDoF(ji, dof, JointDamp) + ke := JointControl(ji, dof, JointTargetStiff) + kd := JointControl(ji, dof, JointTargetDamp) targetPos := JointControl(ji, dof, JointTargetPos) targetVel := JointControl(ji, dof, JointTargetVel) if ke > 0.0 { // has position control @@ -312,7 +326,7 @@ func StepSolveJoints(i uint32) { //gosl:kernel } } } - if jAngularN > 0 { // angular + if jt == Fixed || jt == Prismatic || jt == Revolute || jt == D6 { qP := xwPQ qC := xwCQ // make quats lie in same hemisphere @@ -376,8 +390,8 @@ func StepSolveJoints(i uint32) { //gosl:kernel di := dof + jLinearN axis := JointAxis(ji, di) JointAxisLimitsUpdate(dof, axis, JointDoF(ji, di, JointLimitLower), JointDoF(ji, di, JointLimitUpper), &axisLimitsD, &axisLimitsA) - ke := JointDoF(ji, di, JointStiff) - kd := JointDoF(ji, di, JointDamp) + ke := JointControl(ji, di, JointTargetStiff) + kd := JointControl(ji, di, JointTargetDamp) targetPos := JointControl(ji, di, JointTargetPos) targetVel := JointControl(ji, di, JointTargetVel) if ke > 0.0 { // has position control @@ -450,8 +464,8 @@ func StepSolveJoints(i uint32) { //gosl:kernel // These are unique to joint: aggregate into dynamics Next in separate step. SetJointPDelta(ji, linDeltaP) - SetJointPAngDelta(ji, angDeltaP) SetJointCDelta(ji, linDeltaC) + SetJointPAngDelta(ji, angDeltaP) SetJointCAngDelta(ji, angDeltaC) } diff --git a/physics/step_joint.goal b/physics/step_joint.goal index f042735c..6f5e703e 100644 --- a/physics/step_joint.goal +++ b/physics/step_joint.goal @@ -34,6 +34,12 @@ func StepJointForces(i uint32) { //gosl:kernel if ji >= params.JointsN { return } + zv := math32.Vec3(0, 0, 0) + SetJointPForce(ji, zv) + SetJointCForce(ji, zv) + SetJointPTorque(ji, zv) + SetJointCTorque(ji, zv) + jt := GetJointType(ji) if !GetJointEnabled(ji) { return @@ -102,21 +108,28 @@ func StepJointForces(i uint32) { //gosl:kernel } } // These are unique to joint: aggregate into dynamics Next in [ForcesFromJoints] - SetJointPForce(ji, f) + SetJointPForce(ji, slmath.Negate3(f)) SetJointCForce(ji, f) - SetJointPTorque(ji, t.Add(slmath.Cross3(dP, f))) + SetJointPTorque(ji, slmath.Negate3(t.Add(slmath.Cross3(dP, f)))) SetJointCTorque(ji, t.Add(slmath.Cross3(dC, f))) } // newton: solvers/xpbd/kernels.py: solve_body_joints -// StepSolveJoints fixes joints after updating bodies. +// StepSolveJoints applies target positions to joints. func StepSolveJoints(i uint32) { //gosl:kernel params := GetParams(0) ji := int32(i) if ji >= params.JointsN { return } + + zv := math32.Vec3(0, 0, 0) + SetJointPDelta(ji, zv) + SetJointCDelta(ji, zv) + SetJointPAngDelta(ji, zv) + SetJointCAngDelta(ji, zv) + jt := GetJointType(ji) if jt == Free || !GetJointEnabled(ji) { return @@ -214,8 +227,8 @@ func StepSolveJoints(i uint32) { //gosl:kernel derr := slmath.Dot3(linearP, vP) + slmath.Dot3(linearC, vC) + slmath.Dot3(angularP, wP) + slmath.Dot3(angularC, wC) lambdaIn := float32(0.0) compliance := params.JointLinearComply - ke := JointDoF(ji, 0, JointStiff) - kd := JointDoF(ji, 0, JointDamp) + ke := JointControl(ji, 0, JointTargetStiff) + kd := JointControl(ji, 0, JointTargetDamp) if ke > 0.0 { compliance = 1.0 / ke } @@ -226,7 +239,8 @@ func StepSolveJoints(i uint32) { //gosl:kernel linDeltaC = linDeltaC.Add(linearC.MulScalar(dLambda * params.JointLinearRelax)) angDeltaC = angDeltaC.Add(angularC.MulScalar(dLambda * params.JointAngularRelax)) } - } else if jLinearN > 0 { // compute joint target, stiffness, damping + } else { // compute joint target, stiffness, damping + // all joints impose linear constraints! var axisLimitsD, axisLimitsA math32.Vector3 var axisTargetPosKeD, axisTargetPosKeA math32.Vector3 var axisTargetVelKdD, axisTargetVelKdA math32.Vector3 @@ -234,8 +248,8 @@ func StepSolveJoints(i uint32) { //gosl:kernel for dof := range jLinearN { axis := JointAxis(ji, dof) JointAxisLimitsUpdate(dof, axis, JointDoF(ji, dof, JointLimitLower), JointDoF(ji, dof, JointLimitUpper), &axisLimitsD, &axisLimitsA) - ke := JointDoF(ji, dof, JointStiff) - kd := JointDoF(ji, dof, JointDamp) + ke := JointControl(ji, dof, JointTargetStiff) + kd := JointControl(ji, dof, JointTargetDamp) targetPos := JointControl(ji, dof, JointTargetPos) targetVel := JointControl(ji, dof, JointTargetVel) if ke > 0.0 { // has position control @@ -310,7 +324,7 @@ func StepSolveJoints(i uint32) { //gosl:kernel } } } - if jAngularN > 0 { // angular + if jt == Fixed || jt == Prismatic || jt == Revolute || jt == D6 { qP := xwPQ qC := xwCQ // make quats lie in same hemisphere @@ -374,8 +388,8 @@ func StepSolveJoints(i uint32) { //gosl:kernel di := dof + jLinearN axis := JointAxis(ji, di) JointAxisLimitsUpdate(dof, axis, JointDoF(ji, di, JointLimitLower), JointDoF(ji, di, JointLimitUpper), &axisLimitsD, &axisLimitsA) - ke := JointDoF(ji, di, JointStiff) - kd := JointDoF(ji, di, JointDamp) + ke := JointControl(ji, di, JointTargetStiff) + kd := JointControl(ji, di, JointTargetDamp) targetPos := JointControl(ji, di, JointTargetPos) targetVel := JointControl(ji, di, JointTargetVel) if ke > 0.0 { // has position control @@ -448,8 +462,8 @@ func StepSolveJoints(i uint32) { //gosl:kernel // These are unique to joint: aggregate into dynamics Next in separate step. SetJointPDelta(ji, linDeltaP) - SetJointPAngDelta(ji, angDeltaP) SetJointCDelta(ji, linDeltaC) + SetJointPAngDelta(ji, angDeltaP) SetJointCAngDelta(ji, angDeltaC) } From f6026284b6afe74728797d5d4e4647cf599cd01a Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Tue, 23 Dec 2025 18:54:26 +0100 Subject: [PATCH 48/97] physics: add shaders --- physics/shaders/StepBodyContactDeltas.wgsl | 489 +++++++++++++++++++++ physics/shaders/StepBodyJointDeltas.wgsl | 476 ++++++++++++++++++++ 2 files changed, 965 insertions(+) create mode 100644 physics/shaders/StepBodyContactDeltas.wgsl create mode 100644 physics/shaders/StepBodyJointDeltas.wgsl diff --git a/physics/shaders/StepBodyContactDeltas.wgsl b/physics/shaders/StepBodyContactDeltas.wgsl new file mode 100644 index 00000000..44f767b9 --- /dev/null +++ b/physics/shaders/StepBodyContactDeltas.wgsl @@ -0,0 +1,489 @@ +// Code generated by "gosl"; DO NOT EDIT +// kernel: StepBodyContactDeltas + +// // Params are global parameters. +@group(0) @binding(0) +var TensorStrides: array; +@group(0) @binding(1) +var Params: array; +// // Bodies are the rigid body elements (dynamic and static), // specifying the constant, non-dynamic properties, // which is initial state for dynamics. // [body][BodyVarsN] +@group(1) @binding(0) +var Bodies: array; +// // Dynamics are the dynamic rigid body elements: these actually move. // Two alternating states are used: Params.Cur and Params.Next. // [dyn body][cur/next][DynamicVarsN] +@group(2) @binding(0) +var Dynamics: array; +@group(2) @binding(3) +var ContactsN: array; +@group(2) @binding(4) +var Contacts: array; +// // JointControls are dynamic joint control inputs, per joint DoF // (in correspondence with [JointDoFs]). This can be uploaded to the // GPU at every step. // [dof][JointControlVarsN] + +alias GPUVars = i32; + +@compute @workgroup_size(64, 1, 1) +fn main(@builtin(workgroup_id) wgid: vec3, @builtin(num_workgroups) nwg: vec3, @builtin(local_invocation_index) loci: u32) { + let idx = loci + (wgid.x + wgid.y * nwg.x + wgid.z * nwg.x * nwg.y) * 64; + StepBodyContactDeltas(idx); +} + +fn Index2D(s0: u32, s1: u32, i0: u32, i1: u32) -> u32 { + return s0 * i0 + s1 * i1; +} + +fn Index3D(s0: u32, s1: u32, s2: u32, i0: u32, i1: u32, i2: u32) -> u32 { + return s0 * i0 + s1 * i1 + s2 * i2; +} + +fn Index1D(s0: u32, i0: u32) -> u32 { + return s0 * i0; +} + + +//////// import: "vars.go" + +//////// import: "body.go" +alias BodyVars = i32; //enums:enum +const BodyShape: BodyVars = 0; +const BodyDynamic: BodyVars = 1; +const BodyWorld: BodyVars = 2; +const BodyGroup: BodyVars = 3; +const BodyHSizeX: BodyVars = 4; +const BodyHSizeY: BodyVars = 5; +const BodyHSizeZ: BodyVars = 6; +const BodyThick: BodyVars = 7; +const BodyMass: BodyVars = 8; +const BodyInvMass: BodyVars = 9; +const BodyBounce: BodyVars = 10; +const BodyFriction: BodyVars = 11; +const BodyFrictionTortion: BodyVars = 12; +const BodyFrictionRolling: BodyVars = 13; +const BodyPosX: BodyVars = 14; +const BodyPosY: BodyVars = 15; +const BodyPosZ: BodyVars = 16; +const BodyQuatX: BodyVars = 17; +const BodyQuatY: BodyVars = 18; +const BodyQuatZ: BodyVars = 19; +const BodyQuatW: BodyVars = 20; +const BodyComX: BodyVars = 21; +const BodyComY: BodyVars = 22; +const BodyComZ: BodyVars = 23; +const BodyInertiaXX: BodyVars = 24; +const BodyInertiaYX: BodyVars = 25; +const BodyInertiaZX: BodyVars = 26; +const BodyInertiaXY: BodyVars = 27; +const BodyInertiaYY: BodyVars = 28; +const BodyInertiaZY: BodyVars = 29; +const BodyInertiaXZ: BodyVars = 30; +const BodyInertiaYZ: BodyVars = 31; +const BodyInertiaZZ: BodyVars = 32; +const BodyInvInertiaXX: BodyVars = 33; +const BodyInvInertiaYX: BodyVars = 34; +const BodyInvInertiaZX: BodyVars = 35; +const BodyInvInertiaXY: BodyVars = 36; +const BodyInvInertiaYY: BodyVars = 37; +const BodyInvInertiaZY: BodyVars = 38; +const BodyInvInertiaXZ: BodyVars = 39; +const BodyInvInertiaYZ: BodyVars = 40; +const BodyInvInertiaZZ: BodyVars = 41; +const BodyRadius: BodyVars = 42; +fn BodyCom(idx: i32) -> vec3 { + return vec3(Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComX))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComY))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComZ))]); +} +fn BodyInertia(idx: i32) -> mat3x3f { + return mat3x3f(Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInertiaXX))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInertiaYX))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInertiaZX))], + Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInertiaXY))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInertiaYY))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInertiaZY))], + Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInertiaXZ))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInertiaYZ))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInertiaZZ))]); +} +fn BodyInvInertia(idx: i32) -> mat3x3f { + return mat3x3f(Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInvInertiaXX))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInvInertiaYX))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInvInertiaZX))], + Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInvInertiaXY))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInvInertiaYY))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInvInertiaZY))], + Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInvInertiaXZ))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInvInertiaYZ))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInvInertiaZZ))]); +} + +//////// import: "contact.go" +alias ContactVars = i32; //enums:enum +const ContactA: ContactVars = 0; +const ContactB: ContactVars = 1; +const ContactPointIdx: ContactVars = 2; +const ContactAPointX: ContactVars = 3; +const ContactAPointY: ContactVars = 4; +const ContactAPointZ: ContactVars = 5; +const ContactBPointX: ContactVars = 6; +const ContactBPointY: ContactVars = 7; +const ContactBPointZ: ContactVars = 8; +const ContactAOffX: ContactVars = 9; +const ContactAOffY: ContactVars = 10; +const ContactAOffZ: ContactVars = 11; +const ContactBOffX: ContactVars = 12; +const ContactBOffY: ContactVars = 13; +const ContactBOffZ: ContactVars = 14; +const ContactAThick: ContactVars = 15; +const ContactBThick: ContactVars = 16; +const ContactNormX: ContactVars = 17; +const ContactNormY: ContactVars = 18; +const ContactNormZ: ContactVars = 19; +const ContactWeight: ContactVars = 20; +const ContactADeltaX: ContactVars = 21; +const ContactADeltaY: ContactVars = 22; +const ContactADeltaZ: ContactVars = 23; +const ContactAAngDeltaX: ContactVars = 24; +const ContactAAngDeltaY: ContactVars = 25; +const ContactAAngDeltaZ: ContactVars = 26; +const ContactBDeltaX: ContactVars = 27; +const ContactBDeltaY: ContactVars = 28; +const ContactBDeltaZ: ContactVars = 29; +const ContactBAngDeltaX: ContactVars = 30; +const ContactBAngDeltaY: ContactVars = 31; +const ContactBAngDeltaZ: ContactVars = 32; +const BroadContactVarsN = ContactAPointX; +fn GetContactA(idx: i32) -> i32 { return i32(bitcast(Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactA))])); } +fn GetContactB(idx: i32) -> i32 { return i32(bitcast(Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactB))])); } +fn ContactADelta(idx: i32) -> vec3 { + return vec3(Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactADeltaX))], Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactADeltaY))], Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactADeltaZ))]); +} +fn ContactAAngDelta(idx: i32) -> vec3 { + return vec3(Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactAAngDeltaX))], Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactAAngDeltaY))], Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactAAngDeltaZ))]); +} +fn ContactBDelta(idx: i32) -> vec3 { + return vec3(Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactBDeltaX))], Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactBDeltaY))], Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactBDeltaZ))]); +} +fn ContactBAngDelta(idx: i32) -> vec3 { + return vec3(Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactBAngDeltaX))], Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactBAngDeltaY))], Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactBAngDeltaZ))]); +} +fn StepBodyContactDeltas(i: u32) { //gosl:kernel + var params = Params[0]; + var di = i32(i); + if (di >= params.DynamicsN) { + return; + } + var bi = DynamicBody(di); + var invMass = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyInvMass))]; + if (invMass == 0) { + return; // no updates + } + var cmax = ContactsN[0]; + var linDel = vec3(0, 0, 0); + var angDel = vec3(0, 0, 0); + var tw = f32(0); + for (var ci=0; ci i32 { return i32(bitcast(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(0), u32(DynBody))])); } +fn DynamicPos(idx: i32,cni: i32) -> vec3 { return vec3(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosZ))]); } +fn SetDynamicPos(idx: i32,cni: i32, pos: vec3) { Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosX))] = pos.x;; Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosY))] = pos.y;; Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosZ))] = pos.z; } +fn DynamicQuat(idx: i32,cni: i32) -> vec4 { return vec4(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatZ))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatW))]); } +fn SetDynamicQuat(idx: i32,cni: i32, rot: vec4) { Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatX))] = rot.x;; Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatY))] = rot.y;; Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatZ))] = rot.z;; Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatW))] = rot.w; } +fn DynamicDelta(idx: i32,cni: i32) -> vec3 { + return vec3(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaZ))]); +} +fn SetDynamicDelta(idx: i32,cni: i32, delta: vec3) { + Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaX))] = delta.x; + Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaY))] = delta.y; + Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaZ))] = delta.z; +} +fn DynamicAngDelta(idx: i32,cni: i32) -> vec3 { + return vec3(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaZ))]); +} +fn SetDynamicAngDelta(idx: i32,cni: i32, angDelta: vec3) { + Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaX))] = angDelta.x; + Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaY))] = angDelta.y; + Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaZ))] = angDelta.z; +} + +//////// import: "enumgen.go" +const BodyVarsN: BodyVars = 43; +const ContactVarsN: ContactVars = 33; +const JointControlVarsN: JointControlVars = 5; +const DynamicVarsN: DynamicVars = 33; +const GPUVarsN: GPUVars = 12; +const JointTypesN: JointTypes = 7; +const JointVarsN: JointVars = 50; +const JointDoFVarsN: JointDoFVars = 5; +const ShapesN: Shapes = 6; + +//////// import: "joint.go" +const JointLimitUnlimited = 1e10; +alias JointTypes = i32; //enums:enum +const Prismatic: JointTypes = 0; +const Revolute: JointTypes = 1; +const Ball: JointTypes = 2; +const Fixed: JointTypes = 3; +const Free: JointTypes = 4; +const Distance: JointTypes = 5; +const D6: JointTypes = 6; +alias JointVars = i32; //enums:enum +const JointType: JointVars = 0; +const JointEnabled: JointVars = 1; +const JointParent: JointVars = 2; +const JointChild: JointVars = 3; +const JointPPosX: JointVars = 4; +const JointPPosY: JointVars = 5; +const JointPPosZ: JointVars = 6; +const JointPQuatX: JointVars = 7; +const JointPQuatY: JointVars = 8; +const JointPQuatZ: JointVars = 9; +const JointPQuatW: JointVars = 10; +const JointCPosX: JointVars = 11; +const JointCPosY: JointVars = 12; +const JointCPosZ: JointVars = 13; +const JointCQuatX: JointVars = 14; +const JointCQuatY: JointVars = 15; +const JointCQuatZ: JointVars = 16; +const JointCQuatW: JointVars = 17; +const JointLinearDoFN: JointVars = 18; +const JointAngularDoFN: JointVars = 19; +const JointDoF1: JointVars = 20; +const JointDoF2: JointVars = 21; +const JointDoF3: JointVars = 22; +const JointDoF4: JointVars = 23; +const JointDoF5: JointVars = 24; +const JointDoF6: JointVars = 25; +const JointPForceX: JointVars = 26; +const JointPForceY: JointVars = 27; +const JointPForceZ: JointVars = 28; +const JointPTorqueX: JointVars = 29; +const JointPTorqueY: JointVars = 30; +const JointPTorqueZ: JointVars = 31; +const JointCForceX: JointVars = 32; +const JointCForceY: JointVars = 33; +const JointCForceZ: JointVars = 34; +const JointCTorqueX: JointVars = 35; +const JointCTorqueY: JointVars = 36; +const JointCTorqueZ: JointVars = 37; +const JointPDeltaX: JointVars = 38; +const JointPDeltaY: JointVars = 39; +const JointPDeltaZ: JointVars = 40; +const JointPAngDeltaX: JointVars = 41; +const JointPAngDeltaY: JointVars = 42; +const JointPAngDeltaZ: JointVars = 43; +const JointCDeltaX: JointVars = 44; +const JointCDeltaY: JointVars = 45; +const JointCDeltaZ: JointVars = 46; +const JointCAngDeltaX: JointVars = 47; +const JointCAngDeltaY: JointVars = 48; +const JointCAngDeltaZ: JointVars = 49; +alias JointDoFVars = i32; //enums:enum +const JointAxisX: JointDoFVars = 0; +const JointAxisY: JointDoFVars = 1; +const JointAxisZ: JointDoFVars = 2; +const JointLimitLower: JointDoFVars = 3; +const JointLimitUpper: JointDoFVars = 4; + +//////// import: "params.go" +struct PhysParams { + Iterations: i32, + Dt: f32, + SubSteps: i32, + ContactMargin: f32, + ContactRelax: f32, // 0.8 def + ContactWeighting: i32, // true + Restitution: i32, // false + JointLinearRelax: f32, // 0.7 def + JointAngularRelax: f32, // 0.4 def + JointLinearComply: f32, // 0 def + JointAngularComply: f32, // 0 def + AngularDamping: f32, // 0 def + SoftRelax: f32, + MaxGeomIter: i32, + ContactsMax: i32, + Cur: i32, + Next: i32, + BodiesN: i32, + DynamicsN: i32, + JointsN: i32, + JointDoFsN: i32, + BodyJointsMax: i32, + BodyCollidePairsN: i32, + pad: i32, + Gravity: vec4, +} + +//////// import: "shapecollide.go" +struct GeomData { + BodyIdx: i32, + Shape: Shapes, + MinSize: f32, + Thick: f32, + Radius: f32, + Size: vec3, + WbR: vec3, + WbQ: vec4, + BwR: vec3, + BwQ: vec4, +} + +//////// import: "shapegeom.go" + +//////// import: "shapes.go" +alias Shapes = i32; //enums:enum +const Plane: Shapes = 0; +const Sphere: Shapes = 1; +const Capsule: Shapes = 2; +const Cylinder: Shapes = 3; +const Box: Shapes = 4; +const Cone: Shapes = 5; + +//////// import: "slmath-matrix3.go" + +//////// import: "slmath-quaternion.go" +fn QuatLength(q: vec4) -> f32 { + return sqrt(q.x*q.x + q.y*q.y + q.z*q.z + q.w*q.w); +} +fn QuatNormalize(q: vec4) -> vec4 { + var nq = q; + var l = QuatLength(q); + if (l == 0) { + nq.x = f32(0); + nq.y = f32(0); + nq.z = f32(0); + nq.w = f32(1); + } else { + l = 1 / l; + nq.x *= l; + nq.y *= l; + nq.z *= l; + nq.w *= l; + }return nq; +} +fn MulQuatVector(q: vec4, v: vec3) -> vec3 { + var xyz = vec3(q.x, q.y, q.z); + var t = Cross3(xyz, v)*(2); +return v+(t*(q.w))+(Cross3(xyz, t)); +} +fn MulQuatVectorInverse(q: vec4, v: vec3) -> vec3 { + var xyz = vec3(q.x, q.y, q.z); + var t = Cross3(xyz, v)*(2); +return v-(t*(q.w))+(Cross3(xyz, t)); +} +fn MulQuats(a: vec4,b: vec4) -> vec4 { + var q: vec4; + q.x = a.x*b.w + a.w*b.x + a.y*b.z - a.z*b.y; + q.y = a.y*b.w + a.w*b.y + a.z*b.x - a.x*b.z; + q.z = a.z*b.w + a.w*b.z + a.x*b.y - a.y*b.x; + q.w = a.w*b.w - a.x*b.x - a.y*b.y - a.z*b.z; +return q; +} + +//////// import: "slmath-vector2.go" + +//////// import: "slmath-vector3.go" +fn Length3(v: vec3) -> f32 { + return sqrt(v.x*v.x + v.y*v.y + v.z*v.z); +} +fn Cross3(v: vec3,o: vec3) -> vec3 { + return vec3(v.y*o.z-v.z*o.y, v.z*o.x-v.x*o.z, v.x*o.y-v.y*o.x); +} + +//////// import: "step.go" + +//////// import: "step_body.go" +fn StepBodyDeltas(di: i32,bi: i32, contacts: bool, cWt: f32, linDel: vec3,angDel: vec3) { + var params = Params[0]; + var invMass = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyInvMass))]; + var inertia = BodyInertia(bi); + var invInertia = BodyInvInertia(bi); + var r0 = DynamicPos(di, params.Next); + var q0 = DynamicQuat(di, params.Next); + var v0 = DynamicDelta(di, params.Next); + var w0 = DynamicAngDelta(di, params.Next); + var weight = f32(1.0); + if (contacts && params.ContactWeighting == 1) { + if (cWt > 0) { + weight = 1.0 / cWt; + } + } + var dp = linDel*(invMass * weight); + var dq = angDel*(weight); + var wb = MulQuatVectorInverse(q0, w0); + var dwb = invInertia*(MulQuatVectorInverse(q0, dq)); + var tb = Cross3(dwb, inertia*(wb+(dwb)))+(Cross3(wb, inertia*(dwb))); + var dw1 = MulQuatVector(q0, dwb-(invInertia*(tb)*(params.Dt))); + var q1 = q0+(MulQuats(vec4(dw1.x, dw1.y, dw1.z, 0), q0)*(0.5 * params.Dt)); + q1 = QuatNormalize(q1); + var com = BodyCom(bi); + var pcom = MulQuatVector(q0, com)+(r0); + var p1 = pcom+(dp*(params.Dt)); + p1 = p1-(MulQuatVector(q1, com)); + var v1 = v0+(dp); + var w1 = w0+(dw1); + if (Length3(v1) < 1e-4) { + v1 = vec3(0, 0, 0); + } + if (Length3(w1) < 1e-4) { + w1 = vec3(0, 0, 0); + } + SetDynamicPos(di, params.Next, p1); + SetDynamicQuat(di, params.Next, q1); + SetDynamicDelta(di, params.Next, v1); + SetDynamicAngDelta(di, params.Next, w1); + Params[0] = params; +} + +//////// import: "step_joint.go" \ No newline at end of file diff --git a/physics/shaders/StepBodyJointDeltas.wgsl b/physics/shaders/StepBodyJointDeltas.wgsl new file mode 100644 index 00000000..cfbc6de1 --- /dev/null +++ b/physics/shaders/StepBodyJointDeltas.wgsl @@ -0,0 +1,476 @@ +// Code generated by "gosl"; DO NOT EDIT +// kernel: StepBodyJointDeltas + +// // Params are global parameters. +@group(0) @binding(0) +var TensorStrides: array; +@group(0) @binding(1) +var Params: array; +// // Bodies are the rigid body elements (dynamic and static), // specifying the constant, non-dynamic properties, // which is initial state for dynamics. // [body][BodyVarsN] +@group(1) @binding(0) +var Bodies: array; +@group(1) @binding(1) +var Joints: array; +@group(1) @binding(3) +var BodyJoints: array; +// // Dynamics are the dynamic rigid body elements: these actually move. // Two alternating states are used: Params.Cur and Params.Next. // [dyn body][cur/next][DynamicVarsN] +@group(2) @binding(0) +var Dynamics: array; +// // JointControls are dynamic joint control inputs, per joint DoF // (in correspondence with [JointDoFs]). This can be uploaded to the // GPU at every step. // [dof][JointControlVarsN] + +alias GPUVars = i32; + +@compute @workgroup_size(64, 1, 1) +fn main(@builtin(workgroup_id) wgid: vec3, @builtin(num_workgroups) nwg: vec3, @builtin(local_invocation_index) loci: u32) { + let idx = loci + (wgid.x + wgid.y * nwg.x + wgid.z * nwg.x * nwg.y) * 64; + StepBodyJointDeltas(idx); +} + +fn Index2D(s0: u32, s1: u32, i0: u32, i1: u32) -> u32 { + return s0 * i0 + s1 * i1; +} + +fn Index3D(s0: u32, s1: u32, s2: u32, i0: u32, i1: u32, i2: u32) -> u32 { + return s0 * i0 + s1 * i1 + s2 * i2; +} + + +//////// import: "vars.go" + +//////// import: "body.go" +alias BodyVars = i32; //enums:enum +const BodyShape: BodyVars = 0; +const BodyDynamic: BodyVars = 1; +const BodyWorld: BodyVars = 2; +const BodyGroup: BodyVars = 3; +const BodyHSizeX: BodyVars = 4; +const BodyHSizeY: BodyVars = 5; +const BodyHSizeZ: BodyVars = 6; +const BodyThick: BodyVars = 7; +const BodyMass: BodyVars = 8; +const BodyInvMass: BodyVars = 9; +const BodyBounce: BodyVars = 10; +const BodyFriction: BodyVars = 11; +const BodyFrictionTortion: BodyVars = 12; +const BodyFrictionRolling: BodyVars = 13; +const BodyPosX: BodyVars = 14; +const BodyPosY: BodyVars = 15; +const BodyPosZ: BodyVars = 16; +const BodyQuatX: BodyVars = 17; +const BodyQuatY: BodyVars = 18; +const BodyQuatZ: BodyVars = 19; +const BodyQuatW: BodyVars = 20; +const BodyComX: BodyVars = 21; +const BodyComY: BodyVars = 22; +const BodyComZ: BodyVars = 23; +const BodyInertiaXX: BodyVars = 24; +const BodyInertiaYX: BodyVars = 25; +const BodyInertiaZX: BodyVars = 26; +const BodyInertiaXY: BodyVars = 27; +const BodyInertiaYY: BodyVars = 28; +const BodyInertiaZY: BodyVars = 29; +const BodyInertiaXZ: BodyVars = 30; +const BodyInertiaYZ: BodyVars = 31; +const BodyInertiaZZ: BodyVars = 32; +const BodyInvInertiaXX: BodyVars = 33; +const BodyInvInertiaYX: BodyVars = 34; +const BodyInvInertiaZX: BodyVars = 35; +const BodyInvInertiaXY: BodyVars = 36; +const BodyInvInertiaYY: BodyVars = 37; +const BodyInvInertiaZY: BodyVars = 38; +const BodyInvInertiaXZ: BodyVars = 39; +const BodyInvInertiaYZ: BodyVars = 40; +const BodyInvInertiaZZ: BodyVars = 41; +const BodyRadius: BodyVars = 42; +fn BodyCom(idx: i32) -> vec3 { + return vec3(Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComX))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComY))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComZ))]); +} +fn BodyInertia(idx: i32) -> mat3x3f { + return mat3x3f(Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInertiaXX))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInertiaYX))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInertiaZX))], + Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInertiaXY))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInertiaYY))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInertiaZY))], + Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInertiaXZ))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInertiaYZ))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInertiaZZ))]); +} +fn BodyInvInertia(idx: i32) -> mat3x3f { + return mat3x3f(Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInvInertiaXX))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInvInertiaYX))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInvInertiaZX))], + Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInvInertiaXY))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInvInertiaYY))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInvInertiaZY))], + Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInvInertiaXZ))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInvInertiaYZ))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInvInertiaZZ))]); +} + +//////// import: "contact.go" +alias ContactVars = i32; //enums:enum +const ContactA: ContactVars = 0; +const ContactB: ContactVars = 1; +const ContactPointIdx: ContactVars = 2; +const ContactAPointX: ContactVars = 3; +const ContactAPointY: ContactVars = 4; +const ContactAPointZ: ContactVars = 5; +const ContactBPointX: ContactVars = 6; +const ContactBPointY: ContactVars = 7; +const ContactBPointZ: ContactVars = 8; +const ContactAOffX: ContactVars = 9; +const ContactAOffY: ContactVars = 10; +const ContactAOffZ: ContactVars = 11; +const ContactBOffX: ContactVars = 12; +const ContactBOffY: ContactVars = 13; +const ContactBOffZ: ContactVars = 14; +const ContactAThick: ContactVars = 15; +const ContactBThick: ContactVars = 16; +const ContactNormX: ContactVars = 17; +const ContactNormY: ContactVars = 18; +const ContactNormZ: ContactVars = 19; +const ContactWeight: ContactVars = 20; +const ContactADeltaX: ContactVars = 21; +const ContactADeltaY: ContactVars = 22; +const ContactADeltaZ: ContactVars = 23; +const ContactAAngDeltaX: ContactVars = 24; +const ContactAAngDeltaY: ContactVars = 25; +const ContactAAngDeltaZ: ContactVars = 26; +const ContactBDeltaX: ContactVars = 27; +const ContactBDeltaY: ContactVars = 28; +const ContactBDeltaZ: ContactVars = 29; +const ContactBAngDeltaX: ContactVars = 30; +const ContactBAngDeltaY: ContactVars = 31; +const ContactBAngDeltaZ: ContactVars = 32; +const BroadContactVarsN = ContactAPointX; + +//////// import: "control.go" +alias JointControlVars = i32; //enums:enum +const JointControlForce: JointControlVars = 0; +const JointTargetStiff: JointControlVars = 1; +const JointTargetPos: JointControlVars = 2; +const JointTargetDamp: JointControlVars = 3; +const JointTargetVel: JointControlVars = 4; + +//////// import: "dynamics.go" +alias DynamicVars = i32; //enums:enum +const DynBody: DynamicVars = 0; +const DynPosX: DynamicVars = 1; +const DynPosY: DynamicVars = 2; +const DynPosZ: DynamicVars = 3; +const DynQuatX: DynamicVars = 4; +const DynQuatY: DynamicVars = 5; +const DynQuatZ: DynamicVars = 6; +const DynQuatW: DynamicVars = 7; +const DynVelX: DynamicVars = 8; +const DynVelY: DynamicVars = 9; +const DynVelZ: DynamicVars = 10; +const DynAngVelX: DynamicVars = 11; +const DynAngVelY: DynamicVars = 12; +const DynAngVelZ: DynamicVars = 13; +const DynAccX: DynamicVars = 14; +const DynAccY: DynamicVars = 15; +const DynAccZ: DynamicVars = 16; +const DynAngAccX: DynamicVars = 17; +const DynAngAccY: DynamicVars = 18; +const DynAngAccZ: DynamicVars = 19; +const DynForceX: DynamicVars = 20; +const DynForceY: DynamicVars = 21; +const DynForceZ: DynamicVars = 22; +const DynTorqueX: DynamicVars = 23; +const DynTorqueY: DynamicVars = 24; +const DynTorqueZ: DynamicVars = 25; +const DynDeltaX: DynamicVars = 26; +const DynDeltaY: DynamicVars = 27; +const DynDeltaZ: DynamicVars = 28; +const DynAngDeltaX: DynamicVars = 29; +const DynAngDeltaY: DynamicVars = 30; +const DynAngDeltaZ: DynamicVars = 31; +const DynContactWeight: DynamicVars = 32; +fn DynamicBody(idx: i32) -> i32 { return i32(bitcast(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(0), u32(DynBody))])); } +fn DynamicPos(idx: i32,cni: i32) -> vec3 { return vec3(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosZ))]); } +fn SetDynamicPos(idx: i32,cni: i32, pos: vec3) { Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosX))] = pos.x;; Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosY))] = pos.y;; Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosZ))] = pos.z; } +fn DynamicQuat(idx: i32,cni: i32) -> vec4 { return vec4(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatZ))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatW))]); } +fn SetDynamicQuat(idx: i32,cni: i32, rot: vec4) { Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatX))] = rot.x;; Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatY))] = rot.y;; Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatZ))] = rot.z;; Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatW))] = rot.w; } +fn DynamicDelta(idx: i32,cni: i32) -> vec3 { + return vec3(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaZ))]); +} +fn SetDynamicDelta(idx: i32,cni: i32, delta: vec3) { + Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaX))] = delta.x; + Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaY))] = delta.y; + Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaZ))] = delta.z; +} +fn DynamicAngDelta(idx: i32,cni: i32) -> vec3 { + return vec3(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaZ))]); +} +fn SetDynamicAngDelta(idx: i32,cni: i32, angDelta: vec3) { + Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaX))] = angDelta.x; + Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaY))] = angDelta.y; + Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaZ))] = angDelta.z; +} + +//////// import: "enumgen.go" +const BodyVarsN: BodyVars = 43; +const ContactVarsN: ContactVars = 33; +const JointControlVarsN: JointControlVars = 5; +const DynamicVarsN: DynamicVars = 33; +const GPUVarsN: GPUVars = 12; +const JointTypesN: JointTypes = 7; +const JointVarsN: JointVars = 50; +const JointDoFVarsN: JointDoFVars = 5; +const ShapesN: Shapes = 6; + +//////// import: "joint.go" +const JointLimitUnlimited = 1e10; +alias JointTypes = i32; //enums:enum +const Prismatic: JointTypes = 0; +const Revolute: JointTypes = 1; +const Ball: JointTypes = 2; +const Fixed: JointTypes = 3; +const Free: JointTypes = 4; +const Distance: JointTypes = 5; +const D6: JointTypes = 6; +alias JointVars = i32; //enums:enum +const JointType: JointVars = 0; +const JointEnabled: JointVars = 1; +const JointParent: JointVars = 2; +const JointChild: JointVars = 3; +const JointPPosX: JointVars = 4; +const JointPPosY: JointVars = 5; +const JointPPosZ: JointVars = 6; +const JointPQuatX: JointVars = 7; +const JointPQuatY: JointVars = 8; +const JointPQuatZ: JointVars = 9; +const JointPQuatW: JointVars = 10; +const JointCPosX: JointVars = 11; +const JointCPosY: JointVars = 12; +const JointCPosZ: JointVars = 13; +const JointCQuatX: JointVars = 14; +const JointCQuatY: JointVars = 15; +const JointCQuatZ: JointVars = 16; +const JointCQuatW: JointVars = 17; +const JointLinearDoFN: JointVars = 18; +const JointAngularDoFN: JointVars = 19; +const JointDoF1: JointVars = 20; +const JointDoF2: JointVars = 21; +const JointDoF3: JointVars = 22; +const JointDoF4: JointVars = 23; +const JointDoF5: JointVars = 24; +const JointDoF6: JointVars = 25; +const JointPForceX: JointVars = 26; +const JointPForceY: JointVars = 27; +const JointPForceZ: JointVars = 28; +const JointPTorqueX: JointVars = 29; +const JointPTorqueY: JointVars = 30; +const JointPTorqueZ: JointVars = 31; +const JointCForceX: JointVars = 32; +const JointCForceY: JointVars = 33; +const JointCForceZ: JointVars = 34; +const JointCTorqueX: JointVars = 35; +const JointCTorqueY: JointVars = 36; +const JointCTorqueZ: JointVars = 37; +const JointPDeltaX: JointVars = 38; +const JointPDeltaY: JointVars = 39; +const JointPDeltaZ: JointVars = 40; +const JointPAngDeltaX: JointVars = 41; +const JointPAngDeltaY: JointVars = 42; +const JointPAngDeltaZ: JointVars = 43; +const JointCDeltaX: JointVars = 44; +const JointCDeltaY: JointVars = 45; +const JointCDeltaZ: JointVars = 46; +const JointCAngDeltaX: JointVars = 47; +const JointCAngDeltaY: JointVars = 48; +const JointCAngDeltaZ: JointVars = 49; +fn JointPDelta(idx: i32) -> vec3 { + return vec3(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPDeltaX))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPDeltaY))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPDeltaZ))]); +} +fn JointPAngDelta(idx: i32) -> vec3 { + return vec3(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPAngDeltaX))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPAngDeltaY))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPAngDeltaZ))]); +} +fn JointCDelta(idx: i32) -> vec3 { + return vec3(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCDeltaX))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCDeltaY))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCDeltaZ))]); +} +fn JointCAngDelta(idx: i32) -> vec3 { + return vec3(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCAngDeltaX))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCAngDeltaY))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCAngDeltaZ))]); +} +alias JointDoFVars = i32; //enums:enum +const JointAxisX: JointDoFVars = 0; +const JointAxisY: JointDoFVars = 1; +const JointAxisZ: JointDoFVars = 2; +const JointLimitLower: JointDoFVars = 3; +const JointLimitUpper: JointDoFVars = 4; + +//////// import: "params.go" +struct PhysParams { + Iterations: i32, + Dt: f32, + SubSteps: i32, + ContactMargin: f32, + ContactRelax: f32, // 0.8 def + ContactWeighting: i32, // true + Restitution: i32, // false + JointLinearRelax: f32, // 0.7 def + JointAngularRelax: f32, // 0.4 def + JointLinearComply: f32, // 0 def + JointAngularComply: f32, // 0 def + AngularDamping: f32, // 0 def + SoftRelax: f32, + MaxGeomIter: i32, + ContactsMax: i32, + Cur: i32, + Next: i32, + BodiesN: i32, + DynamicsN: i32, + JointsN: i32, + JointDoFsN: i32, + BodyJointsMax: i32, + BodyCollidePairsN: i32, + pad: i32, + Gravity: vec4, +} + +//////// import: "shapecollide.go" +struct GeomData { + BodyIdx: i32, + Shape: Shapes, + MinSize: f32, + Thick: f32, + Radius: f32, + Size: vec3, + WbR: vec3, + WbQ: vec4, + BwR: vec3, + BwQ: vec4, +} + +//////// import: "shapegeom.go" + +//////// import: "shapes.go" +alias Shapes = i32; //enums:enum +const Plane: Shapes = 0; +const Sphere: Shapes = 1; +const Capsule: Shapes = 2; +const Cylinder: Shapes = 3; +const Box: Shapes = 4; +const Cone: Shapes = 5; + +//////// import: "slmath-matrix3.go" + +//////// import: "slmath-quaternion.go" +fn QuatLength(q: vec4) -> f32 { + return sqrt(q.x*q.x + q.y*q.y + q.z*q.z + q.w*q.w); +} +fn QuatNormalize(q: vec4) -> vec4 { + var nq = q; + var l = QuatLength(q); + if (l == 0) { + nq.x = f32(0); + nq.y = f32(0); + nq.z = f32(0); + nq.w = f32(1); + } else { + l = 1 / l; + nq.x *= l; + nq.y *= l; + nq.z *= l; + nq.w *= l; + }return nq; +} +fn MulQuatVector(q: vec4, v: vec3) -> vec3 { + var xyz = vec3(q.x, q.y, q.z); + var t = Cross3(xyz, v)*(2); +return v+(t*(q.w))+(Cross3(xyz, t)); +} +fn MulQuatVectorInverse(q: vec4, v: vec3) -> vec3 { + var xyz = vec3(q.x, q.y, q.z); + var t = Cross3(xyz, v)*(2); +return v-(t*(q.w))+(Cross3(xyz, t)); +} +fn MulQuats(a: vec4,b: vec4) -> vec4 { + var q: vec4; + q.x = a.x*b.w + a.w*b.x + a.y*b.z - a.z*b.y; + q.y = a.y*b.w + a.w*b.y + a.z*b.x - a.x*b.z; + q.z = a.z*b.w + a.w*b.z + a.x*b.y - a.y*b.x; + q.w = a.w*b.w - a.x*b.x - a.y*b.y - a.z*b.z; +return q; +} + +//////// import: "slmath-vector2.go" + +//////// import: "slmath-vector3.go" +fn Length3(v: vec3) -> f32 { + return sqrt(v.x*v.x + v.y*v.y + v.z*v.z); +} +fn Cross3(v: vec3,o: vec3) -> vec3 { + return vec3(v.y*o.z-v.z*o.y, v.z*o.x-v.x*o.z, v.x*o.y-v.y*o.x); +} + +//////// import: "step.go" + +//////// import: "step_body.go" +fn StepBodyJointDeltas(i: u32) { //gosl:kernel + var params = Params[0]; + var di = i32(i); + if (di >= params.DynamicsN) { + return; + } + var bi = DynamicBody(di); + var invMass = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyInvMass))]; + if (invMass == 0) { + return; // no updates + } + var np = BodyJoints[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(di), u32(0), u32(0))]; + var nc = BodyJoints[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(di), u32(1), u32(0))]; + var linDel = vec3(0, 0, 0); + var angDel = vec3(0, 0, 0); + for (var i = i32(1); + i <= np; i++) { + var ji = BodyJoints[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(di), u32(0), u32(i))]; + var d = JointPDelta(ji); + linDel = linDel+(d); + var a = JointPAngDelta(ji); + angDel = angDel+(a); + } + for (var i = i32(1); + i <= nc; i++) { + var ji = BodyJoints[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(di), u32(1), u32(i))]; + var d = JointCDelta(ji); + linDel = linDel+(d); + var a = JointCAngDelta(ji); + angDel = angDel+(a); + } + StepBodyDeltas(di, bi, false, f32(f32(0)), linDel, angDel); + Params[0] = params; +} +fn StepBodyDeltas(di: i32,bi: i32, contacts: bool, cWt: f32, linDel: vec3,angDel: vec3) { + var params = Params[0]; + var invMass = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyInvMass))]; + var inertia = BodyInertia(bi); + var invInertia = BodyInvInertia(bi); + var r0 = DynamicPos(di, params.Next); + var q0 = DynamicQuat(di, params.Next); + var v0 = DynamicDelta(di, params.Next); + var w0 = DynamicAngDelta(di, params.Next); + var weight = f32(1.0); + if (contacts && params.ContactWeighting == 1) { + if (cWt > 0) { + weight = 1.0 / cWt; + } + } + var dp = linDel*(invMass * weight); + var dq = angDel*(weight); + var wb = MulQuatVectorInverse(q0, w0); + var dwb = invInertia*(MulQuatVectorInverse(q0, dq)); + var tb = Cross3(dwb, inertia*(wb+(dwb)))+(Cross3(wb, inertia*(dwb))); + var dw1 = MulQuatVector(q0, dwb-(invInertia*(tb)*(params.Dt))); + var q1 = q0+(MulQuats(vec4(dw1.x, dw1.y, dw1.z, 0), q0)*(0.5 * params.Dt)); + q1 = QuatNormalize(q1); + var com = BodyCom(bi); + var pcom = MulQuatVector(q0, com)+(r0); + var p1 = pcom+(dp*(params.Dt)); + p1 = p1-(MulQuatVector(q1, com)); + var v1 = v0+(dp); + var w1 = w0+(dw1); + if (Length3(v1) < 1e-4) { + v1 = vec3(0, 0, 0); + } + if (Length3(w1) < 1e-4) { + w1 = vec3(0, 0, 0); + } + SetDynamicPos(di, params.Next, p1); + SetDynamicQuat(di, params.Next, q1); + SetDynamicDelta(di, params.Next, v1); + SetDynamicAngDelta(di, params.Next, w1); + Params[0] = params; +} + +//////// import: "step_joint.go" \ No newline at end of file From 38fbcf7eba56522d122b8cd4cd50d84a649a42b9 Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Tue, 23 Dec 2025 20:20:28 +0100 Subject: [PATCH 49/97] physics: gosl remove gosl.go file --- gosl/gotosl/translate.go | 3 ++ patterns/README.md | 63 +++++++++++++++++++++++++++++++++ physics/examples/test1/test1.go | 16 ++++----- 3 files changed, 73 insertions(+), 9 deletions(-) create mode 100644 patterns/README.md diff --git a/gosl/gotosl/translate.go b/gosl/gotosl/translate.go index c5f01f92..44e87757 100644 --- a/gosl/gotosl/translate.go +++ b/gosl/gotosl/translate.go @@ -91,6 +91,9 @@ func (st *State) TranslateDir(pf string) error { continue } if gofn == "gosl.go" { + if !st.Config.Keep { + os.Remove(gofp) + } continue } var buf bytes.Buffer diff --git a/patterns/README.md b/patterns/README.md new file mode 100644 index 00000000..2204e255 --- /dev/null +++ b/patterns/README.md @@ -0,0 +1,63 @@ +# Patterns + +This package contains functions that generate n-dimensional patterns (in tensors) based on various algorithms, typically for use as inputs to neural network models or other such learning systems. It also has some routines for helping manage collections of such patterns. + +In general the [tensorfs](../tensorfs) system is used to manage a "vocabulary" of such patterns. The `tensor.RowMajor` API is used to organize a list (rows) of patterns. + +## Permuted Binary and FlipBits + +The `PermutedBinary*` functions create binary patterns with a specific number of "on" vs. "off" bits, which can be useful for enforcing a target level of activity. + +The `FlipBits*` functions preserve any existing activity levels while randomly flipping a specific number of bits on or off. + +## Mixing patterns + +The `Mix` function acts a bit like a multi-track mixer, combining different streams of patterns together in a higher-dimensional composite pattern. + +## Managing rows + +Some misc functions help managing rows of data: + +* `SplitRows`: split out subsets of a larger list. +* `ReplicateRows`: replicate multiple copies of a given row. +* `Shuffle`: permuted order of rows. + +## Random seed + +A separate random number source can be established, using the [randx](../base/randx) package. + +## Usage examples + +### Permuted Binary + +```Go + a := dir.Float32("A", 6, 3, 3) // 6 rows of 3x3 patterns + nOn := patterns.NFromPct(0.3, 9) // 30% activity + nDiff := patterns.NFromPct(0.4, nOn) // 40% max overlap + patterns.PermutedBinaryMinDiff(a, nOn, 1, 0, nDiff) // ensures minimum distance +``` + +### Replicate, assemble, and split rows + +```Go + ctx1 := dir.Float32("ctxt1") + patterns.ReplicateRows(ctx1, a.SubSpace(0), 6) // 6x first row of 'a' above + ab := dir.Float32("ab", 0, 3, 3) + ab.AppendFrom(a) // add a patterns + ab.AppendFrom(b) // add b patterns +``` + +```Go + // split 12 items into 3 sets of 4 + patterns.SplitRows(dir, ab, []string{"as", "bs", "cs"}, 3, 3) +``` + +### Mix patterns + +```Go + mix := dir.Float32("mix") + patterns.Mix(mix, 12, a, b, ctx1, ctx1, empty, b) // make 12 rows from given sources + mix.SetShapeSizes(12, 3, 2, 3, 3) // reshape to 3x2 = "outer" dims x 3x3 inner +``` + + diff --git a/physics/examples/test1/test1.go b/physics/examples/test1/test1.go index 0b93627a..189ab94a 100644 --- a/physics/examples/test1/test1.go +++ b/physics/examples/test1/test1.go @@ -20,6 +20,7 @@ import ( "cogentcore.org/core/tree" "cogentcore.org/core/xyz" "cogentcore.org/core/xyz/xyzcore" + _ "cogentcore.org/lab/gosl/slbool/slboolcore" // include to get gui views "cogentcore.org/lab/physics" "cogentcore.org/lab/physics/world" ) @@ -50,7 +51,7 @@ func main() { params := physics.GetParams(0) params.Dt = 0.0001 params.SubSteps = 1 - params.Gravity.Y = 0 + // params.Gravity.Y = 0 params.ContactRelax = 0.1 params.Restitution.SetBool(false) fv.SetStruct(params) @@ -67,17 +68,14 @@ func main() { depth := height * .15 _ = width b1 := wr.NewDynamic(wl, "body", physics.Box, "purple", 1.0, math32.Vec3(height, height, depth), - math32.Vec3(0, height+2, 0), rot) + math32.Vec3(0, height*2, 0), rot) _ = b1 // bj := wl.NewJointRevolute(-1, b1.DynamicIndex, math32.Vec3(0, 0, 0), math32.Vec3(0, -height, 0), math32.Vec3(0, 1, 0)) - bj := wl.NewJointPrismatic(-1, b1.DynamicIndex, math32.Vec3(0, 0, 0), math32.Vec3(0, -height, 0), math32.Vec3(1, 0, 0)) - physics.SetJointControlForce(bj, 0, .1) - // physics.SetJointTargetPos(bj, 0, 1) - // physics.SetJointDoF(bj, 0, physics.JointDamp, 0.01) - // physics.SetJointDoF(bj, 0, physics.JointStiff, 1) // this makes a big difference - // bj := wl.NewJointFree(-1, b1.DynamicIndex, math32.Vec3(0, 0, 0), math32.Vec3(0, -height, 0)) - // _ = bj + // bj := wl.NewJointPrismatic(-1, b1.DynamicIndex, math32.Vec3(0, 0, 0), math32.Vec3(0, -height, 0), math32.Vec3(1, 0, 0)) + // // physics.SetJointControlForce(bj, 0, .1) + // physics.SetJointTargetPos(bj, 0, 0, 1000) + // physics.SetJointTargetVel(bj, 0, 0, 1) wr.Init(wl) From 6179bd37f16bf3451be894a22770ee93d3e2f54f Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Tue, 23 Dec 2025 21:37:23 +0100 Subject: [PATCH 50/97] physics: fix basic issues in restitution but still needs a more thorough approach. Basically need to integrate during entire time of penetration and then compute escape velocity based on the saved incoming velocity just prior to impact. --- physics/contact.go | 5 +- physics/contact.goal | 5 +- physics/examples/balls/balls.go | 6 +- physics/examples/pendula/pendula.go | 2 +- physics/examples/test1/test1.go | 162 ++++++++++++++++++++------ physics/params.go | 8 +- physics/shaders/CollisionNarrow.wgsl | 2 +- physics/shaders/StepBodyContacts.wgsl | 4 +- physics/shapecollide.go | 2 +- physics/shapecollide.goal | 2 +- 10 files changed, 144 insertions(+), 54 deletions(-) diff --git a/physics/contact.go b/physics/contact.go index b59343b7..48992e30 100644 --- a/physics/contact.go +++ b/physics/contact.go @@ -608,7 +608,7 @@ func StepBodyContacts(i uint32) { //gosl:kernel vB = VelocityAtPoint(v0B, w0B, dB).Add(grav) vBnew = VelocityAtPoint(v1B, w1B, dB) - dBnew = slmath.MulQuatVectorInverse(q0B, slmath.Cross3(dB, nnorm)) // norm is not - here.. + dBnew = slmath.MulQuatVectorInverse(q0B, slmath.Cross3(dB, norm)) // norm is not - here.. mInvBr = mInvB + slmath.Dot3(dBnew, iInvB.MulVector3(dBnew)) } mInv := mInvAr + mInvBr @@ -616,6 +616,7 @@ func StepBodyContacts(i uint32) { //gosl:kernel relVel1 := slmath.Dot3(nnorm, vAnew.Sub(vBnew)) if relVel0 < 0 { dv := -(relVel1 - relVel0*bounce) / mInv + // fmt.Println(dv, relVel1, relVel0, bounce, mInv) if diA >= 0 { dvA := nnorm.MulScalar(mInvA * dv) dwA := slmath.MulQuatVector(q0A, iInvA.MulVector3(dAnew).MulScalar(dv)) @@ -623,7 +624,7 @@ func StepBodyContacts(i uint32) { //gosl:kernel angDeltaA = angDeltaA.Add(dwA) } if diB >= 0 { - dvB := nnorm.MulScalar(mInvB * dv) + dvB := norm.MulScalar(mInvB * dv) dwB := slmath.MulQuatVector(q0B, iInvB.MulVector3(dBnew).MulScalar(dv)) linDeltaB = linDeltaB.Add(dvB) angDeltaB = angDeltaB.Add(dwB) diff --git a/physics/contact.goal b/physics/contact.goal index b7f6ff5f..bd2ef035 100644 --- a/physics/contact.goal +++ b/physics/contact.goal @@ -606,7 +606,7 @@ func StepBodyContacts(i uint32) { //gosl:kernel vB = VelocityAtPoint(v0B, w0B, dB).Add(grav) vBnew = VelocityAtPoint(v1B, w1B, dB) - dBnew = slmath.MulQuatVectorInverse(q0B, slmath.Cross3(dB, nnorm)) // norm is not - here.. + dBnew = slmath.MulQuatVectorInverse(q0B, slmath.Cross3(dB, norm)) // norm is not - here.. mInvBr = mInvB + slmath.Dot3(dBnew, iInvB.MulVector3(dBnew)) } mInv := mInvAr + mInvBr @@ -614,6 +614,7 @@ func StepBodyContacts(i uint32) { //gosl:kernel relVel1 := slmath.Dot3(nnorm, vAnew.Sub(vBnew)) if relVel0 < 0 { dv := -(relVel1 - relVel0 * bounce) / mInv + // fmt.Println(dv, relVel1, relVel0, bounce, mInv) if diA >= 0 { dvA := nnorm.MulScalar(mInvA * dv) dwA := slmath.MulQuatVector(q0A, iInvA.MulVector3(dAnew).MulScalar(dv)) @@ -621,7 +622,7 @@ func StepBodyContacts(i uint32) { //gosl:kernel angDeltaA = angDeltaA.Add(dwA) } if diB >= 0 { - dvB := nnorm.MulScalar(mInvB * dv) + dvB := norm.MulScalar(mInvB * dv) dwB := slmath.MulQuatVector(q0B, iInvB.MulVector3(dBnew).MulScalar(dv)) linDeltaB = linDeltaB.Add(dvB) angDeltaB = angDeltaB.Add(dwB) diff --git a/physics/examples/balls/balls.go b/physics/examples/balls/balls.go index e39df89e..380f2668 100644 --- a/physics/examples/balls/balls.go +++ b/physics/examples/balls/balls.go @@ -1,4 +1,4 @@ -// Copyright (c) 2019, Cogent Core. All rights reserved. +// Copyright (c) 2025, Cogent Core. All rights reserved. // Use of this source code is governed by a BSD-style // license that can be found in the LICENSE file. @@ -108,9 +108,9 @@ func main() { params.Dt = 0.0001 // leaks balls > 0.0005 params.SubSteps = 100 // major speedup by inner-stepping // params.Gravity.Y = 0 - params.ContactRelax = 0.1 // 0.1 seems most physical -- 0.2 getting a bit more ke? + params.ContactRelax = 0.2 // 0.1 seems most physical -- 0.2 getting a bit more ke? params.Restitution.SetBool(false) // not working! - params.ContactMargin = 0.1 // 0.1 better than .01 -- leaks a few + params.ContactMargin = 0 // 0.1 better than .01 -- leaks a few bpf.SetStruct(bs) wpf.SetStruct(params) diff --git a/physics/examples/pendula/pendula.go b/physics/examples/pendula/pendula.go index bc45cdd5..97880099 100644 --- a/physics/examples/pendula/pendula.go +++ b/physics/examples/pendula/pendula.go @@ -1,4 +1,4 @@ -// Copyright (c) 2019, Cogent Core. All rights reserved. +// Copyright (c) 2025, Cogent Core. All rights reserved. // Use of this source code is governed by a BSD-style // license that can be found in the LICENSE file. diff --git a/physics/examples/test1/test1.go b/physics/examples/test1/test1.go index 189ab94a..1c6b2b2a 100644 --- a/physics/examples/test1/test1.go +++ b/physics/examples/test1/test1.go @@ -1,14 +1,13 @@ -// Copyright (c) 2019, Cogent Core. All rights reserved. +// Copyright (c) 2025, Cogent Core. All rights reserved. // Use of this source code is governed by a BSD-style // license that can be found in the LICENSE file. package main -//go:generate core generate +//go:generate core generate -add-types import ( "fmt" - "time" "cogentcore.org/core/colors" "cogentcore.org/core/core" @@ -25,14 +24,50 @@ import ( "cogentcore.org/lab/physics/world" ) +// Test has sim params +type Test struct { + // Height to start from + Height float32 + + // Size of test item + Size float32 + + // Mass of test item + Mass float32 + + Bounce float32 + Friction float32 + FrictionTortion float32 + FrictionRolling float32 +} + +func (b *Test) Defaults() { + b.Height = 50 + b.Size = 0.5 + b.Mass = 0.1 + + b.Bounce = 0.5 + b.Friction = 0 + b.FrictionTortion = 0 + b.FrictionRolling = 0 +} + func main() { // gpu.Debug = true b := core.NewBody("test1").SetTitle("Physics Test") split := core.NewSplits(b) - // tv := core.NewTree(core.NewFrame(split)) - fv := core.NewForm(split) + fpanel := core.NewFrame(split) + fpanel.Styler(func(s *styles.Style) { + s.Direction = styles.Column + s.Grow.Set(1, 1) + }) + + bpf := core.NewForm(fpanel) + wpf := core.NewForm(fpanel) + tbvw := core.NewTabs(split) scfr, _ := tbvw.NewTab("3D View") + split.SetSplits(0.2, 0.8) se := xyzcore.NewSceneEditor(scfr) se.UpdateWidget() @@ -46,40 +81,63 @@ func main() { wr := world.NewWorld(sc) + bs := &Test{} + bs.Defaults() + wl := physics.NewWorld() wl.GPU = false + params := physics.GetParams(0) - params.Dt = 0.0001 - params.SubSteps = 1 + params.Dt = 0.0001 // leaks balls > 0.0005 + params.SubSteps = 100 // major speedup by inner-stepping // params.Gravity.Y = 0 - params.ContactRelax = 0.1 - params.Restitution.SetBool(false) - fv.SetStruct(params) - - split.SetSplits(0.2, 0.8) - - rot := math32.NewQuat(0, 0, 0, 1) - // thick := float32(0.1) - wr.NewBody(wl, "floor", physics.Plane, "#D0D0D080", math32.Vec3(10, 0, 10), - math32.Vec3(0, 0, 0), rot) - - height := float32(.5) - width := height * .4 - depth := height * .15 - _ = width - b1 := wr.NewDynamic(wl, "body", physics.Box, "purple", 1.0, math32.Vec3(height, height, depth), - math32.Vec3(0, height*2, 0), rot) - _ = b1 + params.ContactRelax = 0.2 // 0.1 seems most physical -- 0.2 getting a bit more ke? + params.Restitution.SetBool(false) // not working! + params.ContactMargin = 0 // 0 for restitution + + bpf.SetStruct(bs) + wpf.SetStruct(params) + + config := func() { + wr.Reset() + wl.Reset() + rot := math32.NewQuat(0, 0, 0, 1) + // thick := float32(0.1) + fl := wr.NewBody(wl, "floor", physics.Plane, "#D0D0D080", math32.Vec3(10, 0, 10), + math32.Vec3(0, 0, 0), rot) + fl.SetBodyBounce(bs.Bounce) + + height := float32(bs.Size) + width := height * .4 + depth := height * .15 + _, _ = width, depth + // b1 := wr.NewDynamic(wl, "body", physics.Box, "purple", 1.0, math32.Vec3(height, height, depth), + // math32.Vec3(0, height*2, 0), rot) + b1 := wr.NewDynamic(wl, "body", physics.Sphere, "purple", 1.0, math32.Vec3(height, height, height), + math32.Vec3(0, height*bs.Height, 0), rot) + // b1.SetBodyBounce(bs.Bounce) + _ = b1 + + // bj := wl.NewJointRevolute(-1, b1.DynamicIndex, math32.Vec3(0, 0, 0), math32.Vec3(0, -height, 0), math32.Vec3(0, 1, 0)) + // bj := wl.NewJointPrismatic(-1, b1.DynamicIndex, math32.Vec3(0, 0, 0), math32.Vec3(0, -height, 0), math32.Vec3(1, 0, 0)) + // // // physics.SetJointControlForce(bj, 0, .1) + // physics.SetJointTargetPos(bj, 0, 1, 1) + // physics.SetJointTargetVel(bj, 0, 0, 1) + wr.Init(wl) + wr.Update() + } - // bj := wl.NewJointRevolute(-1, b1.DynamicIndex, math32.Vec3(0, 0, 0), math32.Vec3(0, -height, 0), math32.Vec3(0, 1, 0)) - // bj := wl.NewJointPrismatic(-1, b1.DynamicIndex, math32.Vec3(0, 0, 0), math32.Vec3(0, -height, 0), math32.Vec3(1, 0, 0)) - // // physics.SetJointControlForce(bj, 0, .1) - // physics.SetJointTargetPos(bj, 0, 0, 1000) - // physics.SetJointTargetVel(bj, 0, 0, 1) + config() - wr.Init(wl) + cycle := 0 - wr.Update() + updateView := func() { + bpf.Update() + wpf.Update() + if se.IsVisible() { + se.NeedsRender() + } + } sc.Camera.Pose.Pos = math32.Vec3(0, 40, 3.5) sc.Camera.LookAt(math32.Vec3(0, 5, 0), math32.Vec3(0, 1, 0)) @@ -95,6 +153,7 @@ func main() { sc.SaveCamera("default") isRunning := false + stop := false stepNButton := func(p *tree.Plan, n int) { nm := fmt.Sprintf("Step %d", n) @@ -109,12 +168,17 @@ func main() { isRunning = true for range n { wl.Step() + cycle++ wr.Update() if se.IsVisible() { se.AsyncLock() se.NeedsRender() se.AsyncUnlock() - time.Sleep(1 * time.Millisecond) + // time.Sleep(1 * time.Nanosecond) + } + if stop { + stop = false + break } } isRunning = false @@ -130,19 +194,43 @@ func main() { core.NewToolbar(bar).Maker(func(p *tree.Plan) { tree.Add(p, func(w *core.Button) { w.SetText("Init").SetIcon(icons.Reset). - SetTooltip("Reset state"). + SetTooltip("Reset physics state back to starting."). OnClick(func(e events.Event) { + if isRunning { + return + } wl.InitState() wr.Update() - if se.IsVisible() { - se.NeedsRender() - } + updateView() }) }) + tree.Add(p, func(w *core.Button) { + w.SetText("Stop").SetIcon(icons.Stop). + SetTooltip("Stop running"). + OnClick(func(e events.Event) { + stop = true + }) + }) + tree.Add(p, func(w *core.Separator) {}) + stepNButton(p, 1) stepNButton(p, 10) stepNButton(p, 100) stepNButton(p, 1000) + stepNButton(p, 10000) + tree.Add(p, func(w *core.Separator) {}) + + tree.Add(p, func(w *core.Button) { + w.SetText("Rebuild").SetIcon(icons.Reset). + SetTooltip("Rebuild the environment, when you change parameters"). + OnClick(func(e events.Event) { + if isRunning { + return + } + config() + updateView() + }) + }) }) }) b.RunMainWindow() diff --git a/physics/params.go b/physics/params.go index 1007c0e7..9cf033a9 100644 --- a/physics/params.go +++ b/physics/params.go @@ -29,8 +29,8 @@ type PhysParams struct { SubSteps int32 `default:"10" min:"1"` // Contact margin is the extra distance for broadphase collision - // around rigid bodies. - ContactMargin float32 `defautl:"0.1"` + // around rigid bodies. This can make some joints potentially unstable if > 0 + ContactMargin float32 `default:"0,0.1"` // ContactRelax is rigid contact relaxation constant. // Higher values cause errros @@ -40,7 +40,7 @@ type PhysParams struct { ContactWeighting slbool.Bool `default:"true"` // true // Restitution takes into account bounciness of objects. - Restitution slbool.Bool `default:"true"` // false + Restitution slbool.Bool `default:"false"` // false // JointLinearRelax is joint linear relaxation constant. JointLinearRelax float32 `default:"0.7"` // 0.7 def @@ -104,7 +104,7 @@ func (pr *PhysParams) Defaults() { pr.SubSteps = 10 pr.Gravity.Set(0, -9.81, 0) - pr.ContactMargin = 0.1 + pr.ContactMargin = 0 pr.ContactRelax = 0.8 pr.ContactWeighting.SetBool(true) pr.Restitution.SetBool(false) diff --git a/physics/shaders/CollisionNarrow.wgsl b/physics/shaders/CollisionNarrow.wgsl index b7b2a0dd..3a40db63 100644 --- a/physics/shaders/CollisionNarrow.wgsl +++ b/physics/shaders/CollisionNarrow.wgsl @@ -666,7 +666,7 @@ fn ColBoxPlane(cpi: i32,maxIter: i32, gdA: ptr, gdB: ptr width || abs(queryB.z) > length) { *norm = Normal3(comA-(pBw)); } else { - *norm = MulQuatVector((*gdB).WbQ, vec3(0, 0, 1)); + *norm = MulQuatVector((*gdB).WbQ, vec3(0, 1, 0)); } } *pA = pAw; diff --git a/physics/shaders/StepBodyContacts.wgsl b/physics/shaders/StepBodyContacts.wgsl index 9e56f388..7793471c 100644 --- a/physics/shaders/StepBodyContacts.wgsl +++ b/physics/shaders/StepBodyContacts.wgsl @@ -293,7 +293,7 @@ if (params.Restitution == 1 && bounce > 0 && (mInvA > 0 || mInvB > 0)) { var v1B = DynamicDelta(diB, params.Next); vB = VelocityAtPoint(v0B, w0B, dB)+(grav); vBnew = VelocityAtPoint(v1B, w1B, dB); - dBnew = MulQuatVectorInverse(q0B, Cross3(dB, nnorm)); // norm is not - here.. + dBnew = MulQuatVectorInverse(q0B, Cross3(dB, norm)); // norm is not - here.. mInvBr = mInvB + Dot3(dBnew, iInvB*(dBnew)); } var mInv = mInvAr + mInvBr; @@ -308,7 +308,7 @@ if (params.Restitution == 1 && bounce > 0 && (mInvA > 0 || mInvB > 0)) { angDeltaA = angDeltaA+(dwA); } if (diB >= 0) { - var dvB = nnorm*(mInvB * dv); + var dvB = norm*(mInvB * dv); var dwB = MulQuatVector(q0B, iInvB*(dBnew)*(dv)); linDeltaB = linDeltaB+(dvB); angDeltaB = angDeltaB+(dwB); diff --git a/physics/shapecollide.go b/physics/shapecollide.go index 64527e55..2f6f5b43 100644 --- a/physics/shapecollide.go +++ b/physics/shapecollide.go @@ -285,7 +285,7 @@ func ColBoxPlane(cpi, maxIter int32, gdA *GeomData, gdB *GeomData, pA, pB, norm // the COM is outside the plane *norm = slmath.Normal3(comA.Sub(pBw)) } else { - *norm = slmath.MulQuatVector(gdB.WbQ, math32.Vec3(0, 0, 1)) + *norm = slmath.MulQuatVector(gdB.WbQ, math32.Vec3(0, 1, 0)) } } *pA = pAw diff --git a/physics/shapecollide.goal b/physics/shapecollide.goal index 9174c9ab..1001f15f 100644 --- a/physics/shapecollide.goal +++ b/physics/shapecollide.goal @@ -282,7 +282,7 @@ func ColBoxPlane(cpi, maxIter int32, gdA *GeomData, gdB *GeomData, pA, pB, norm // the COM is outside the plane *norm = slmath.Normal3(comA.Sub(pBw)) } else { - *norm = slmath.MulQuatVector(gdB.WbQ, math32.Vec3(0, 0, 1)) + *norm = slmath.MulQuatVector(gdB.WbQ, math32.Vec3(0, 1, 0)) } } *pA = pAw From 2708e183d4d4c3fd6529835a8d2bf69fca065b71 Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Tue, 23 Dec 2025 21:46:40 +0100 Subject: [PATCH 51/97] physics: update todo --- physics/README.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/physics/README.md b/physics/README.md index 8f911d49..2e5248c0 100644 --- a/physics/README.md +++ b/physics/README.md @@ -10,3 +10,5 @@ The [world](world) visualization sub-package manages a `View` element that links * Muscles: https://mujoco.readthedocs.io/en/stable/modeling.html#muscles +* fix basic issues in restitution: needs a more thorough approach. Basically need to integrate during entire time of penetration and then compute escape velocity based on the saved incoming velocity just prior to impact. + From 14ba872c05ce9f053e37900ed914d0d30ce69c57 Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Wed, 24 Dec 2025 10:47:44 +0100 Subject: [PATCH 52/97] physics: add an Editor widget for basic interactive running. can use in docs. --- physics/examples/balls/balls.go | 177 ++----------------- physics/examples/pendula/pendula.go | 260 +++++++-------------------- physics/params.go | 15 ++ physics/world/editor.go | 262 ++++++++++++++++++++++++++++ physics/world/typegen.go | 39 ++++- 5 files changed, 393 insertions(+), 360 deletions(-) create mode 100644 physics/world/editor.go diff --git a/physics/examples/balls/balls.go b/physics/examples/balls/balls.go index 380f2668..16e93a2b 100644 --- a/physics/examples/balls/balls.go +++ b/physics/examples/balls/balls.go @@ -7,19 +7,11 @@ package main //go:generate core generate -add-types import ( - "fmt" "math/rand/v2" "cogentcore.org/core/colors" "cogentcore.org/core/core" - "cogentcore.org/core/events" - "cogentcore.org/core/icons" "cogentcore.org/core/math32" - "cogentcore.org/core/styles" - "cogentcore.org/core/styles/abilities" - "cogentcore.org/core/tree" - "cogentcore.org/core/xyz" - "cogentcore.org/core/xyz/xyzcore" _ "cogentcore.org/lab/gosl/slbool/slboolcore" // include to get gui views "cogentcore.org/lab/physics" "cogentcore.org/lab/physics/world" @@ -70,69 +62,32 @@ func (b *Balls) Defaults() { } func main() { - // gpu.Debug = true b := core.NewBody("test1").SetTitle("Physics Balls") - split := core.NewSplits(b) - fpanel := core.NewFrame(split) - fpanel.Styler(func(s *styles.Style) { - s.Direction = styles.Column - s.Grow.Set(1, 1) - }) - - bpf := core.NewForm(fpanel) - wpf := core.NewForm(fpanel) - - tbvw := core.NewTabs(split) - scfr, _ := tbvw.NewTab("3D View") - split.SetSplits(0.2, 0.8) - - se := xyzcore.NewSceneEditor(scfr) - se.UpdateWidget() - sc := se.SceneXYZ() - - sc.Background = colors.Scheme.Select.Container - xyz.NewAmbient(sc, "ambient", 0.3, xyz.DirectSun) - - dir := xyz.NewDirectional(sc, "dir", 1, xyz.DirectSun) - dir.Pos.Set(0, 2, 1) - - wr := world.NewWorld(sc) + ed := world.NewEditor(b) bs := &Balls{} bs.Defaults() - wl := physics.NewWorld() - wl.GPU = true - - params := physics.GetParams(0) - params.Dt = 0.0001 // leaks balls > 0.0005 - params.SubSteps = 100 // major speedup by inner-stepping - // params.Gravity.Y = 0 - params.ContactRelax = 0.2 // 0.1 seems most physical -- 0.2 getting a bit more ke? - params.Restitution.SetBool(false) // not working! - params.ContactMargin = 0 // 0.1 better than .01 -- leaks a few - - bpf.SetStruct(bs) - wpf.SetStruct(params) + ed.SetUserParams(bs) - config := func() { - wr.Reset() - wl.Reset() + ed.SetConfigFunc(func() { + ph := ed.Physics + wr := ed.World rot := math32.NewQuat(0, 0, 0, 1) - wr.NewBody(wl, "floor", physics.Plane, "#D0D0D080", math32.Vec3(0, 0, 0), + wr.NewBody(ph, "floor", physics.Plane, "#D0D0D080", math32.Vec3(0, 0, 0), math32.Vec3(0, 0, 0), rot) hw := bs.Width / 2 hd := bs.Depth / 2 hh := bs.Height / 2 ht := bs.Thick / 2 - wr.NewBody(wl, "back-wall", physics.Box, "#0000FFA0", math32.Vec3(hw, hh, ht), + wr.NewBody(ph, "back-wall", physics.Box, "#0000FFA0", math32.Vec3(hw, hh, ht), math32.Vec3(0, hh, -hd), rot) - wr.NewBody(wl, "left-wall", physics.Box, "#FF0000A0", math32.Vec3(ht, hh, hd), + wr.NewBody(ph, "left-wall", physics.Box, "#FF0000A0", math32.Vec3(ht, hh, hd), math32.Vec3(-hw, hh, 0), rot) - wr.NewBody(wl, "right-wall", physics.Box, "#00FF00A0", math32.Vec3(ht, hh, hd), + wr.NewBody(ph, "right-wall", physics.Box, "#00FF00A0", math32.Vec3(ht, hh, hd), math32.Vec3(hw, hh, 0), rot) - wr.NewBody(wl, "front-wall", physics.Box, "#FFFF00A0", math32.Vec3(hw, hh, ht), + wr.NewBody(ph, "front-wall", physics.Box, "#FFFF00A0", math32.Vec3(hw, hh, ht), math32.Vec3(0, hh, hd), rot) box := bs.Width * .9 @@ -142,7 +97,7 @@ func main() { x := rand.Float32()*box - 0.5*box z := rand.Float32()*box - 0.5*box clr := colors.Names[i%len(colors.Names)] - bl := wr.NewDynamic(wl, "ball", physics.Sphere, clr, bs.Mass, math32.Vec3(size, size, size), + bl := wr.NewDynamic(ph, "ball", physics.Sphere, clr, bs.Mass, math32.Vec3(size, size, size), math32.Vec3(x, size+ht, z), rot) if !bs.Collide { physics.SetBodyGroup(bl.Index, int32(i+1)) // only collide within same group @@ -152,115 +107,7 @@ func main() { bl.SetBodyFrictionTortion(bs.FrictionTortion) bl.SetBodyFrictionRolling(bs.FrictionRolling) } - wr.Init(wl) - wr.Update() - } - - config() - - cycle := 0 - - updateView := func() { - bpf.Update() - wpf.Update() - if se.IsVisible() { - se.NeedsRender() - } - } - - sc.Camera.Pose.Pos = math32.Vec3(0, 40, 3.5) - sc.Camera.LookAt(math32.Vec3(0, 5, 0), math32.Vec3(0, 1, 0)) - sc.SaveCamera("3") - - sc.Camera.Pose.Pos = math32.Vec3(-1.33, 2.24, 3.55) - sc.Camera.LookAt(math32.Vec3(0, .5, 0), math32.Vec3(0, 1, 0)) - sc.SaveCamera("2") - - sc.Camera.Pose.Pos = math32.Vec3(0, 80, 75) - sc.Camera.LookAt(math32.Vec3(0, 5, 0), math32.Vec3(0, 1, 0)) - sc.SaveCamera("1") - sc.SaveCamera("default") - - isRunning := false - stop := false - - stepNButton := func(p *tree.Plan, n int) { - nm := fmt.Sprintf("Step %d", n) - tree.AddAt(p, nm, func(w *core.Button) { - w.SetText(nm).SetIcon(icons.PlayArrow). - SetTooltip(fmt.Sprintf("Step state %d times", n)). - OnClick(func(e events.Event) { - if isRunning { - return - } - go func() { - isRunning = true - for range n { - wl.Step() - cycle++ - wr.Update() - if se.IsVisible() { - se.AsyncLock() - se.NeedsRender() - se.AsyncUnlock() - // time.Sleep(1 * time.Nanosecond) - } - if stop { - stop = false - break - } - } - isRunning = false - }() - }) - w.Styler(func(s *styles.Style) { - s.SetAbilities(true, abilities.RepeatClickable) - }) - }) - } - - b.AddTopBar(func(bar *core.Frame) { - core.NewToolbar(bar).Maker(func(p *tree.Plan) { - tree.Add(p, func(w *core.Button) { - w.SetText("Init").SetIcon(icons.Reset). - SetTooltip("Reset physics state back to starting."). - OnClick(func(e events.Event) { - if isRunning { - return - } - wl.InitState() - wr.Update() - updateView() - }) - }) - tree.Add(p, func(w *core.Button) { - w.SetText("Stop").SetIcon(icons.Stop). - SetTooltip("Stop running"). - OnClick(func(e events.Event) { - stop = true - }) - }) - tree.Add(p, func(w *core.Separator) {}) - - stepNButton(p, 1) - stepNButton(p, 10) - stepNButton(p, 100) - stepNButton(p, 1000) - stepNButton(p, 10000) - tree.Add(p, func(w *core.Separator) {}) - - tree.Add(p, func(w *core.Button) { - w.SetText("Rebuild").SetIcon(icons.Reset). - SetTooltip("Rebuild the environment, when you change parameters"). - OnClick(func(e events.Event) { - if isRunning { - return - } - config() - updateView() - }) - }) - }) }) + b.RunMainWindow() } diff --git a/physics/examples/pendula/pendula.go b/physics/examples/pendula/pendula.go index 97880099..d01f33cb 100644 --- a/physics/examples/pendula/pendula.go +++ b/physics/examples/pendula/pendula.go @@ -11,14 +11,7 @@ import ( "cogentcore.org/core/colors" "cogentcore.org/core/core" - "cogentcore.org/core/events" - "cogentcore.org/core/icons" "cogentcore.org/core/math32" - "cogentcore.org/core/styles" - "cogentcore.org/core/styles/abilities" - "cogentcore.org/core/tree" - "cogentcore.org/core/xyz" - "cogentcore.org/core/xyz/xyzcore" _ "cogentcore.org/lab/gosl/slbool/slboolcore" // include to get gui views "cogentcore.org/lab/physics" "cogentcore.org/lab/physics/world" @@ -27,27 +20,41 @@ import ( // Pendula has sim params type Pendula struct { - // Number of balls: if collide, then run out of memory above 1000 or so + // Number of bar elements to add to the pendulum. More interesting the more you add! NPendula int - ForceOn int + // StartVert starts the pendulum in the vertical orientation + // (else horizontal, so it has somewhere to go). Need to add force if vertical. + StartVert bool + + // TargetDegFromVert is the target number of degrees off of vertical + // for each joint. Critical for this to not be 0 for StartVert. + TargetDegFromVert int + + // Timestep in msec to add a force + ForceOn int + + // Timestep in msec to stop adding force ForceOff int - Force float32 + // Force to add + Force float32 + + // half-size of the pendulum elements. HSize math32.Vector3 - // Mass of each ball (kg) + // Mass of each bar (kg) Mass float32 - // do the elements collide with each other? + // do the elements collide with each other? this is currently bad! Collide bool - // Stiff is the strength of positional constraints - // when imposing them. + // Stiff is the strength of the positional constraint to keep + // each bar in a vertical position. Stiff float32 - // Damp is the strength of velocity constraints - // when imposing them. + // Damp is the strength of the velocity constraint to keep each + // bar not moving. Damp float32 } @@ -60,214 +67,79 @@ func (b *Pendula) Defaults() { b.ForceOff = 102 b.Force = 0 - b.Damp = 0.5 - b.Stiff = 1e4 + b.Damp = 0 + b.Stiff = 0 } func main() { - // gpu.Debug = true b := core.NewBody("test1").SetTitle("Physics Pendula") - split := core.NewSplits(b) - fpanel := core.NewFrame(split) - fpanel.Styler(func(s *styles.Style) { - s.Direction = styles.Column - s.Grow.Set(1, 1) - }) - - bpf := core.NewForm(fpanel) - wpf := core.NewForm(fpanel) - - tbvw := core.NewTabs(split) - scfr, _ := tbvw.NewTab("3D View") - split.SetSplits(0.2, 0.8) - - se := xyzcore.NewSceneEditor(scfr) - se.UpdateWidget() - sc := se.SceneXYZ() - - sc.Background = colors.Scheme.Select.Container - xyz.NewAmbient(sc, "ambient", 0.3, xyz.DirectSun) - - dir := xyz.NewDirectional(sc, "dir", 1, xyz.DirectSun) - dir.Pos.Set(0, 2, 1) - - wr := world.NewWorld(sc) + ed := world.NewEditor(b) ps := &Pendula{} ps.Defaults() - wl := physics.NewWorld() - wl.GPU = true - - params := physics.GetParams(0) - params.Dt = 0.0001 // leaks balls > 0.0005 - params.SubSteps = 100 // major speedup by inner-stepping - // params.Gravity.Y = 0 - params.Restitution.SetBool(false) // not working! - params.ContactMargin = 0.1 // 0.1 better than .01 -- leaks a few + ed.SetUserParams(ps) - bpf.SetStruct(ps) - wpf.SetStruct(params) + var botJoint int32 - var topJoint int32 - - config := func() { - wr.Reset() - wl.Reset() + ed.SetConfigFunc(func() { + ph := ed.Physics + wr := ed.World rot := math32.NewQuat(0, 0, 0, 1) - _ = rot rleft := math32.NewQuatAxisAngle(math32.Vec3(0, 0, 1), -math32.Pi/2) - _ = rleft + + if ps.StartVert { + rleft = rot + } + stY := 2*ps.HSize.Y*float32(ps.NPendula+1) + 1 clr := colors.Names[0] - pb := wr.NewDynamic(wl, "top", physics.Box, clr, ps.Mass, ps.HSize, - math32.Vec3(-ps.HSize.Y, stY, 0), rleft) + x := -ps.HSize.Y + y := stY + if ps.StartVert { + x = 0 + y -= ps.HSize.Y + } + pb := wr.NewDynamic(ph, "top", physics.Box, clr, ps.Mass, ps.HSize, math32.Vec3(x, y, 0), rleft) if !ps.Collide { physics.SetBodyGroup(pb.Index, int32(1)) } - ji := wl.NewJointRevolute(-1, pb.DynamicIndex, math32.Vec3(0, stY, 0), math32.Vec3(0, ps.HSize.Y, 0), math32.Vec3(0, 0, 1)) - // physics.SetJointTargetPos(ji, 0, math32.Pi/2, 1) // let it swing! - physics.SetJointTargetPos(ji, 0, 0, 0) // let it swing! - physics.SetJointTargetVel(ji, 0, 0, 0) // let it swing! + targ := math32.DegToRad(float32(ps.TargetDegFromVert)) - topJoint = ji + ji := ph.NewJointRevolute(-1, pb.DynamicIndex, math32.Vec3(0, stY, 0), math32.Vec3(0, ps.HSize.Y, 0), math32.Vec3(0, 0, 1)) + physics.SetJointTargetPos(ji, 0, targ, ps.Stiff) + physics.SetJointTargetVel(ji, 0, 0, ps.Damp) for i := 1; i < ps.NPendula; i++ { clr := colors.Names[i%len(colors.Names)] x := -float32(i)*ps.HSize.Y*2 - ps.HSize.Y - cb := wr.NewDynamic(wl, "child", physics.Box, clr, ps.Mass, ps.HSize, - math32.Vec3(x, stY, 0), rleft) + y := stY + if ps.StartVert { + y = stY + x + x = 0 + } + cb := wr.NewDynamic(ph, "child", physics.Box, clr, ps.Mass, ps.HSize, + math32.Vec3(x, y, 0), rleft) if !ps.Collide { physics.SetBodyGroup(cb.Index, int32(1+i)) } - ji = wl.NewJointRevolute(pb.DynamicIndex, cb.DynamicIndex, math32.Vec3(0, -ps.HSize.Y, 0), math32.Vec3(0, ps.HSize.Y, 0), math32.Vec3(0, 0, 1)) - physics.SetJointTargetPos(ji, 0, 0, 0) // let it swing! - physics.SetJointTargetVel(ji, 0, 0, 0) // let it swing! + ji = ph.NewJointRevolute(pb.DynamicIndex, cb.DynamicIndex, math32.Vec3(0, -ps.HSize.Y, 0), math32.Vec3(0, ps.HSize.Y, 0), math32.Vec3(0, 0, 1)) + physics.SetJointTargetPos(ji, 0, targ, ps.Stiff) + physics.SetJointTargetVel(ji, 0, 0, ps.Damp) pb = cb + botJoint = ji } - wr.Init(wl) - wr.Update() - } - - config() - - cycle := 0 + }) - updateView := func() { - bpf.Update() - wpf.Update() - if se.IsVisible() { - se.NeedsRender() + ed.SetControlFunc(func(timeStep int) { + if timeStep >= ps.ForceOn && timeStep < ps.ForceOff { + fmt.Println(timeStep, "\tforce on:", ps.Force) + physics.SetJointControlForce(botJoint, 0, ps.Force) + } else { + physics.SetJointControlForce(botJoint, 0, 0) } - } - - sc.Camera.Pose.Pos = math32.Vec3(0, 40, 3.5) - sc.Camera.LookAt(math32.Vec3(0, 5, 0), math32.Vec3(0, 1, 0)) - sc.SaveCamera("3") - - sc.Camera.Pose.Pos = math32.Vec3(-1.33, 2.24, 3.55) - sc.Camera.LookAt(math32.Vec3(0, .5, 0), math32.Vec3(0, 1, 0)) - sc.SaveCamera("2") - - sc.Camera.Pose.Pos = math32.Vec3(0, 6, 4.5) - sc.Camera.LookAt(math32.Vec3(0, 3, 0), math32.Vec3(0, 1, 0)) - sc.SaveCamera("1") - sc.SaveCamera("default") - - isRunning := false - stop := false - - stepNButton := func(p *tree.Plan, n int) { - nm := fmt.Sprintf("Step %d", n) - tree.AddAt(p, nm, func(w *core.Button) { - w.SetText(nm).SetIcon(icons.PlayArrow). - SetTooltip(fmt.Sprintf("Step state %d times", n)). - OnClick(func(e events.Event) { - if isRunning { - fmt.Println("still running...") - return - } - go func() { - isRunning = true - for range n { - wl.Step() - cycle++ - if cycle >= ps.ForceOn && cycle < ps.ForceOff { - fmt.Println(cycle, "\tforce on:", ps.Force) - physics.SetJointControlForce(topJoint, 0, ps.Force) - } else { - physics.SetJointControlForce(topJoint, 0, 0) - } - wr.Update() - if se.IsVisible() { - se.AsyncLock() - se.NeedsRender() - se.AsyncUnlock() - // time.Sleep(1 * time.Nanosecond) - } - if stop { - stop = false - break - } - } - isRunning = false - }() - }) - w.Styler(func(s *styles.Style) { - s.SetAbilities(true, abilities.RepeatClickable) - }) - }) - } - - b.AddTopBar(func(bar *core.Frame) { - core.NewToolbar(bar).Maker(func(p *tree.Plan) { - tree.Add(p, func(w *core.Button) { - w.SetText("Init").SetIcon(icons.Reset). - SetTooltip("Reset physics state back to starting."). - OnClick(func(e events.Event) { - if isRunning { - fmt.Println("still running...") - return - } - stop = false - cycle = 0 - wl.InitState() - wr.Update() - updateView() - }) - }) - tree.Add(p, func(w *core.Button) { - w.SetText("Stop").SetIcon(icons.Stop). - SetTooltip("Stop running"). - OnClick(func(e events.Event) { - stop = true - }) - }) - tree.Add(p, func(w *core.Separator) {}) - - stepNButton(p, 1) - stepNButton(p, 10) - stepNButton(p, 100) - stepNButton(p, 1000) - stepNButton(p, 10000) - tree.Add(p, func(w *core.Separator) {}) - - tree.Add(p, func(w *core.Button) { - w.SetText("Rebuild").SetIcon(icons.Reset). - SetTooltip("Rebuild the environment, when you change parameters"). - OnClick(func(e events.Event) { - if isRunning { - fmt.Println("still running...") - return - } - stop = false - cycle = 0 - config() - updateView() - }) - }) - }) }) + b.RunMainWindow() } diff --git a/physics/params.go b/physics/params.go index 9cf033a9..f27fee8d 100644 --- a/physics/params.go +++ b/physics/params.go @@ -5,6 +5,7 @@ package physics import ( + "cogentcore.org/core/math32" "cogentcore.org/lab/gosl/slbool" "cogentcore.org/lab/gosl/slvec" ) @@ -119,4 +120,18 @@ func (pr *PhysParams) Defaults() { pr.MaxGeomIter = 10 } +// StepsToMsec returns the given number of individual Step calls +// converted into milliseconds, suitable for driving controls. +func (pr *PhysParams) StepsToMsec(steps int) int { + msper := 1000 * pr.Dt * float32(pr.SubSteps) + return int(math32.Round(float32(steps) * msper)) +} + +// StepsToMsec returns the given number of individual Step calls +// converted into milliseconds, suitable for driving controls, +// Using the currently-set Params. +func StepsToMsec(steps int) int { + return GetParams(0).StepsToMsec(steps) +} + //gosl:end diff --git a/physics/world/editor.go b/physics/world/editor.go new file mode 100644 index 00000000..c4128dd4 --- /dev/null +++ b/physics/world/editor.go @@ -0,0 +1,262 @@ +// Copyright (c) 2025, Cogent Core. All rights reserved. +// Use of this source code is governed by a BSD-style +// license that can be found in the LICENSE file. + +package world + +import ( + "fmt" + + "cogentcore.org/core/colors" + "cogentcore.org/core/core" + "cogentcore.org/core/events" + "cogentcore.org/core/icons" + "cogentcore.org/core/math32" + "cogentcore.org/core/styles" + "cogentcore.org/core/styles/abilities" + "cogentcore.org/core/tree" + "cogentcore.org/core/xyz" + "cogentcore.org/core/xyz/xyzcore" + _ "cogentcore.org/lab/gosl/slbool/slboolcore" // include to get gui views + "cogentcore.org/lab/physics" +) + +// Editor provides a basic viewer and parameter controller widget +// for exploring physics models. +type Editor struct { //types:add + core.Frame + + // Physics has the physics simulation. + Physics *physics.World + + // World has the 3D GUI visualization. + World *World + + // UserParams is a struct with parameters for configuring the physics sim. + // These are displayed in the editor. + UserParams any + + // ConfigFunc is the function that configures the world. + ConfigFunc func() + + // ControlFunc is the function that sets control parameters, + // based on the current timestep (in milliseconds, converted from physics time). + ControlFunc func(timeStep int) + + // IsRunning is true if currently running sim. + isRunning bool + + // Stop triggers topping of running. + stop bool + + // TimeStep is current time step in physics update cycles. + TimeStep int + + // Scene is the xyz GUI visualization widget. + scene *xyzcore.SceneEditor + + // Toolbar is the top toolbar. + toolbar *core.Toolbar + + // Splits is the container for elements. + splits *core.Splits + + // UserParamsForm has the user's config parameters. + userParamsForm *core.Form + + // ParamsForm has the Physics parameters. + paramsForm *core.Form +} + +func (pe *Editor) CopyFieldsFrom(frm tree.Node) { + fr := frm.(*Editor) + pe.Frame.CopyFieldsFrom(&fr.Frame) +} + +func (pe *Editor) Init() { + pe.Frame.Init() + + pe.Styler(func(s *styles.Style) { + s.Grow.Set(1, 1) + s.Direction = styles.Column + }) + + // pe.Updater(func() { + // pe.World.Update() + // }) + + pe.OnShow(func(e events.Event) { + if pe.UserParams != nil { + pe.userParamsForm.SetStruct(pe.UserParams) + } + params := &pe.Physics.Params[0] + pe.paramsForm.SetStruct(params) + pe.Update() + }) + + tree.AddChildAt(pe, "tb", func(w *core.Toolbar) { + pe.toolbar = w + w.Maker(pe.MakeToolbar) + }) + + tree.AddChildAt(pe, "splits", func(w *core.Splits) { + pe.splits = w + pe.splits.SetSplits(0.2, 0.8) + tree.AddChildAt(w, "forms", func(w *core.Frame) { + w.Styler(func(s *styles.Style) { + s.Direction = styles.Column + s.Grow.Set(1, 1) + }) + tree.AddChildAt(w, "users", func(w *core.Form) { + pe.userParamsForm = w + }) + tree.AddChildAt(w, "params", func(w *core.Form) { + pe.paramsForm = w + }) + }) + + tree.AddChildAt(w, "scene", func(w *xyzcore.SceneEditor) { + pe.scene = w + w.UpdateWidget() + sc := pe.scene.SceneXYZ() + + sc.Background = colors.Scheme.Select.Container + xyz.NewAmbient(sc, "ambient", 0.3, xyz.DirectSun) + + dir := xyz.NewDirectional(sc, "dir", 1, xyz.DirectSun) + dir.Pos.Set(0, 2, 1) + + pe.World = NewWorld(sc) + pe.Physics = physics.NewWorld() + + sc.Camera.Pose.Pos = math32.Vec3(0, 40, 3.5) + sc.Camera.LookAt(math32.Vec3(0, 5, 0), math32.Vec3(0, 1, 0)) + sc.SaveCamera("3") + + sc.Camera.Pose.Pos = math32.Vec3(-1.33, 2.24, 3.55) + sc.Camera.LookAt(math32.Vec3(0, .5, 0), math32.Vec3(0, 1, 0)) + sc.SaveCamera("2") + + sc.Camera.Pose.Pos = math32.Vec3(0, 25, 20) + sc.Camera.LookAt(math32.Vec3(0, 3, 0), math32.Vec3(0, 1, 0)) + sc.SaveCamera("1") + sc.SaveCamera("default") + + pe.ConfigWorld() + }) + }) +} + +// ConfigWorld configures the physics world. +func (pe *Editor) ConfigWorld() { + if pe.isRunning { + core.MessageSnackbar(pe, "Simulation is still running...") + return + } + pe.World.Reset() + pe.Physics.Reset() + if pe.ConfigFunc != nil { + pe.ConfigFunc() + } + pe.World.Init(pe.Physics) + pe.World.Update() + pe.stop = false + pe.TimeStep = 0 +} + +// Restart restarts the simulation, returning true if successful (i.e., not running). +func (pe *Editor) Restart() bool { + if pe.isRunning { + core.MessageSnackbar(pe, "Simulation is still running...") + return false + } + pe.stop = false + pe.TimeStep = 0 + pe.World.Init(pe.Physics) + pe.World.Update() + pe.Update() + return true +} + +func (pe *Editor) MakeToolbar(p *tree.Plan) { + stepNButton := func(n int) { + nm := fmt.Sprintf("Step %d", n) + tree.AddAt(p, nm, func(w *core.Button) { + w.FirstStyler(func(s *styles.Style) { s.SetEnabled(!pe.isRunning) }) + w.SetText(nm).SetIcon(icons.PlayArrow). + SetTooltip(fmt.Sprintf("Step state %d times", n)). + OnClick(func(e events.Event) { + if pe.isRunning { + fmt.Println("still running...") + return + } + go func() { + pe.isRunning = true + pe.toolbar.AsyncLock() + pe.toolbar.UpdateRender() + pe.toolbar.AsyncUnlock() + for range n { + if pe.ControlFunc != nil { + pe.ControlFunc(physics.StepsToMsec(pe.TimeStep)) + } + pe.Physics.Step() + pe.TimeStep++ + pe.World.Update() + pe.Scene.AsyncLock() + pe.Scene.NeedsRender() + pe.Scene.AsyncUnlock() + if pe.stop { + pe.stop = false + break + } + } + pe.isRunning = false + pe.AsyncLock() + pe.Update() + pe.AsyncUnlock() + }() + }) + w.Styler(func(s *styles.Style) { + s.SetAbilities(true, abilities.RepeatClickable) + }) + }) + } + + tree.Add(p, func(w *core.Button) { + w.SetText("Restart").SetIcon(icons.Reset). + SetTooltip("Reset physics state back to starting."). + OnClick(func(e events.Event) { + pe.Restart() + }) + w.FirstStyler(func(s *styles.Style) { s.SetEnabled(!pe.isRunning) }) + }) + tree.Add(p, func(w *core.Button) { + w.SetText("Stop").SetIcon(icons.Stop). + SetTooltip("Stop running"). + OnClick(func(e events.Event) { + pe.stop = true + }) + w.FirstStyler(func(s *styles.Style) { s.SetEnabled(pe.isRunning) }) + }) + tree.Add(p, func(w *core.Separator) {}) + + stepNButton(1) + stepNButton(10) + stepNButton(100) + stepNButton(1000) + stepNButton(10000) + + tree.Add(p, func(w *core.Separator) {}) + + tree.Add(p, func(w *core.Button) { + w.SetText("Rebuild").SetIcon(icons.Reset). + SetTooltip("Rebuild the environment, when you change parameters"). + OnClick(func(e events.Event) { + if !pe.Restart() { + return + } + pe.ConfigWorld() + }) + w.FirstStyler(func(s *styles.Style) { s.SetEnabled(!pe.isRunning) }) + }) +} diff --git a/physics/world/typegen.go b/physics/world/typegen.go index 6b2fc78d..6c91d27c 100644 --- a/physics/world/typegen.go +++ b/physics/world/typegen.go @@ -3,9 +3,46 @@ package world import ( + "cogentcore.org/core/tree" "cogentcore.org/core/types" + "cogentcore.org/lab/physics" ) var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics/world.Camera", IDName: "camera", Doc: "Camera defines the properties of a camera needed for rendering from a node.", Fields: []types.Field{{Name: "Size", Doc: "size of image to record"}, {Name: "FOV", Doc: "field of view in degrees"}, {Name: "Near", Doc: "near plane z coordinate"}, {Name: "Far", Doc: "far plane z coordinate"}, {Name: "MaxD", Doc: "maximum distance for depth maps. Anything above is 1.\nThis is independent of Near / Far rendering (though must be < Far)\nand is for normalized depth maps."}, {Name: "LogD", Doc: "use the natural log of 1 + depth for normalized depth values in display etc."}, {Name: "MSample", Doc: "number of multi-samples to use for antialising -- 4 is best and default."}, {Name: "UpDir", Doc: "up direction for camera. Defaults to positive Y axis,\nand is reset by call to LookAt method."}}}) -var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics/world.World", IDName: "world", Doc: "World connects a Virtual World with an [xyz.Scene] to visualize the world,\nincluding ability to render offscreen.", Fields: []types.Field{{Name: "World", Doc: "World is the root Group node of the virtual world"}, {Name: "Scene", Doc: "Scene is the [xyz.Scene] object for visualizing."}, {Name: "Root", Doc: "Root is the root Group node in the Scene under which the world is rendered."}}}) +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics/world.Editor", IDName: "editor", Doc: "Editor provides a basic viewer and parameter controller widget\nfor exploring physics models.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Embeds: []types.Field{{Name: "Frame"}}, Fields: []types.Field{{Name: "Physics", Doc: "Physics has the physics simulation."}, {Name: "World", Doc: "World has the 3D GUI visualization."}, {Name: "UserParams", Doc: "UserParams is a struct with parameters for configuring the physics sim.\nThese are displayed in the editor."}, {Name: "ConfigFunc", Doc: "ConfigFunc is the function that configures the world."}, {Name: "ControlFunc", Doc: "ControlFunc is the function that sets control parameters,\nbased on the current timestep (in milliseconds, converted from physics time)."}, {Name: "isRunning", Doc: "IsRunning is true if currently running sim."}, {Name: "stop", Doc: "Stop triggers topping of running."}, {Name: "TimeStep", Doc: "TimeStep is current time step in physics update cycles."}, {Name: "scene", Doc: "Scene is the xyz GUI visualization widget."}, {Name: "toolbar", Doc: "Toolbar is the top toolbar."}, {Name: "splits", Doc: "Splits is the container for elements."}, {Name: "userParamsForm", Doc: "UserParamsForm has the user's config parameters."}, {Name: "paramsForm", Doc: "ParamsForm has the Physics parameters."}}}) + +// NewEditor returns a new [Editor] with the given optional parent: +// Editor provides a basic viewer and parameter controller widget +// for exploring physics models. +func NewEditor(parent ...tree.Node) *Editor { return tree.New[Editor](parent...) } + +// SetPhysics sets the [Editor.Physics]: +// Physics has the physics simulation. +func (t *Editor) SetPhysics(v *physics.World) *Editor { t.Physics = v; return t } + +// SetWorld sets the [Editor.World]: +// World has the 3D GUI visualization. +func (t *Editor) SetWorld(v *World) *Editor { t.World = v; return t } + +// SetUserParams sets the [Editor.UserParams]: +// UserParams is a struct with parameters for configuring the physics sim. +// These are displayed in the editor. +func (t *Editor) SetUserParams(v any) *Editor { t.UserParams = v; return t } + +// SetConfigFunc sets the [Editor.ConfigFunc]: +// ConfigFunc is the function that configures the world. +func (t *Editor) SetConfigFunc(v func()) *Editor { t.ConfigFunc = v; return t } + +// SetControlFunc sets the [Editor.ControlFunc]: +// ControlFunc is the function that sets control parameters, +// based on the current timestep (in milliseconds, converted from physics time). +func (t *Editor) SetControlFunc(v func(timeStep int)) *Editor { t.ControlFunc = v; return t } + +// SetTimeStep sets the [Editor.TimeStep]: +// TimeStep is current time step in physics update cycles. +func (t *Editor) SetTimeStep(v int) *Editor { t.TimeStep = v; return t } + +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics/world.View", IDName: "view", Doc: "View has visualization functions for physics elements.", Fields: []types.Field{{Name: "Name", Doc: "Name is a name for element (index always appended)."}, {Name: "Shape", Doc: "Shape is the physical shape of the element."}, {Name: "Color", Doc: "Color is the color of the element."}, {Name: "Size", Doc: "Size is the size (per shape)."}, {Name: "Pos", Doc: "Pos is the position."}, {Name: "Quat", Doc: "Quat is the rotation as a quaternion."}, {Name: "NewView", Doc: "NewView is a function that returns a new [xyz.Node]\nto represent this element. If nil, uses appropriate defaults."}, {Name: "InitView", Doc: "InitView is a function that initializes a new [xyz.Node]\nthat represents this element. If nil, uses appropriate defaults."}, {Name: "Index", Doc: "Index is the index of the element in a list."}, {Name: "DynamicIndex", Doc: "DynamicIndex is the index of a dynamic element (-1 if not dynamic)."}}}) + +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics/world.World", IDName: "world", Doc: "World displays a [physics.World] using a [xyz.Scene].\nOne World can be used for multiple different [physics.World]s which\nis more efficient when running multiple in parallel.\nInitial construction of the physics and visualization happens here.", Fields: []types.Field{{Name: "Scene", Doc: "Scene is the [xyz.Scene] object for visualizing."}, {Name: "Root", Doc: "Root is the root Group node in the Scene under which the world is rendered."}, {Name: "Views", Doc: "Views are the view elements for each body in [physics.World]."}}}) From 152d5fb6d1b14f2d40332c0c3e107cb940e0130e Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Wed, 24 Dec 2025 21:05:34 +0100 Subject: [PATCH 53/97] physics: rename physics/world -> phyxyz ("physics"; xyz view of physics -- world is a terrible name at least), and integrate with docs -- docs example of pendulum now in place. Added view api for setting body and world. added inertia for cylinder and capsule. --- docs/content/physics.md | 86 ++- physics/README.md | 6 +- physics/body.go | 15 + physics/body.goal | 15 + physics/examples/balls/balls.go | 4 +- physics/examples/pendula/pendula.go | 22 +- physics/examples/test1/test1.go | 4 +- physics/examples/virtroom/virtroom.go | 20 +- physics/{world => phyxyz}/camera.go | 2 +- physics/{world => phyxyz}/depth.go | 2 +- physics/{world => phyxyz}/editor.go | 30 +- physics/{world => phyxyz}/nogui.go | 2 +- physics/phyxyz/typegen.go | 48 ++ physics/{world => phyxyz}/view.go | 125 ++++- physics/{world => phyxyz}/world.go | 8 +- physics/shapes.go | 93 +--- physics/world/typegen.go | 48 -- .../cogentcore_org-lab-physics-phyxyz.go | 26 + .../labsymbols/cogentcore_org-lab-physics.go | 504 ++++++++++++++++++ yaegilab/labsymbols/make | 2 +- 20 files changed, 879 insertions(+), 183 deletions(-) rename physics/{world => phyxyz}/camera.go (98%) rename physics/{world => phyxyz}/depth.go (99%) rename physics/{world => phyxyz}/editor.go (91%) rename physics/{world => phyxyz}/nogui.go (97%) create mode 100644 physics/phyxyz/typegen.go rename physics/{world => phyxyz}/view.go (56%) rename physics/{world => phyxyz}/world.go (96%) delete mode 100644 physics/world/typegen.go create mode 100644 yaegilab/labsymbols/cogentcore_org-lab-physics-phyxyz.go create mode 100644 yaegilab/labsymbols/cogentcore_org-lab-physics.go diff --git a/docs/content/physics.md b/docs/content/physics.md index b7df7321..101349a3 100644 --- a/docs/content/physics.md +++ b/docs/content/physics.md @@ -2,14 +2,94 @@ bibfile = "ccnlab.json" +++ -**Physics** is a 3D physics simulator for creating virtual environments, including simulated robots and animals, which can run on the GPU or CPU using [[GoSL]]. See [[doc:physics]] for the API docs. The [xyz](https://cogentcore.org/core/xyz) 3D visualization framework can be used to view the physics using the [[doc:physics/world]] package, including grabbing first-person views from the perspective of a body element within the virtual world. It is actively used for simulating motor learning in [axon](https://github.com/emergent/axon). +**Physics** is a 3D physics simulator for creating virtual environments, including simulated robots and animals, which can run on the GPU or CPU using [[GoSL]]. See [[doc:physics]] for the API docs. The [xyz](https://cogentcore.org/core/xyz) 3D visualization framework can be used to view the physics using the [[doc:physics/phyxyz]] package, including grabbing first-person views from the perspective of a body element within the virtual world. It is actively used for simulating motor learning in [axon](https://github.com/emergent/axon). -It is based on the design and algorithms from the [newton-physics](https://newton-physics.github.io/newton/guide/overview.html) framework developed by Disney Research, Google DeepMind, and NVIDIA, and implemented in the [NVIDIA Warp](https://nvidia.github.io/warp/basics.html) framework, which is conceptually similar to GoSL. +Physics is based on the design and algorithms from the [newton-physics](https://newton-physics.github.io/newton/guide/overview.html) framework developed by Disney Research, Google DeepMind, and NVIDIA, and implemented in the [NVIDIA Warp](https://nvidia.github.io/warp/basics.html) framework, which is conceptually similar to GoSL. The XPBD (eXtended Position-Based Dynamics) solver is exclusively implemented ([[@MacklinMullerChentanez16]] and [[@MullerMacklinChentanezEtAl20]]), which has impressive capabilities as shown in this [YouTube video](https://www.youtube.com/watch?v=CPq87E1vD8k). The key idea is to avoid working in the world of higher-order derivatives (accelerations) and use robust position-based updates, as discussed in [[#Physics solver algorithms]]. Consistent with [xyz](https://cogentcore.org/core/xyz) and [gpu](https://cogentcore.org/core/gpu), the default coordinate system has `Y` as the up axis, and `Z` is the depth axis, consistent with the [USD](https://openusd.org/release/index.html) standard. Newton-physics uses `Z` up by default, which is the robotics standard. +## Examples + +### Multi-link pendulum + +The following example shows a complete simulation of a multi-link pendulum: + +{id="sim_pendula" title="N Pendula" collapsed="false"} +```Goal +ed := phyxyz.NewEditor(b) +ed.CameraPos = math32.Vec3(0, 3, 3) +ed.Styler(func(s *styles.Style) { + s.Min.Y.Em(40) +}) + +params := struct{NPendula int}{} + +params.NPendula = 2 +ed.SetUserParams(¶ms) + +ed.SetConfigFunc(func() { + ph := ed.Physics + wr := ed.World + hsz := math32.Vec3(0.05, .2, 0.05) + mass := float32(0.1) + stY := 4*hsz.Y + x := -hsz.Y + + rleft := math32.NewQuatAxisAngle(math32.Vec3(0, 0, 1), -math32.Pi/2) + pb := wr.NewDynamic(ph, "top", physics.Capsule, "blue", mass, hsz, math32.Vec3(x, stY, 0), rleft) + pb.SetBodyGroup(1) // no collide across groups + ji := pb.NewJointRevolute(ph, nil, math32.Vec3(0, stY, 0), math32.Vec3(0, hsz.Y, 0), math32.Vec3(0, 0, 1)) + physics.SetJointTargetPos(ji, 0, 0, 0) + physics.SetJointTargetVel(ji, 0, 0, 0) + + for i := 1; i < params.NPendula; i++ { + clr := colors.Names[i%len(colors.Names)] + x = -float32(i)*hsz.Y*2 - hsz.Y + cb := wr.NewDynamic(ph, "child", physics.Capsule, clr, mass, hsz, math32.Vec3(x, stY, 0), rleft) + cb.SetBodyGroup(1+i) + ji = cb.NewJointRevolute(ph, pb, math32.Vec3(0, -hsz.Y, 0), math32.Vec3(0, hsz.Y, 0), math32.Vec3(0, 0, 1)) + physics.SetJointTargetPos(ji, 0, 0, 0) + physics.SetJointTargetVel(ji, 0, 0, 0) + pb = cb + } +}) +``` + +The [[doc:physics/phyxyz/Editor]] widget provides the [[doc:physics/World]] and [[doc:physics/phyxyz/World]] elements, and the `ConfigFunc` function that configures the physics elements. Stepping through these elements in order: + +```go + rleft := math32.NewQuatAxisAngle(math32.Vec3(0, 0, 1), -math32.Pi/2) + pb := wr.NewDynamic(ph, "top", physics.Box, "blue", mass, hsz, math32.Vec3(x, stY, 0), rleft) +``` + +The `math32.Quat` quaternion provides all the rotational math used in `xyz` and `physics`, and the `rleft` instance represents a -90 degree rotation about the Z (depth axis), which is what causes the pendulum to start in a horizontal orientation. + +The `NewDynamic` method adds a new dynamic body element with a default visualization (this is an `phyxyz` wrapper around the same method in the physics space). Dynamic elements are updated by the physics engine, while `NewBody` would create a static rigid body element that doesn't move (unless you specifically change its position). The return value is a [[doc:physics/phyxyz/View]] which provides the visualization of a physics body. It uses the `Index` of the body to get updated values. The same visualization can be used for any physics sim with the same order of bodies. + +```go + pb.SetBodyGroup(1) // no collide across groups +``` + +The `Group` property of a body can be set to fine-tune collision logic. Positive-numbered groups only collide with each other and any negative-numbered groups, while negative-numbered groups only collide with positive numbered and not within the group. 0 means it doesn't collide with anything. With the crazy dynamics that emerge with multiple arms, it is good to let them all pass through each other. + +```go + ji := pb.NewJointRevolute(ph, nil, math32.Vec3(0, stY, 0), math32.Vec3(0, hsz.Y, 0), math32.Vec3(0, 0, 1)) + physics.SetJointTargetPos(ji, 0, 0, 0) + physics.SetJointTargetVel(ji, 0, 0, 0) +``` + +This creates a new joint where the `pb` body is the child and `nil` means the "world" is the parent (i.e., it is just anchored in a fixed world location). We specify the parent and child relative positions for this joint, which is relative to each such body (in the case of a world joint, it is in world coordinates). Note that these are the _unrotated_ positions, so we are specifying the _vertical_ (Y) axis offset here for the child (`pb`) body. This means that the joint is positioned at the top of the body, because all sizes are specified as _half_ sizes (like a radius instead of a diameter). + +The next two lines specify the target position and velocity of this joint, along with a _stiffness_ (position) and _damping_ (velocity) parameter, which indicate how _strongly_ to enforce these constraints. The values of 0 here indicate that they are not enforced at all, because we want the links to swing freely. You can try using positive values there and see what happens! + +The remaining code just does this same kind of thing for the further links, and should be relatively clear. + +### Joint control + + + ## GoSL infrastructure As discussed in [[GoSL]], to run equivalent code on the GPU and the CPU (i.e., standard Go), all of the data needs to be represented in large arrays, implemented via [[tensor]]s, and all processing occurs via _parallel for loops_ that effectively process each element of these data arrays in parallel. Enum types are used to define the variables as the last inner-most dimension in the data tensors, e.g., [[doc:physics.BodyVars]], with accessor functions to get the relevant `math32` types (e.g., `math32.Vector3`) across the X,Y,Z components. @@ -56,7 +136,7 @@ Use `NewJoint*` with _dynamic_ body indexes to create joints (e.g., `NewJointPri ## World viewer -Typically, bodies are created using the enhanced functions in the [[doc:physics/world]] package, which provides a [[doc:physics/world.View]] wrapper for physics bodies. This wrapper has a default `Color` setting to provide simple color coding of bodies, and supports `NewView` and `InitView` functions that allow arbitrary visualization dynamics to be associated with each body (textures, meshes, dynamic updating etc). +Typically, bodies are created using the enhanced functions in the [[doc:physics/phyxyz]] package, which provides a [[doc:physics/phyxyz.View]] wrapper for physics bodies. This wrapper has a default `Color` setting to provide simple color coding of bodies, and supports `NewView` and `InitView` functions that allow arbitrary visualization dynamics to be associated with each body (textures, meshes, dynamic updating etc). ## Physics solver algorithms diff --git a/physics/README.md b/physics/README.md index 2e5248c0..75fe4998 100644 --- a/physics/README.md +++ b/physics/README.md @@ -4,10 +4,14 @@ The `physics` engine is a 3D physics simulator for creating virtual environments See [physics docs](https://cogentcore.org/lab/physics) for the main docs. -The [world](world) visualization sub-package manages a `View` element that links to physics bodies and generates an [xyz](https://cogentcore.org/core/xyz) 3D scenegraph based on the physics bodies, and updates this visualization efficiently as the physics is updated. +The [phyxyz](phyxyz) ("physics") visualization sub-package manages a `View` element that links to physics bodies and generates an [xyz](https://cogentcore.org/core/xyz) 3D scenegraph based on the physics bodies, and updates this visualization efficiently as the physics is updated. There is an `Editor` widget that makes it easy to explore physics sims. ## TODO +* sphere-sphere collision definitely not right: sometimes too much and sometimes not at all. do all the tests.. + +* pendula: if starting in vertical with 4+ links, it gets unstable when target pos is at 0, even with 0 stiff! + * Muscles: https://mujoco.readthedocs.io/en/stable/modeling.html#muscles * fix basic issues in restitution: needs a more thorough approach. Basically need to integrate during entire time of penetration and then compute escape velocity based on the saved incoming velocity just prior to impact. diff --git a/physics/body.go b/physics/body.go index 8f5be162..ee3dd834 100644 --- a/physics/body.go +++ b/physics/body.go @@ -134,6 +134,9 @@ func GetBodyDynamic(idx int32) int32 { return int32(math.Float32bits(Bodies.Value(int(idx), int(BodyDynamic)))) } +// SetBodyWorld partitions bodies into different worlds for +// collision detection: Global bodies = -1 can collide with +// everything; otherwise only items within the same world collide. func SetBodyWorld(idx, w int32) { Bodies.Set(math.Float32frombits(uint32(w)), int(idx), int(BodyWorld)) } @@ -142,6 +145,18 @@ func GetBodyWorld(idx int32) int32 { return int32(math.Float32bits(Bodies.Value(int(idx), int(BodyWorld)))) } +// SetBodyGroup partitions bodies within worlds into different groups +// for collision detection. 0 does not collide with anything. +// Negative numbers are global within a world, except they don't +// collide amongst themselves (all non-dynamic bodies should go +// in -1 because they don't collide amongst each-other, but do +// potentially collide with dynamics). +// Positive numbers only collide amongst themselves, and with +// negative groups, but not other positive groups. This is for +// more special-purpose dynamics: in general use 1 for all dynamic +// bodies. There is an automatic constraint that the two objects +// within a single joint do not collide with each other, so this +// does not need to be handled here. func SetBodyGroup(idx, w int32) { Bodies.Set(math.Float32frombits(uint32(w)), int(idx), int(BodyGroup)) } diff --git a/physics/body.goal b/physics/body.goal index fa83a0f0..fe10b405 100644 --- a/physics/body.goal +++ b/physics/body.goal @@ -132,6 +132,9 @@ func GetBodyDynamic(idx int32) int32 { return int32(math.Float32bits(Bodies[idx, BodyDynamic])) } +// SetBodyWorld partitions bodies into different worlds for +// collision detection: Global bodies = -1 can collide with +// everything; otherwise only items within the same world collide. func SetBodyWorld(idx, w int32) { Bodies[idx, BodyWorld] = math.Float32frombits(uint32(w)) } @@ -140,6 +143,18 @@ func GetBodyWorld(idx int32) int32 { return int32(math.Float32bits(Bodies[idx, BodyWorld])) } +// SetBodyGroup partitions bodies within worlds into different groups +// for collision detection. 0 does not collide with anything. +// Negative numbers are global within a world, except they don't +// collide amongst themselves (all non-dynamic bodies should go +// in -1 because they don't collide amongst each-other, but do +// potentially collide with dynamics). +// Positive numbers only collide amongst themselves, and with +// negative groups, but not other positive groups. This is for +// more special-purpose dynamics: in general use 1 for all dynamic +// bodies. There is an automatic constraint that the two objects +// within a single joint do not collide with each other, so this +// does not need to be handled here. func SetBodyGroup(idx, w int32) { Bodies[idx, BodyGroup] = math.Float32frombits(uint32(w)) } diff --git a/physics/examples/balls/balls.go b/physics/examples/balls/balls.go index 16e93a2b..ff329243 100644 --- a/physics/examples/balls/balls.go +++ b/physics/examples/balls/balls.go @@ -14,7 +14,7 @@ import ( "cogentcore.org/core/math32" _ "cogentcore.org/lab/gosl/slbool/slboolcore" // include to get gui views "cogentcore.org/lab/physics" - "cogentcore.org/lab/physics/world" + "cogentcore.org/lab/physics/phyxyz" ) // Balls has sim params @@ -63,7 +63,7 @@ func (b *Balls) Defaults() { func main() { b := core.NewBody("test1").SetTitle("Physics Balls") - ed := world.NewEditor(b) + ed := phyxyz.NewEditor(b) bs := &Balls{} bs.Defaults() diff --git a/physics/examples/pendula/pendula.go b/physics/examples/pendula/pendula.go index d01f33cb..975e92a8 100644 --- a/physics/examples/pendula/pendula.go +++ b/physics/examples/pendula/pendula.go @@ -14,7 +14,7 @@ import ( "cogentcore.org/core/math32" _ "cogentcore.org/lab/gosl/slbool/slboolcore" // include to get gui views "cogentcore.org/lab/physics" - "cogentcore.org/lab/physics/world" + "cogentcore.org/lab/physics/phyxyz" ) // Pendula has sim params @@ -73,7 +73,8 @@ func (b *Pendula) Defaults() { func main() { b := core.NewBody("test1").SetTitle("Physics Pendula") - ed := world.NewEditor(b) + ed := phyxyz.NewEditor(b) + ed.CameraPos = math32.Vec3(0, 3, 3) ps := &Pendula{} ps.Defaults() @@ -92,7 +93,7 @@ func main() { rleft = rot } - stY := 2*ps.HSize.Y*float32(ps.NPendula+1) + 1 + stY := 4 * ps.HSize.Y clr := colors.Names[0] x := -ps.HSize.Y y := stY @@ -100,31 +101,30 @@ func main() { x = 0 y -= ps.HSize.Y } - pb := wr.NewDynamic(ph, "top", physics.Box, clr, ps.Mass, ps.HSize, math32.Vec3(x, y, 0), rleft) + pb := wr.NewDynamic(ph, "top", physics.Capsule, clr, ps.Mass, ps.HSize, math32.Vec3(x, y, 0), rleft) if !ps.Collide { - physics.SetBodyGroup(pb.Index, int32(1)) + pb.SetBodyGroup(1) } targ := math32.DegToRad(float32(ps.TargetDegFromVert)) - ji := ph.NewJointRevolute(-1, pb.DynamicIndex, math32.Vec3(0, stY, 0), math32.Vec3(0, ps.HSize.Y, 0), math32.Vec3(0, 0, 1)) + ji := pb.NewJointRevolute(ph, nil, math32.Vec3(0, stY, 0), math32.Vec3(0, ps.HSize.Y, 0), math32.Vec3(0, 0, 1)) physics.SetJointTargetPos(ji, 0, targ, ps.Stiff) physics.SetJointTargetVel(ji, 0, 0, ps.Damp) for i := 1; i < ps.NPendula; i++ { - clr := colors.Names[i%len(colors.Names)] + clr := colors.Names[12+i%len(colors.Names)] x := -float32(i)*ps.HSize.Y*2 - ps.HSize.Y y := stY if ps.StartVert { y = stY + x x = 0 } - cb := wr.NewDynamic(ph, "child", physics.Box, clr, ps.Mass, ps.HSize, - math32.Vec3(x, y, 0), rleft) + cb := wr.NewDynamic(ph, "child", physics.Capsule, clr, ps.Mass, ps.HSize, math32.Vec3(x, y, 0), rleft) if !ps.Collide { - physics.SetBodyGroup(cb.Index, int32(1+i)) + cb.SetBodyGroup(1 + i) } - ji = ph.NewJointRevolute(pb.DynamicIndex, cb.DynamicIndex, math32.Vec3(0, -ps.HSize.Y, 0), math32.Vec3(0, ps.HSize.Y, 0), math32.Vec3(0, 0, 1)) + ji = cb.NewJointRevolute(ph, pb, math32.Vec3(0, -ps.HSize.Y, 0), math32.Vec3(0, ps.HSize.Y, 0), math32.Vec3(0, 0, 1)) physics.SetJointTargetPos(ji, 0, targ, ps.Stiff) physics.SetJointTargetVel(ji, 0, 0, ps.Damp) pb = cb diff --git a/physics/examples/test1/test1.go b/physics/examples/test1/test1.go index 1c6b2b2a..b7e8c5e3 100644 --- a/physics/examples/test1/test1.go +++ b/physics/examples/test1/test1.go @@ -21,7 +21,7 @@ import ( "cogentcore.org/core/xyz/xyzcore" _ "cogentcore.org/lab/gosl/slbool/slboolcore" // include to get gui views "cogentcore.org/lab/physics" - "cogentcore.org/lab/physics/world" + "cogentcore.org/lab/physics/phyxyz" ) // Test has sim params @@ -79,7 +79,7 @@ func main() { dir := xyz.NewDirectional(sc, "dir", 1, xyz.DirectSun) dir.Pos.Set(0, 2, 1) - wr := world.NewWorld(sc) + wr := phyxyz.NewWorld(sc) bs := &Test{} bs.Defaults() diff --git a/physics/examples/virtroom/virtroom.go b/physics/examples/virtroom/virtroom.go index 935b60c3..9be1b518 100644 --- a/physics/examples/virtroom/virtroom.go +++ b/physics/examples/virtroom/virtroom.go @@ -24,7 +24,7 @@ import ( "cogentcore.org/core/xyz" "cogentcore.org/core/xyz/xyzcore" "cogentcore.org/lab/physics" - "cogentcore.org/lab/physics/world" + "cogentcore.org/lab/physics/phyxyz" ) var NoGUI bool @@ -75,7 +75,7 @@ type Env struct { //types:add DepthVals []float32 // offscreen render camera settings - Camera world.Camera + Camera phyxyz.Camera // color map to use for rendering depth map DepthMap core.ColorMapName @@ -83,17 +83,17 @@ type Env struct { //types:add // The [physics World]. Physics *physics.World - // Visualization of the physics world. - World *world.World + // Visualization of the physics phyxyz. + World *phyxyz.World // 3D visualization of the Scene SceneEditor *xyzcore.SceneEditor // emer object - Emer *world.View `display:"-"` + Emer *phyxyz.View `display:"-"` // Right eye of emer - EyeR *world.View `display:"-"` + EyeR *phyxyz.View `display:"-"` // snapshot image EyeRImg *core.Image `display:"-"` @@ -123,7 +123,7 @@ func (ev *Env) MakeWorld(sc *xyz.Scene) { dir := xyz.NewDirectional(sc, "dir", 1, xyz.DirectSun) dir.Pos.Set(0, 2, 1) // default: 0,1,1 = above and behind us (we are at 0,0,X) - ev.World = world.NewWorld(sc) + ev.World = phyxyz.NewWorld(sc) ev.MakeRoom("room1", ev.Width, ev.Depth, ev.Height, ev.Thick) ev.MakeEmer("emer", ev.EmerHt) ev.World.Init(ev.Physics) @@ -164,7 +164,7 @@ func (ev *Env) ViewDepth(depth []float32) { cmap := colormap.AvailableMaps[string(ev.DepthMap)] img := image.NewRGBA(image.Rectangle{Max: ev.Camera.Size}) ev.DepthImage.SetImage(img) - world.DepthImage(img, depth, cmap, &ev.Camera) + phyxyz.DepthImage(img, depth, cmap, &ev.Camera) ev.DepthImage.NeedsRender() } @@ -331,7 +331,7 @@ func (ev *Env) ConfigGUI() *core.Body { ev.MakeWorld(sc) // local toolbar for manipulating emer - // etb.Maker(world.MakeStateToolbar(&ev.Emer.Rel, func() { + // etb.Maker(phyxyz.MakeStateToolbar(&ev.Emer.Rel, func() { // ev.World.Update() // ev.SceneEditor.NeedsRender() // })) @@ -433,7 +433,7 @@ func (ev *Env) NoGUIRun() { if err != nil { panic(err) } - sc := world.NoDisplayScene(gp, dev) + sc := phyxyz.NoDisplayScene(gp, dev) ev.MakeWorld(sc) img := ev.RenderEyeImg() diff --git a/physics/world/camera.go b/physics/phyxyz/camera.go similarity index 98% rename from physics/world/camera.go rename to physics/phyxyz/camera.go index a88ca255..b593ffa9 100644 --- a/physics/world/camera.go +++ b/physics/phyxyz/camera.go @@ -2,7 +2,7 @@ // Use of this source code is governed by a BSD-style // license that can be found in the LICENSE file. -package world +package phyxyz import ( "image" diff --git a/physics/world/depth.go b/physics/phyxyz/depth.go similarity index 99% rename from physics/world/depth.go rename to physics/phyxyz/depth.go index 6ac87c18..9a64149c 100644 --- a/physics/world/depth.go +++ b/physics/phyxyz/depth.go @@ -2,7 +2,7 @@ // Use of this source code is governed by a BSD-style // license that can be found in the LICENSE file. -package world +package phyxyz import ( "image" diff --git a/physics/world/editor.go b/physics/phyxyz/editor.go similarity index 91% rename from physics/world/editor.go rename to physics/phyxyz/editor.go index c4128dd4..43283a90 100644 --- a/physics/world/editor.go +++ b/physics/phyxyz/editor.go @@ -2,7 +2,7 @@ // Use of this source code is governed by a BSD-style // license that can be found in the LICENSE file. -package world +package phyxyz import ( "fmt" @@ -43,6 +43,11 @@ type Editor struct { //types:add // based on the current timestep (in milliseconds, converted from physics time). ControlFunc func(timeStep int) + // CameraPos provides the default initial camera position, looking at the origin. + // Set this to larger numbers to zoom out, and smaller numbers to zoom in. + // Defaults to math32.Vec3(0, 25, 20). + CameraPos math32.Vector3 + // IsRunning is true if currently running sim. isRunning bool @@ -75,25 +80,13 @@ func (pe *Editor) CopyFieldsFrom(frm tree.Node) { func (pe *Editor) Init() { pe.Frame.Init() + pe.CameraPos = math32.Vec3(0, 25, 20) pe.Styler(func(s *styles.Style) { s.Grow.Set(1, 1) s.Direction = styles.Column }) - // pe.Updater(func() { - // pe.World.Update() - // }) - - pe.OnShow(func(e events.Event) { - if pe.UserParams != nil { - pe.userParamsForm.SetStruct(pe.UserParams) - } - params := &pe.Physics.Params[0] - pe.paramsForm.SetStruct(params) - pe.Update() - }) - tree.AddChildAt(pe, "tb", func(w *core.Toolbar) { pe.toolbar = w w.Maker(pe.MakeToolbar) @@ -112,6 +105,11 @@ func (pe *Editor) Init() { }) tree.AddChildAt(w, "params", func(w *core.Form) { pe.paramsForm = w + if pe.UserParams != nil { + pe.userParamsForm.SetStruct(pe.UserParams) + } + params := &pe.Physics.Params[0] + pe.paramsForm.SetStruct(params) }) }) @@ -137,8 +135,8 @@ func (pe *Editor) Init() { sc.Camera.LookAt(math32.Vec3(0, .5, 0), math32.Vec3(0, 1, 0)) sc.SaveCamera("2") - sc.Camera.Pose.Pos = math32.Vec3(0, 25, 20) - sc.Camera.LookAt(math32.Vec3(0, 3, 0), math32.Vec3(0, 1, 0)) + sc.Camera.Pose.Pos = pe.CameraPos + sc.Camera.LookAt(math32.Vec3(0, 0, 0), math32.Vec3(0, 1, 0)) sc.SaveCamera("1") sc.SaveCamera("default") diff --git a/physics/world/nogui.go b/physics/phyxyz/nogui.go similarity index 97% rename from physics/world/nogui.go rename to physics/phyxyz/nogui.go index 07be1bc7..3c55d0fe 100644 --- a/physics/world/nogui.go +++ b/physics/phyxyz/nogui.go @@ -2,7 +2,7 @@ // Use of this source code is governed by a BSD-style // license that can be found in the LICENSE file. -package world +package phyxyz import ( "image" diff --git a/physics/phyxyz/typegen.go b/physics/phyxyz/typegen.go new file mode 100644 index 00000000..1ccdf432 --- /dev/null +++ b/physics/phyxyz/typegen.go @@ -0,0 +1,48 @@ +// Code generated by "core generate -add-types"; DO NOT EDIT. + +package phyxyz + +import ( + "cogentcore.org/core/tree" + "cogentcore.org/core/types" + "cogentcore.org/lab/physics" +) + +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics/phyxyz.Camera", IDName: "camera", Doc: "Camera defines the properties of a camera needed for rendering from a node.", Fields: []types.Field{{Name: "Size", Doc: "size of image to record"}, {Name: "FOV", Doc: "field of view in degrees"}, {Name: "Near", Doc: "near plane z coordinate"}, {Name: "Far", Doc: "far plane z coordinate"}, {Name: "MaxD", Doc: "maximum distance for depth maps. Anything above is 1.\nThis is independent of Near / Far rendering (though must be < Far)\nand is for normalized depth maps."}, {Name: "LogD", Doc: "use the natural log of 1 + depth for normalized depth values in display etc."}, {Name: "MSample", Doc: "number of multi-samples to use for antialising -- 4 is best and default."}, {Name: "UpDir", Doc: "up direction for camera. Defaults to positive Y axis,\nand is reset by call to LookAt method."}}}) + +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics/phyxyz.Editor", IDName: "editor", Doc: "Editor provides a basic viewer and parameter controller widget\nfor exploring physics models.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Embeds: []types.Field{{Name: "Frame"}}, Fields: []types.Field{{Name: "Physics", Doc: "Physics has the physics simulation."}, {Name: "World", Doc: "World has the 3D GUI visualization."}, {Name: "UserParams", Doc: "UserParams is a struct with parameters for configuring the physics sim.\nThese are displayed in the editor."}, {Name: "ConfigFunc", Doc: "ConfigFunc is the function that configures the world."}, {Name: "ControlFunc", Doc: "ControlFunc is the function that sets control parameters,\nbased on the current timestep (in milliseconds, converted from physics time)."}, {Name: "isRunning", Doc: "IsRunning is true if currently running sim."}, {Name: "stop", Doc: "Stop triggers topping of running."}, {Name: "TimeStep", Doc: "TimeStep is current time step in physics update cycles."}, {Name: "scene", Doc: "Scene is the xyz GUI visualization widget."}, {Name: "toolbar", Doc: "Toolbar is the top toolbar."}, {Name: "splits", Doc: "Splits is the container for elements."}, {Name: "userParamsForm", Doc: "UserParamsForm has the user's config parameters."}, {Name: "paramsForm", Doc: "ParamsForm has the Physics parameters."}}}) + +// NewEditor returns a new [Editor] with the given optional parent: +// Editor provides a basic viewer and parameter controller widget +// for exploring physics models. +func NewEditor(parent ...tree.Node) *Editor { return tree.New[Editor](parent...) } + +// SetPhysics sets the [Editor.Physics]: +// Physics has the physics simulation. +func (t *Editor) SetPhysics(v *physics.World) *Editor { t.Physics = v; return t } + +// SetWorld sets the [Editor.World]: +// World has the 3D GUI visualization. +func (t *Editor) SetWorld(v *World) *Editor { t.World = v; return t } + +// SetUserParams sets the [Editor.UserParams]: +// UserParams is a struct with parameters for configuring the physics sim. +// These are displayed in the editor. +func (t *Editor) SetUserParams(v any) *Editor { t.UserParams = v; return t } + +// SetConfigFunc sets the [Editor.ConfigFunc]: +// ConfigFunc is the function that configures the world. +func (t *Editor) SetConfigFunc(v func()) *Editor { t.ConfigFunc = v; return t } + +// SetControlFunc sets the [Editor.ControlFunc]: +// ControlFunc is the function that sets control parameters, +// based on the current timestep (in milliseconds, converted from physics time). +func (t *Editor) SetControlFunc(v func(timeStep int)) *Editor { t.ControlFunc = v; return t } + +// SetTimeStep sets the [Editor.TimeStep]: +// TimeStep is current time step in physics update cycles. +func (t *Editor) SetTimeStep(v int) *Editor { t.TimeStep = v; return t } + +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics/phyxyz.View", IDName: "view", Doc: "View has visualization functions for physics elements.", Fields: []types.Field{{Name: "Name", Doc: "Name is a name for element (index always appended)."}, {Name: "Shape", Doc: "Shape is the physical shape of the element."}, {Name: "Color", Doc: "Color is the color of the element."}, {Name: "Size", Doc: "Size is the size (per shape)."}, {Name: "Pos", Doc: "Pos is the position."}, {Name: "Quat", Doc: "Quat is the rotation as a quaternion."}, {Name: "NewView", Doc: "NewView is a function that returns a new [xyz.Node]\nto represent this element. If nil, uses appropriate defaults."}, {Name: "InitView", Doc: "InitView is a function that initializes a new [xyz.Node]\nthat represents this element. If nil, uses appropriate defaults."}, {Name: "Index", Doc: "Index is the index of the element in a list."}, {Name: "DynamicIndex", Doc: "DynamicIndex is the index of a dynamic element (-1 if not dynamic)."}}}) + +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics/phyxyz.World", IDName: "world", Doc: "World displays a [physics.World] using a [xyz.Scene].\nOne World can be used for multiple different [physics.World]s which\nis more efficient when running multiple in parallel.\nInitial construction of the physics and visualization happens here.", Fields: []types.Field{{Name: "Scene", Doc: "Scene is the [xyz.Scene] object for visualizing."}, {Name: "Root", Doc: "Root is the root Group node in the Scene under which the world is rendered."}, {Name: "Views", Doc: "Views are the view elements for each body in [physics.World]."}}}) diff --git a/physics/world/view.go b/physics/phyxyz/view.go similarity index 56% rename from physics/world/view.go rename to physics/phyxyz/view.go index baa313d5..8a40cf74 100644 --- a/physics/world/view.go +++ b/physics/phyxyz/view.go @@ -2,7 +2,7 @@ // Use of this source code is governed by a BSD-style // license that can be found in the LICENSE file. -package world +package phyxyz import ( "strconv" @@ -53,8 +53,8 @@ type View struct { // NewBody adds a new body with given parameters. // Returns the View which can then be further customized. // Use this for Static elements; NewDynamic for dynamic elements. -func (wr *World) NewBody(wl *physics.World, name string, shape physics.Shapes, clr string, size, pos math32.Vector3, rot math32.Quat) *View { - idx := wl.NewBody(shape, size, pos, rot) +func (wr *World) NewBody(ph *physics.World, name string, shape physics.Shapes, clr string, size, pos math32.Vector3, rot math32.Quat) *View { + idx := ph.NewBody(shape, size, pos, rot) vw := &View{Name: name, Index: idx, DynamicIndex: -1, Shape: shape, Color: clr, Size: size, Pos: pos, Quat: rot} wr.Views = append(wr.Views, vw) return vw @@ -62,8 +62,8 @@ func (wr *World) NewBody(wl *physics.World, name string, shape physics.Shapes, c // NewDynamic adds a new dynamic body with given parameters. // Returns the View which can then be further customized. -func (wr *World) NewDynamic(wl *physics.World, name string, shape physics.Shapes, clr string, mass float32, size, pos math32.Vector3, rot math32.Quat) *View { - idx, dyIdx := wl.NewDynamic(shape, mass, size, pos, rot) +func (wr *World) NewDynamic(ph *physics.World, name string, shape physics.Shapes, clr string, mass float32, size, pos math32.Vector3, rot math32.Quat) *View { + idx, dyIdx := ph.NewDynamic(shape, mass, size, pos, rot) vw := &View{Name: name, Index: idx, DynamicIndex: dyIdx, Shape: shape, Color: clr, Size: size, Pos: pos, Quat: rot} wr.Views = append(wr.Views, vw) return vw @@ -218,6 +218,29 @@ func (vw *View) SphereInit(sld *xyz.Solid) { }) } +// SetBodyWorld partitions bodies into different worlds for +// collision detection: Global bodies = -1 can collide with +// everything; otherwise only items within the same world collide. +func (vw *View) SetBodyWorld(world int) { + physics.SetBodyWorld(vw.Index, int32(world)) +} + +// SetBodyGroup partitions bodies within worlds into different groups +// for collision detection. 0 does not collide with anything. +// Negative numbers are global within a world, except they don't +// collide amongst themselves (all non-dynamic bodies should go +// in -1 because they don't collide amongst each-other, but do +// potentially collide with dynamics). +// Positive numbers only collide amongst themselves, and with +// negative groups, but not other positive groups. This is for +// more special-purpose dynamics: in general use 1 for all dynamic +// bodies. There is an automatic constraint that the two objects +// within a single joint do not collide with each other, so this +// does not need to be handled here. +func (vw *View) SetBodyGroup(group int) { + physics.SetBodyGroup(vw.Index, int32(group)) +} + // SetBodyBounce specifies the COR or coefficient of restitution (0..1), // which determines how elastic the collision is, // i.e., final velocity / initial velocity. @@ -239,3 +262,95 @@ func (vw *View) SetBodyFrictionTortion(val float32) { func (vw *View) SetBodyFrictionRolling(val float32) { physics.Bodies.Set(val, int(vw.Index), int(physics.BodyFrictionRolling)) } + +// NewJointFixed adds a new Fixed joint as a child of given parent. +// Use nil for parent to add a world-anchored joint. +// ppos, cpos are the relative positions from the parent, child. +// These are for the non-rotated body (i.e., body rotation is applied +// to these positions as well). +// Sets relative rotation matricies to identity by default. +func (vw *View) NewJointFixed(ph *physics.World, parent *View, ppos, cpos math32.Vector3) int32 { + pidx := int32(-1) + if parent != nil { + pidx = parent.DynamicIndex + } + return ph.NewJointFixed(pidx, vw.DynamicIndex, ppos, cpos) +} + +// NewJointPrismatic adds a new Prismatic (slider) joint as a child +// of given parent. Use nil for parent to add a world-anchored joint. +// ppos, cpos are the relative positions from the parent, child. +// These are for the non-rotated body (i.e., body rotation is applied +// to these positions as well). +// Sets relative rotation matricies to identity by default. +// axis is the axis of articulation for the joint. +// Use [SetJointDoF] to set the remaining DoF parameters. +func (vw *View) NewJointPrismatic(ph *physics.World, parent *View, ppos, cpos, axis math32.Vector3) int32 { + pidx := int32(-1) + if parent != nil { + pidx = parent.DynamicIndex + } + return ph.NewJointPrismatic(pidx, vw.DynamicIndex, ppos, cpos, axis) +} + +// NewJointRevolute adds a new Revolute (hinge, axel) joint as a child +// of given parent. Use nil for parent to add a world-anchored joint. +// ppos, cpos are the relative positions from the parent, child. +// These are for the non-rotated body (i.e., body rotation is applied +// to these positions as well). +// Sets relative rotation matricies to identity by default. +// axis is the axis of articulation for the joint. +// Use [SetJointDoF] to set the remaining DoF parameters. +func (vw *View) NewJointRevolute(ph *physics.World, parent *View, ppos, cpos, axis math32.Vector3) int32 { + pidx := int32(-1) + if parent != nil { + pidx = parent.DynamicIndex + } + return ph.NewJointRevolute(pidx, vw.DynamicIndex, ppos, cpos, axis) +} + +// NewJointBall adds a new Ball joint (3 angular DoF) as a child +// of given parent. Use nil for parent to add a world-anchored joint. +// ppos, cpos are the relative positions from the parent, child. +// These are for the non-rotated body (i.e., body rotation is applied +// to these positions as well). +// Sets relative rotation matricies to identity by default. +// Use [SetJointDoF] to set the remaining DoF parameters. +func (vw *View) NewJointBall(ph *physics.World, parent *View, ppos, cpos math32.Vector3) int32 { + pidx := int32(-1) + if parent != nil { + pidx = parent.DynamicIndex + } + return ph.NewJointBall(pidx, vw.DynamicIndex, ppos, cpos) +} + +// NewJointDistance adds a new Distance joint (6 DoF), +// with distance constrained only on the first linear X axis, +// as a child of given parent. Use nil for parent to add a world-anchored joint. +// ppos, cpos are the relative positions from the parent, child. +// These are for the non-rotated body (i.e., body rotation is applied +// to these positions as well). +// Sets relative rotation matricies to identity by default. +// Use [SetJointDoF] to set the remaining DoF parameters. +func (vw *View) NewJointDistance(ph *physics.World, parent *View, ppos, cpos math32.Vector3, minDist, maxDist float32) int32 { + pidx := int32(-1) + if parent != nil { + pidx = parent.DynamicIndex + } + return ph.NewJointDistance(pidx, vw.DynamicIndex, ppos, cpos, minDist, maxDist) +} + +// NewJointFree adds a new Free joint as a child +// of given parent. Use nil for parent to add a world-anchored joint. +// ppos, cpos are the relative positions from the parent, child. +// These are for the non-rotated body (i.e., body rotation is applied +// to these positions as well). +// Sets relative rotation matricies to identity by default. +// Use [SetJointDoF] to set the remaining DoF parameters. +func (vw *View) NewJointFree(ph *physics.World, parent *View, ppos, cpos math32.Vector3) int32 { + pidx := int32(-1) + if parent != nil { + pidx = parent.DynamicIndex + } + return ph.NewJointFree(pidx, vw.DynamicIndex, ppos, cpos) +} diff --git a/physics/world/world.go b/physics/phyxyz/world.go similarity index 96% rename from physics/world/world.go rename to physics/phyxyz/world.go index 10c7f64a..c83314c6 100644 --- a/physics/world/world.go +++ b/physics/phyxyz/world.go @@ -2,9 +2,9 @@ // Use of this source code is governed by a BSD-style // license that can be found in the LICENSE file. -// package world implements visualization of [physics] using [xyz] +// package phyxyz implements visualization of [physics] using [xyz] // 3D graphics. -package world +package phyxyz //go:generate core generate -add-types @@ -46,8 +46,8 @@ func NewWorld(sc *xyz.Scene) *World { // and calls Config on [physics.World]. // Call this _once_ after making all the new Views and Bodies. // (will return if already called). -func (wr *World) Init(wl *physics.World) { - wl.Config() +func (wr *World) Init(ph *physics.World) { + ph.Config() if len(wr.Root.Makers.Normal) > 0 { return } diff --git a/physics/shapes.go b/physics/shapes.go index 5269dad5..c3b90ae1 100644 --- a/physics/shapes.go +++ b/physics/shapes.go @@ -158,6 +158,22 @@ func (sh Shapes) Inertia(sz math32.Vector3, mass float32) math32.Matrix3 { // v := 4.0 / 3.0 * math32.Pi * r * r * r ia := 2.0 / 5.0 * mass * r * r inertia = math32.Mat3(ia, 0.0, 0.0, 0.0, ia, 0.0, 0.0, 0.0, ia) + case Capsule: + r := sz.X + h := sz.Y * 2 + vs := (4.0 / 3.0) * math32.Pi * r * r * r + vc := math32.Pi * r * r * h + ms := mass * (vs / (vs + vc)) + mc := mass * (vc / (vs + vc)) + ia := mc*(0.25*r*r+(1.0/12.0)*h*h) + ms*(0.4*r*r+0.375*r*h+0.25*h*h) + ib := (mc*0.5 + ms*0.4) * r * r + inertia = math32.Mat3(ia, 0.0, 0.0, 0.0, ib, 0.0, 0.0, 0.0, ia) + case Cylinder: + r := sz.X + h := sz.Y * 2 + ia := (1.0 / 12) * mass * (3*r*r + h*h) + ib := (1.0 / 2.0) * mass * r * r + inertia = math32.Mat3(ia, 0.0, 0.0, 0.0, ib, 0.0, 0.0, 0.0, ia) case Box: w := 2 * sz.X h := 2 * sz.Y @@ -166,64 +182,11 @@ func (sh Shapes) Inertia(sz math32.Vector3, mass float32) math32.Matrix3 { ib := 1.0 / 12.0 * mass * (w*w + d*d) ic := 1.0 / 12.0 * mass * (w*w + h*h) inertia = math32.Mat3(ia, 0.0, 0.0, 0.0, ib, 0.0, 0.0, 0.0, ic) - // todo: others: } return inertia } /* -def compute_capsule_inertia(density: float, r: float, h: float) -> tuple[float, wp.vec3, wp.mat33]: - """Helper to compute mass and inertia of a solid capsule extending along the z-axis - - Args: - density: The capsule density - r: The capsule radius - h: The capsule height (full height of the interior cylinder) - - Returns: - - A tuple of (mass, inertia) with inertia specified around the origin - """ - - ms = density * (4.0 / 3.0) * wp.pi * r * r * r - mc = density * wp.pi * r * r * h - - # total mass - m = ms + mc - - # adapted from ODE - Ia = mc * (0.25 * r * r + (1.0 / 12.0) * h * h) + ms * (0.4 * r * r + 0.375 * r * h + 0.25 * h * h) - Ib = (mc * 0.5 + ms * 0.4) * r * r - - # For Z-axis orientation: I_xx = I_yy = Ia, I_zz = Ib - I = wp.mat33([[Ia, 0.0, 0.0], [0.0, Ia, 0.0], [0.0, 0.0, Ib]]) - - return (m, wp.vec3(), I) - - -def compute_cylinder_inertia(density: float, r: float, h: float) -> tuple[float, wp.vec3, wp.mat33]: - """Helper to compute mass and inertia of a solid cylinder extending along the z-axis - - Args: - density: The cylinder density - r: The cylinder radius - h: The cylinder height (extent along the z-axis) - - Returns: - - A tuple of (mass, inertia) with inertia specified around the origin - """ - - m = density * wp.pi * r * r * h - - Ia = 1 / 12 * m * (3 * r * r + h * h) - Ib = 1 / 2 * m * r * r - - # For Z-axis orientation: I_xx = I_yy = Ia, I_zz = Ib - I = wp.mat33([[Ia, 0.0, 0.0], [0.0, Ia, 0.0], [0.0, 0.0, Ib]]) - - return (m, wp.vec3(), I) - def compute_cone_inertia(density: float, r: float, h: float) -> tuple[float, wp.vec3, wp.mat33]: """Helper to compute mass and inertia of a solid cone extending along the z-axis @@ -286,27 +249,3 @@ def compute_ellipsoid_inertia(density: float, a: float, b: float, c: float) -> t return (m, wp.vec3(), I) */ - -/* -def compute_box_inertia(density: float, w: float, h: float, d: float) -> tuple[float, wp.vec3, wp.mat33]: - """Helper to compute mass and inertia of a solid box - - Args: - density: The box density - w: The box width along the x-axis - h: The box height along the y-axis - d: The box depth along the z-axis - - Returns: - - A tuple of (mass, inertia) with inertia specified around the origin - """ - - v = w * h * d - m = density * v - I = compute_box_inertia_from_mass(m, w, h, d) - - return (m, wp.vec3(), I) - -} -*/ diff --git a/physics/world/typegen.go b/physics/world/typegen.go deleted file mode 100644 index 6c91d27c..00000000 --- a/physics/world/typegen.go +++ /dev/null @@ -1,48 +0,0 @@ -// Code generated by "core generate -add-types"; DO NOT EDIT. - -package world - -import ( - "cogentcore.org/core/tree" - "cogentcore.org/core/types" - "cogentcore.org/lab/physics" -) - -var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics/world.Camera", IDName: "camera", Doc: "Camera defines the properties of a camera needed for rendering from a node.", Fields: []types.Field{{Name: "Size", Doc: "size of image to record"}, {Name: "FOV", Doc: "field of view in degrees"}, {Name: "Near", Doc: "near plane z coordinate"}, {Name: "Far", Doc: "far plane z coordinate"}, {Name: "MaxD", Doc: "maximum distance for depth maps. Anything above is 1.\nThis is independent of Near / Far rendering (though must be < Far)\nand is for normalized depth maps."}, {Name: "LogD", Doc: "use the natural log of 1 + depth for normalized depth values in display etc."}, {Name: "MSample", Doc: "number of multi-samples to use for antialising -- 4 is best and default."}, {Name: "UpDir", Doc: "up direction for camera. Defaults to positive Y axis,\nand is reset by call to LookAt method."}}}) - -var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics/world.Editor", IDName: "editor", Doc: "Editor provides a basic viewer and parameter controller widget\nfor exploring physics models.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Embeds: []types.Field{{Name: "Frame"}}, Fields: []types.Field{{Name: "Physics", Doc: "Physics has the physics simulation."}, {Name: "World", Doc: "World has the 3D GUI visualization."}, {Name: "UserParams", Doc: "UserParams is a struct with parameters for configuring the physics sim.\nThese are displayed in the editor."}, {Name: "ConfigFunc", Doc: "ConfigFunc is the function that configures the world."}, {Name: "ControlFunc", Doc: "ControlFunc is the function that sets control parameters,\nbased on the current timestep (in milliseconds, converted from physics time)."}, {Name: "isRunning", Doc: "IsRunning is true if currently running sim."}, {Name: "stop", Doc: "Stop triggers topping of running."}, {Name: "TimeStep", Doc: "TimeStep is current time step in physics update cycles."}, {Name: "scene", Doc: "Scene is the xyz GUI visualization widget."}, {Name: "toolbar", Doc: "Toolbar is the top toolbar."}, {Name: "splits", Doc: "Splits is the container for elements."}, {Name: "userParamsForm", Doc: "UserParamsForm has the user's config parameters."}, {Name: "paramsForm", Doc: "ParamsForm has the Physics parameters."}}}) - -// NewEditor returns a new [Editor] with the given optional parent: -// Editor provides a basic viewer and parameter controller widget -// for exploring physics models. -func NewEditor(parent ...tree.Node) *Editor { return tree.New[Editor](parent...) } - -// SetPhysics sets the [Editor.Physics]: -// Physics has the physics simulation. -func (t *Editor) SetPhysics(v *physics.World) *Editor { t.Physics = v; return t } - -// SetWorld sets the [Editor.World]: -// World has the 3D GUI visualization. -func (t *Editor) SetWorld(v *World) *Editor { t.World = v; return t } - -// SetUserParams sets the [Editor.UserParams]: -// UserParams is a struct with parameters for configuring the physics sim. -// These are displayed in the editor. -func (t *Editor) SetUserParams(v any) *Editor { t.UserParams = v; return t } - -// SetConfigFunc sets the [Editor.ConfigFunc]: -// ConfigFunc is the function that configures the world. -func (t *Editor) SetConfigFunc(v func()) *Editor { t.ConfigFunc = v; return t } - -// SetControlFunc sets the [Editor.ControlFunc]: -// ControlFunc is the function that sets control parameters, -// based on the current timestep (in milliseconds, converted from physics time). -func (t *Editor) SetControlFunc(v func(timeStep int)) *Editor { t.ControlFunc = v; return t } - -// SetTimeStep sets the [Editor.TimeStep]: -// TimeStep is current time step in physics update cycles. -func (t *Editor) SetTimeStep(v int) *Editor { t.TimeStep = v; return t } - -var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics/world.View", IDName: "view", Doc: "View has visualization functions for physics elements.", Fields: []types.Field{{Name: "Name", Doc: "Name is a name for element (index always appended)."}, {Name: "Shape", Doc: "Shape is the physical shape of the element."}, {Name: "Color", Doc: "Color is the color of the element."}, {Name: "Size", Doc: "Size is the size (per shape)."}, {Name: "Pos", Doc: "Pos is the position."}, {Name: "Quat", Doc: "Quat is the rotation as a quaternion."}, {Name: "NewView", Doc: "NewView is a function that returns a new [xyz.Node]\nto represent this element. If nil, uses appropriate defaults."}, {Name: "InitView", Doc: "InitView is a function that initializes a new [xyz.Node]\nthat represents this element. If nil, uses appropriate defaults."}, {Name: "Index", Doc: "Index is the index of the element in a list."}, {Name: "DynamicIndex", Doc: "DynamicIndex is the index of a dynamic element (-1 if not dynamic)."}}}) - -var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics/world.World", IDName: "world", Doc: "World displays a [physics.World] using a [xyz.Scene].\nOne World can be used for multiple different [physics.World]s which\nis more efficient when running multiple in parallel.\nInitial construction of the physics and visualization happens here.", Fields: []types.Field{{Name: "Scene", Doc: "Scene is the [xyz.Scene] object for visualizing."}, {Name: "Root", Doc: "Root is the root Group node in the Scene under which the world is rendered."}, {Name: "Views", Doc: "Views are the view elements for each body in [physics.World]."}}}) diff --git a/yaegilab/labsymbols/cogentcore_org-lab-physics-phyxyz.go b/yaegilab/labsymbols/cogentcore_org-lab-physics-phyxyz.go new file mode 100644 index 00000000..ae7df1e4 --- /dev/null +++ b/yaegilab/labsymbols/cogentcore_org-lab-physics-phyxyz.go @@ -0,0 +1,26 @@ +// Code generated by 'yaegi extract cogentcore.org/lab/physics/phyxyz'. DO NOT EDIT. + +package labsymbols + +import ( + "cogentcore.org/lab/physics/phyxyz" + "reflect" +) + +func init() { + Symbols["cogentcore.org/lab/physics/phyxyz/phyxyz"] = map[string]reflect.Value{ + // function, constant and variable definitions + "DepthImage": reflect.ValueOf(phyxyz.DepthImage), + "DepthNorm": reflect.ValueOf(phyxyz.DepthNorm), + "MakeStateToolbar": reflect.ValueOf(phyxyz.MakeStateToolbar), + "NewEditor": reflect.ValueOf(phyxyz.NewEditor), + "NewWorld": reflect.ValueOf(phyxyz.NewWorld), + "NoDisplayScene": reflect.ValueOf(phyxyz.NoDisplayScene), + + // type definitions + "Camera": reflect.ValueOf((*phyxyz.Camera)(nil)), + "Editor": reflect.ValueOf((*phyxyz.Editor)(nil)), + "View": reflect.ValueOf((*phyxyz.View)(nil)), + "World": reflect.ValueOf((*phyxyz.World)(nil)), + } +} diff --git a/yaegilab/labsymbols/cogentcore_org-lab-physics.go b/yaegilab/labsymbols/cogentcore_org-lab-physics.go new file mode 100644 index 00000000..339782b4 --- /dev/null +++ b/yaegilab/labsymbols/cogentcore_org-lab-physics.go @@ -0,0 +1,504 @@ +// Code generated by 'yaegi extract cogentcore.org/lab/physics'. DO NOT EDIT. + +package labsymbols + +import ( + "cogentcore.org/lab/physics" + "go/constant" + "go/token" + "reflect" +) + +func init() { + Symbols["cogentcore.org/lab/physics/physics"] = map[string]reflect.Value{ + // function, constant and variable definitions + "AddBroadContacts": reflect.ValueOf(physics.AddBroadContacts), + "AngMotionMax": reflect.ValueOf(constant.MakeFromLiteral("0.78539816339744830961566084581987572104929234984377645524373614869749573092390898867610892833397416837141584743218870486667557894677841571570962890997663229058521681784366119671858541551046073436737060546875", token.FLOAT, 0)), + "AngularCorrection": reflect.ValueOf(physics.AngularCorrection), + "Ball": reflect.ValueOf(physics.Ball), + "Bodies": reflect.ValueOf(&physics.Bodies).Elem(), + "BodiesVar": reflect.ValueOf(physics.BodiesVar), + "BodyBounce": reflect.ValueOf(physics.BodyBounce), + "BodyCollidePairs": reflect.ValueOf(&physics.BodyCollidePairs).Elem(), + "BodyCollidePairsVar": reflect.ValueOf(physics.BodyCollidePairsVar), + "BodyCom": reflect.ValueOf(physics.BodyCom), + "BodyComX": reflect.ValueOf(physics.BodyComX), + "BodyComY": reflect.ValueOf(physics.BodyComY), + "BodyComZ": reflect.ValueOf(physics.BodyComZ), + "BodyDynamic": reflect.ValueOf(physics.BodyDynamic), + "BodyDynamicPos": reflect.ValueOf(physics.BodyDynamicPos), + "BodyDynamicQuat": reflect.ValueOf(physics.BodyDynamicQuat), + "BodyFriction": reflect.ValueOf(physics.BodyFriction), + "BodyFrictionRolling": reflect.ValueOf(physics.BodyFrictionRolling), + "BodyFrictionTortion": reflect.ValueOf(physics.BodyFrictionTortion), + "BodyGroup": reflect.ValueOf(physics.BodyGroup), + "BodyHSize": reflect.ValueOf(physics.BodyHSize), + "BodyHSizeX": reflect.ValueOf(physics.BodyHSizeX), + "BodyHSizeY": reflect.ValueOf(physics.BodyHSizeY), + "BodyHSizeZ": reflect.ValueOf(physics.BodyHSizeZ), + "BodyInertia": reflect.ValueOf(physics.BodyInertia), + "BodyInertiaXX": reflect.ValueOf(physics.BodyInertiaXX), + "BodyInertiaXY": reflect.ValueOf(physics.BodyInertiaXY), + "BodyInertiaXZ": reflect.ValueOf(physics.BodyInertiaXZ), + "BodyInertiaYX": reflect.ValueOf(physics.BodyInertiaYX), + "BodyInertiaYY": reflect.ValueOf(physics.BodyInertiaYY), + "BodyInertiaYZ": reflect.ValueOf(physics.BodyInertiaYZ), + "BodyInertiaZX": reflect.ValueOf(physics.BodyInertiaZX), + "BodyInertiaZY": reflect.ValueOf(physics.BodyInertiaZY), + "BodyInertiaZZ": reflect.ValueOf(physics.BodyInertiaZZ), + "BodyInvInertia": reflect.ValueOf(physics.BodyInvInertia), + "BodyInvInertiaXX": reflect.ValueOf(physics.BodyInvInertiaXX), + "BodyInvInertiaXY": reflect.ValueOf(physics.BodyInvInertiaXY), + "BodyInvInertiaXZ": reflect.ValueOf(physics.BodyInvInertiaXZ), + "BodyInvInertiaYX": reflect.ValueOf(physics.BodyInvInertiaYX), + "BodyInvInertiaYY": reflect.ValueOf(physics.BodyInvInertiaYY), + "BodyInvInertiaYZ": reflect.ValueOf(physics.BodyInvInertiaYZ), + "BodyInvInertiaZX": reflect.ValueOf(physics.BodyInvInertiaZX), + "BodyInvInertiaZY": reflect.ValueOf(physics.BodyInvInertiaZY), + "BodyInvInertiaZZ": reflect.ValueOf(physics.BodyInvInertiaZZ), + "BodyInvMass": reflect.ValueOf(physics.BodyInvMass), + "BodyJoints": reflect.ValueOf(&physics.BodyJoints).Elem(), + "BodyJointsVar": reflect.ValueOf(physics.BodyJointsVar), + "BodyMass": reflect.ValueOf(physics.BodyMass), + "BodyPos": reflect.ValueOf(physics.BodyPos), + "BodyPosX": reflect.ValueOf(physics.BodyPosX), + "BodyPosY": reflect.ValueOf(physics.BodyPosY), + "BodyPosZ": reflect.ValueOf(physics.BodyPosZ), + "BodyQuat": reflect.ValueOf(physics.BodyQuat), + "BodyQuatW": reflect.ValueOf(physics.BodyQuatW), + "BodyQuatX": reflect.ValueOf(physics.BodyQuatX), + "BodyQuatY": reflect.ValueOf(physics.BodyQuatY), + "BodyQuatZ": reflect.ValueOf(physics.BodyQuatZ), + "BodyRadius": reflect.ValueOf(physics.BodyRadius), + "BodyShape": reflect.ValueOf(physics.BodyShape), + "BodyThick": reflect.ValueOf(physics.BodyThick), + "BodyVarsN": reflect.ValueOf(physics.BodyVarsN), + "BodyVarsValues": reflect.ValueOf(physics.BodyVarsValues), + "BodyWorld": reflect.ValueOf(physics.BodyWorld), + "BorrowedGPU": reflect.ValueOf(&physics.BorrowedGPU).Elem(), + "Box": reflect.ValueOf(physics.Box), + "BoxEdge": reflect.ValueOf(physics.BoxEdge), + "BoxSDF": reflect.ValueOf(physics.BoxSDF), + "BoxSDFGrad": reflect.ValueOf(physics.BoxSDFGrad), + "BoxVertex": reflect.ValueOf(physics.BoxVertex), + "BroadContactVarsN": reflect.ValueOf(physics.BroadContactVarsN), + "BroadContacts": reflect.ValueOf(&physics.BroadContacts).Elem(), + "BroadContactsN": reflect.ValueOf(&physics.BroadContactsN).Elem(), + "BroadContactsNVar": reflect.ValueOf(physics.BroadContactsNVar), + "BroadContactsVar": reflect.ValueOf(physics.BroadContactsVar), + "Capsule": reflect.ValueOf(physics.Capsule), + "CapsuleSDF": reflect.ValueOf(physics.CapsuleSDF), + "ClosestEdgeBox": reflect.ValueOf(physics.ClosestEdgeBox), + "ClosestEdgeCapsule": reflect.ValueOf(physics.ClosestEdgeCapsule), + "ClosestEdgePlane": reflect.ValueOf(physics.ClosestEdgePlane), + "ClosestPointBox": reflect.ValueOf(physics.ClosestPointBox), + "ClosestPointLineSegment": reflect.ValueOf(physics.ClosestPointLineSegment), + "ClosestPointPlane": reflect.ValueOf(physics.ClosestPointPlane), + "ColBoxBox": reflect.ValueOf(physics.ColBoxBox), + "ColBoxCapsule": reflect.ValueOf(physics.ColBoxCapsule), + "ColBoxPlane": reflect.ValueOf(physics.ColBoxPlane), + "ColCapsuleCapsule": reflect.ValueOf(physics.ColCapsuleCapsule), + "ColCapsulePlane": reflect.ValueOf(physics.ColCapsulePlane), + "ColCylinderPlane": reflect.ValueOf(physics.ColCylinderPlane), + "ColSphereBox": reflect.ValueOf(physics.ColSphereBox), + "ColSphereCapsule": reflect.ValueOf(physics.ColSphereCapsule), + "ColSpherePlane": reflect.ValueOf(physics.ColSpherePlane), + "ColSphereSphere": reflect.ValueOf(physics.ColSphereSphere), + "CollisionBroad": reflect.ValueOf(physics.CollisionBroad), + "CollisionNarrow": reflect.ValueOf(physics.CollisionNarrow), + "ComputeGPU": reflect.ValueOf(&physics.ComputeGPU).Elem(), + "Cone": reflect.ValueOf(physics.Cone), + "ConeSDF": reflect.ValueOf(physics.ConeSDF), + "ContactA": reflect.ValueOf(physics.ContactA), + "ContactAAngDelta": reflect.ValueOf(physics.ContactAAngDelta), + "ContactAAngDeltaX": reflect.ValueOf(physics.ContactAAngDeltaX), + "ContactAAngDeltaY": reflect.ValueOf(physics.ContactAAngDeltaY), + "ContactAAngDeltaZ": reflect.ValueOf(physics.ContactAAngDeltaZ), + "ContactADelta": reflect.ValueOf(physics.ContactADelta), + "ContactADeltaX": reflect.ValueOf(physics.ContactADeltaX), + "ContactADeltaY": reflect.ValueOf(physics.ContactADeltaY), + "ContactADeltaZ": reflect.ValueOf(physics.ContactADeltaZ), + "ContactAOff": reflect.ValueOf(physics.ContactAOff), + "ContactAOffX": reflect.ValueOf(physics.ContactAOffX), + "ContactAOffY": reflect.ValueOf(physics.ContactAOffY), + "ContactAOffZ": reflect.ValueOf(physics.ContactAOffZ), + "ContactAPoint": reflect.ValueOf(physics.ContactAPoint), + "ContactAPointX": reflect.ValueOf(physics.ContactAPointX), + "ContactAPointY": reflect.ValueOf(physics.ContactAPointY), + "ContactAPointZ": reflect.ValueOf(physics.ContactAPointZ), + "ContactAThick": reflect.ValueOf(physics.ContactAThick), + "ContactB": reflect.ValueOf(physics.ContactB), + "ContactBAngDelta": reflect.ValueOf(physics.ContactBAngDelta), + "ContactBAngDeltaX": reflect.ValueOf(physics.ContactBAngDeltaX), + "ContactBAngDeltaY": reflect.ValueOf(physics.ContactBAngDeltaY), + "ContactBAngDeltaZ": reflect.ValueOf(physics.ContactBAngDeltaZ), + "ContactBDelta": reflect.ValueOf(physics.ContactBDelta), + "ContactBDeltaX": reflect.ValueOf(physics.ContactBDeltaX), + "ContactBDeltaY": reflect.ValueOf(physics.ContactBDeltaY), + "ContactBDeltaZ": reflect.ValueOf(physics.ContactBDeltaZ), + "ContactBOff": reflect.ValueOf(physics.ContactBOff), + "ContactBOffX": reflect.ValueOf(physics.ContactBOffX), + "ContactBOffY": reflect.ValueOf(physics.ContactBOffY), + "ContactBOffZ": reflect.ValueOf(physics.ContactBOffZ), + "ContactBPoint": reflect.ValueOf(physics.ContactBPoint), + "ContactBPointX": reflect.ValueOf(physics.ContactBPointX), + "ContactBPointY": reflect.ValueOf(physics.ContactBPointY), + "ContactBPointZ": reflect.ValueOf(physics.ContactBPointZ), + "ContactBThick": reflect.ValueOf(physics.ContactBThick), + "ContactConstraint": reflect.ValueOf(physics.ContactConstraint), + "ContactNorm": reflect.ValueOf(physics.ContactNorm), + "ContactNormX": reflect.ValueOf(physics.ContactNormX), + "ContactNormY": reflect.ValueOf(physics.ContactNormY), + "ContactNormZ": reflect.ValueOf(physics.ContactNormZ), + "ContactPointIdx": reflect.ValueOf(physics.ContactPointIdx), + "ContactPoints": reflect.ValueOf(physics.ContactPoints), + "ContactVarsN": reflect.ValueOf(physics.ContactVarsN), + "ContactVarsValues": reflect.ValueOf(physics.ContactVarsValues), + "ContactWeight": reflect.ValueOf(physics.ContactWeight), + "Contacts": reflect.ValueOf(&physics.Contacts).Elem(), + "ContactsN": reflect.ValueOf(&physics.ContactsN).Elem(), + "ContactsNVar": reflect.ValueOf(physics.ContactsNVar), + "ContactsVar": reflect.ValueOf(physics.ContactsVar), + "Cylinder": reflect.ValueOf(physics.Cylinder), + "CylinderSDF": reflect.ValueOf(physics.CylinderSDF), + "D6": reflect.ValueOf(physics.D6), + "Distance": reflect.ValueOf(physics.Distance), + "DynAccX": reflect.ValueOf(physics.DynAccX), + "DynAccY": reflect.ValueOf(physics.DynAccY), + "DynAccZ": reflect.ValueOf(physics.DynAccZ), + "DynAngAccX": reflect.ValueOf(physics.DynAngAccX), + "DynAngAccY": reflect.ValueOf(physics.DynAngAccY), + "DynAngAccZ": reflect.ValueOf(physics.DynAngAccZ), + "DynAngDeltaX": reflect.ValueOf(physics.DynAngDeltaX), + "DynAngDeltaY": reflect.ValueOf(physics.DynAngDeltaY), + "DynAngDeltaZ": reflect.ValueOf(physics.DynAngDeltaZ), + "DynAngVelX": reflect.ValueOf(physics.DynAngVelX), + "DynAngVelY": reflect.ValueOf(physics.DynAngVelY), + "DynAngVelZ": reflect.ValueOf(physics.DynAngVelZ), + "DynBody": reflect.ValueOf(physics.DynBody), + "DynContactWeight": reflect.ValueOf(physics.DynContactWeight), + "DynDeltaX": reflect.ValueOf(physics.DynDeltaX), + "DynDeltaY": reflect.ValueOf(physics.DynDeltaY), + "DynDeltaZ": reflect.ValueOf(physics.DynDeltaZ), + "DynForceX": reflect.ValueOf(physics.DynForceX), + "DynForceY": reflect.ValueOf(physics.DynForceY), + "DynForceZ": reflect.ValueOf(physics.DynForceZ), + "DynPosX": reflect.ValueOf(physics.DynPosX), + "DynPosY": reflect.ValueOf(physics.DynPosY), + "DynPosZ": reflect.ValueOf(physics.DynPosZ), + "DynQuatW": reflect.ValueOf(physics.DynQuatW), + "DynQuatX": reflect.ValueOf(physics.DynQuatX), + "DynQuatY": reflect.ValueOf(physics.DynQuatY), + "DynQuatZ": reflect.ValueOf(physics.DynQuatZ), + "DynTorqueX": reflect.ValueOf(physics.DynTorqueX), + "DynTorqueY": reflect.ValueOf(physics.DynTorqueY), + "DynTorqueZ": reflect.ValueOf(physics.DynTorqueZ), + "DynVelX": reflect.ValueOf(physics.DynVelX), + "DynVelY": reflect.ValueOf(physics.DynVelY), + "DynVelZ": reflect.ValueOf(physics.DynVelZ), + "DynamicAcc": reflect.ValueOf(physics.DynamicAcc), + "DynamicAngAcc": reflect.ValueOf(physics.DynamicAngAcc), + "DynamicAngDelta": reflect.ValueOf(physics.DynamicAngDelta), + "DynamicAngVel": reflect.ValueOf(physics.DynamicAngVel), + "DynamicBody": reflect.ValueOf(physics.DynamicBody), + "DynamicDelta": reflect.ValueOf(physics.DynamicDelta), + "DynamicForce": reflect.ValueOf(physics.DynamicForce), + "DynamicPos": reflect.ValueOf(physics.DynamicPos), + "DynamicQuat": reflect.ValueOf(physics.DynamicQuat), + "DynamicTorque": reflect.ValueOf(physics.DynamicTorque), + "DynamicVarsN": reflect.ValueOf(physics.DynamicVarsN), + "DynamicVarsValues": reflect.ValueOf(physics.DynamicVarsValues), + "DynamicVel": reflect.ValueOf(physics.DynamicVel), + "Dynamics": reflect.ValueOf(&physics.Dynamics).Elem(), + "DynamicsCurToNext": reflect.ValueOf(physics.DynamicsCurToNext), + "DynamicsVar": reflect.ValueOf(physics.DynamicsVar), + "Fixed": reflect.ValueOf(physics.Fixed), + "ForcesFromJoints": reflect.ValueOf(physics.ForcesFromJoints), + "Free": reflect.ValueOf(physics.Free), + "GPUInit": reflect.ValueOf(physics.GPUInit), + "GPUInitialized": reflect.ValueOf(&physics.GPUInitialized).Elem(), + "GPURelease": reflect.ValueOf(physics.GPURelease), + "GPUSystem": reflect.ValueOf(&physics.GPUSystem).Elem(), + "GPUVarsN": reflect.ValueOf(physics.GPUVarsN), + "GPUVarsValues": reflect.ValueOf(physics.GPUVarsValues), + "GetBodyDynamic": reflect.ValueOf(physics.GetBodyDynamic), + "GetBodyGroup": reflect.ValueOf(physics.GetBodyGroup), + "GetBodyShape": reflect.ValueOf(physics.GetBodyShape), + "GetBodyWorld": reflect.ValueOf(physics.GetBodyWorld), + "GetBroadContactA": reflect.ValueOf(physics.GetBroadContactA), + "GetBroadContactB": reflect.ValueOf(physics.GetBroadContactB), + "GetBroadContactPointIdx": reflect.ValueOf(physics.GetBroadContactPointIdx), + "GetContactA": reflect.ValueOf(physics.GetContactA), + "GetContactB": reflect.ValueOf(physics.GetContactB), + "GetContactPointIdx": reflect.ValueOf(physics.GetContactPointIdx), + "GetJointAngularDoFN": reflect.ValueOf(physics.GetJointAngularDoFN), + "GetJointEnabled": reflect.ValueOf(physics.GetJointEnabled), + "GetJointLinearDoFN": reflect.ValueOf(physics.GetJointLinearDoFN), + "GetJointType": reflect.ValueOf(physics.GetJointType), + "GetParams": reflect.ValueOf(physics.GetParams), + "GroupsCollide": reflect.ValueOf(physics.GroupsCollide), + "InitDynamics": reflect.ValueOf(physics.InitDynamics), + "InitGeomData": reflect.ValueOf(physics.InitGeomData), + "JointAngularDoFN": reflect.ValueOf(physics.JointAngularDoFN), + "JointAxis": reflect.ValueOf(physics.JointAxis), + "JointAxisDoF": reflect.ValueOf(physics.JointAxisDoF), + "JointAxisLimitsUpdate": reflect.ValueOf(physics.JointAxisLimitsUpdate), + "JointAxisTarget": reflect.ValueOf(physics.JointAxisTarget), + "JointAxisX": reflect.ValueOf(physics.JointAxisX), + "JointAxisY": reflect.ValueOf(physics.JointAxisY), + "JointAxisZ": reflect.ValueOf(physics.JointAxisZ), + "JointCAngDelta": reflect.ValueOf(physics.JointCAngDelta), + "JointCAngDeltaX": reflect.ValueOf(physics.JointCAngDeltaX), + "JointCAngDeltaY": reflect.ValueOf(physics.JointCAngDeltaY), + "JointCAngDeltaZ": reflect.ValueOf(physics.JointCAngDeltaZ), + "JointCDelta": reflect.ValueOf(physics.JointCDelta), + "JointCDeltaX": reflect.ValueOf(physics.JointCDeltaX), + "JointCDeltaY": reflect.ValueOf(physics.JointCDeltaY), + "JointCDeltaZ": reflect.ValueOf(physics.JointCDeltaZ), + "JointCForce": reflect.ValueOf(physics.JointCForce), + "JointCForceX": reflect.ValueOf(physics.JointCForceX), + "JointCForceY": reflect.ValueOf(physics.JointCForceY), + "JointCForceZ": reflect.ValueOf(physics.JointCForceZ), + "JointCPos": reflect.ValueOf(physics.JointCPos), + "JointCPosX": reflect.ValueOf(physics.JointCPosX), + "JointCPosY": reflect.ValueOf(physics.JointCPosY), + "JointCPosZ": reflect.ValueOf(physics.JointCPosZ), + "JointCQuat": reflect.ValueOf(physics.JointCQuat), + "JointCQuatW": reflect.ValueOf(physics.JointCQuatW), + "JointCQuatX": reflect.ValueOf(physics.JointCQuatX), + "JointCQuatY": reflect.ValueOf(physics.JointCQuatY), + "JointCQuatZ": reflect.ValueOf(physics.JointCQuatZ), + "JointCTorque": reflect.ValueOf(physics.JointCTorque), + "JointCTorqueX": reflect.ValueOf(physics.JointCTorqueX), + "JointCTorqueY": reflect.ValueOf(physics.JointCTorqueY), + "JointCTorqueZ": reflect.ValueOf(physics.JointCTorqueZ), + "JointChild": reflect.ValueOf(physics.JointChild), + "JointChildIndex": reflect.ValueOf(physics.JointChildIndex), + "JointControl": reflect.ValueOf(physics.JointControl), + "JointControlForce": reflect.ValueOf(physics.JointControlForce), + "JointControlVarsN": reflect.ValueOf(physics.JointControlVarsN), + "JointControlVarsValues": reflect.ValueOf(physics.JointControlVarsValues), + "JointControls": reflect.ValueOf(&physics.JointControls).Elem(), + "JointControlsVar": reflect.ValueOf(physics.JointControlsVar), + "JointDoF": reflect.ValueOf(physics.JointDoF), + "JointDoF1": reflect.ValueOf(physics.JointDoF1), + "JointDoF2": reflect.ValueOf(physics.JointDoF2), + "JointDoF3": reflect.ValueOf(physics.JointDoF3), + "JointDoF4": reflect.ValueOf(physics.JointDoF4), + "JointDoF5": reflect.ValueOf(physics.JointDoF5), + "JointDoF6": reflect.ValueOf(physics.JointDoF6), + "JointDoFIndex": reflect.ValueOf(physics.JointDoFIndex), + "JointDoFVarsN": reflect.ValueOf(physics.JointDoFVarsN), + "JointDoFVarsValues": reflect.ValueOf(physics.JointDoFVarsValues), + "JointDoFs": reflect.ValueOf(&physics.JointDoFs).Elem(), + "JointDoFsVar": reflect.ValueOf(physics.JointDoFsVar), + "JointEnabled": reflect.ValueOf(physics.JointEnabled), + "JointLimitLower": reflect.ValueOf(physics.JointLimitLower), + "JointLimitUnlimited": reflect.ValueOf(constant.MakeFromLiteral("10000000000", token.FLOAT, 0)), + "JointLimitUpper": reflect.ValueOf(physics.JointLimitUpper), + "JointLinearDoFN": reflect.ValueOf(physics.JointLinearDoFN), + "JointPAngDelta": reflect.ValueOf(physics.JointPAngDelta), + "JointPAngDeltaX": reflect.ValueOf(physics.JointPAngDeltaX), + "JointPAngDeltaY": reflect.ValueOf(physics.JointPAngDeltaY), + "JointPAngDeltaZ": reflect.ValueOf(physics.JointPAngDeltaZ), + "JointPDelta": reflect.ValueOf(physics.JointPDelta), + "JointPDeltaX": reflect.ValueOf(physics.JointPDeltaX), + "JointPDeltaY": reflect.ValueOf(physics.JointPDeltaY), + "JointPDeltaZ": reflect.ValueOf(physics.JointPDeltaZ), + "JointPForce": reflect.ValueOf(physics.JointPForce), + "JointPForceX": reflect.ValueOf(physics.JointPForceX), + "JointPForceY": reflect.ValueOf(physics.JointPForceY), + "JointPForceZ": reflect.ValueOf(physics.JointPForceZ), + "JointPPos": reflect.ValueOf(physics.JointPPos), + "JointPPosX": reflect.ValueOf(physics.JointPPosX), + "JointPPosY": reflect.ValueOf(physics.JointPPosY), + "JointPPosZ": reflect.ValueOf(physics.JointPPosZ), + "JointPQuat": reflect.ValueOf(physics.JointPQuat), + "JointPQuatW": reflect.ValueOf(physics.JointPQuatW), + "JointPQuatX": reflect.ValueOf(physics.JointPQuatX), + "JointPQuatY": reflect.ValueOf(physics.JointPQuatY), + "JointPQuatZ": reflect.ValueOf(physics.JointPQuatZ), + "JointPTorque": reflect.ValueOf(physics.JointPTorque), + "JointPTorqueX": reflect.ValueOf(physics.JointPTorqueX), + "JointPTorqueY": reflect.ValueOf(physics.JointPTorqueY), + "JointPTorqueZ": reflect.ValueOf(physics.JointPTorqueZ), + "JointParent": reflect.ValueOf(physics.JointParent), + "JointParentIndex": reflect.ValueOf(physics.JointParentIndex), + "JointTargetDamp": reflect.ValueOf(physics.JointTargetDamp), + "JointTargetPos": reflect.ValueOf(physics.JointTargetPos), + "JointTargetStiff": reflect.ValueOf(physics.JointTargetStiff), + "JointTargetVel": reflect.ValueOf(physics.JointTargetVel), + "JointType": reflect.ValueOf(physics.JointType), + "JointTypesN": reflect.ValueOf(physics.JointTypesN), + "JointTypesValues": reflect.ValueOf(physics.JointTypesValues), + "JointVarsN": reflect.ValueOf(physics.JointVarsN), + "JointVarsValues": reflect.ValueOf(physics.JointVarsValues), + "Joints": reflect.ValueOf(&physics.Joints).Elem(), + "JointsVar": reflect.ValueOf(physics.JointsVar), + "NewGeomData": reflect.ValueOf(physics.NewGeomData), + "NewWorld": reflect.ValueOf(physics.NewWorld), + "OneIfNonzero": reflect.ValueOf(physics.OneIfNonzero), + "Params": reflect.ValueOf(&physics.Params).Elem(), + "ParamsVar": reflect.ValueOf(physics.ParamsVar), + "Plane": reflect.ValueOf(physics.Plane), + "PlaneEdge": reflect.ValueOf(physics.PlaneEdge), + "PlaneSDF": reflect.ValueOf(physics.PlaneSDF), + "PositionalCorrection": reflect.ValueOf(physics.PositionalCorrection), + "Prismatic": reflect.ValueOf(physics.Prismatic), + "ReadFromGPU": reflect.ValueOf(physics.ReadFromGPU), + "Revolute": reflect.ValueOf(physics.Revolute), + "RunCollisionBroad": reflect.ValueOf(physics.RunCollisionBroad), + "RunCollisionBroadCPU": reflect.ValueOf(physics.RunCollisionBroadCPU), + "RunCollisionBroadGPU": reflect.ValueOf(physics.RunCollisionBroadGPU), + "RunCollisionNarrow": reflect.ValueOf(physics.RunCollisionNarrow), + "RunCollisionNarrowCPU": reflect.ValueOf(physics.RunCollisionNarrowCPU), + "RunCollisionNarrowGPU": reflect.ValueOf(physics.RunCollisionNarrowGPU), + "RunDone": reflect.ValueOf(physics.RunDone), + "RunDynamicsCurToNext": reflect.ValueOf(physics.RunDynamicsCurToNext), + "RunDynamicsCurToNextCPU": reflect.ValueOf(physics.RunDynamicsCurToNextCPU), + "RunDynamicsCurToNextGPU": reflect.ValueOf(physics.RunDynamicsCurToNextGPU), + "RunForcesFromJoints": reflect.ValueOf(physics.RunForcesFromJoints), + "RunForcesFromJointsCPU": reflect.ValueOf(physics.RunForcesFromJointsCPU), + "RunForcesFromJointsGPU": reflect.ValueOf(physics.RunForcesFromJointsGPU), + "RunGPUSync": reflect.ValueOf(physics.RunGPUSync), + "RunInitDynamics": reflect.ValueOf(physics.RunInitDynamics), + "RunInitDynamicsCPU": reflect.ValueOf(physics.RunInitDynamicsCPU), + "RunInitDynamicsGPU": reflect.ValueOf(physics.RunInitDynamicsGPU), + "RunOneCollisionBroad": reflect.ValueOf(physics.RunOneCollisionBroad), + "RunOneCollisionNarrow": reflect.ValueOf(physics.RunOneCollisionNarrow), + "RunOneDynamicsCurToNext": reflect.ValueOf(physics.RunOneDynamicsCurToNext), + "RunOneForcesFromJoints": reflect.ValueOf(physics.RunOneForcesFromJoints), + "RunOneInitDynamics": reflect.ValueOf(physics.RunOneInitDynamics), + "RunOneStepBodyContactDeltas": reflect.ValueOf(physics.RunOneStepBodyContactDeltas), + "RunOneStepBodyContacts": reflect.ValueOf(physics.RunOneStepBodyContacts), + "RunOneStepBodyJointDeltas": reflect.ValueOf(physics.RunOneStepBodyJointDeltas), + "RunOneStepInit": reflect.ValueOf(physics.RunOneStepInit), + "RunOneStepIntegrateBodies": reflect.ValueOf(physics.RunOneStepIntegrateBodies), + "RunOneStepJointForces": reflect.ValueOf(physics.RunOneStepJointForces), + "RunOneStepSolveJoints": reflect.ValueOf(physics.RunOneStepSolveJoints), + "RunStepBodyContactDeltas": reflect.ValueOf(physics.RunStepBodyContactDeltas), + "RunStepBodyContactDeltasCPU": reflect.ValueOf(physics.RunStepBodyContactDeltasCPU), + "RunStepBodyContactDeltasGPU": reflect.ValueOf(physics.RunStepBodyContactDeltasGPU), + "RunStepBodyContacts": reflect.ValueOf(physics.RunStepBodyContacts), + "RunStepBodyContactsCPU": reflect.ValueOf(physics.RunStepBodyContactsCPU), + "RunStepBodyContactsGPU": reflect.ValueOf(physics.RunStepBodyContactsGPU), + "RunStepBodyJointDeltas": reflect.ValueOf(physics.RunStepBodyJointDeltas), + "RunStepBodyJointDeltasCPU": reflect.ValueOf(physics.RunStepBodyJointDeltasCPU), + "RunStepBodyJointDeltasGPU": reflect.ValueOf(physics.RunStepBodyJointDeltasGPU), + "RunStepInit": reflect.ValueOf(physics.RunStepInit), + "RunStepInitCPU": reflect.ValueOf(physics.RunStepInitCPU), + "RunStepInitGPU": reflect.ValueOf(physics.RunStepInitGPU), + "RunStepIntegrateBodies": reflect.ValueOf(physics.RunStepIntegrateBodies), + "RunStepIntegrateBodiesCPU": reflect.ValueOf(physics.RunStepIntegrateBodiesCPU), + "RunStepIntegrateBodiesGPU": reflect.ValueOf(physics.RunStepIntegrateBodiesGPU), + "RunStepJointForces": reflect.ValueOf(physics.RunStepJointForces), + "RunStepJointForcesCPU": reflect.ValueOf(physics.RunStepJointForcesCPU), + "RunStepJointForcesGPU": reflect.ValueOf(physics.RunStepJointForcesGPU), + "RunStepSolveJoints": reflect.ValueOf(physics.RunStepSolveJoints), + "RunStepSolveJointsCPU": reflect.ValueOf(physics.RunStepSolveJointsCPU), + "RunStepSolveJointsGPU": reflect.ValueOf(physics.RunStepSolveJointsGPU), + "SetBodyBounce": reflect.ValueOf(physics.SetBodyBounce), + "SetBodyCom": reflect.ValueOf(physics.SetBodyCom), + "SetBodyDynamic": reflect.ValueOf(physics.SetBodyDynamic), + "SetBodyFriction": reflect.ValueOf(physics.SetBodyFriction), + "SetBodyFrictionRolling": reflect.ValueOf(physics.SetBodyFrictionRolling), + "SetBodyFrictionTortion": reflect.ValueOf(physics.SetBodyFrictionTortion), + "SetBodyGroup": reflect.ValueOf(physics.SetBodyGroup), + "SetBodyHSize": reflect.ValueOf(physics.SetBodyHSize), + "SetBodyInertia": reflect.ValueOf(physics.SetBodyInertia), + "SetBodyInvInertia": reflect.ValueOf(physics.SetBodyInvInertia), + "SetBodyPos": reflect.ValueOf(physics.SetBodyPos), + "SetBodyQuat": reflect.ValueOf(physics.SetBodyQuat), + "SetBodyShape": reflect.ValueOf(physics.SetBodyShape), + "SetBodyWorld": reflect.ValueOf(physics.SetBodyWorld), + "SetBroadContactA": reflect.ValueOf(physics.SetBroadContactA), + "SetBroadContactB": reflect.ValueOf(physics.SetBroadContactB), + "SetBroadContactPointIdx": reflect.ValueOf(physics.SetBroadContactPointIdx), + "SetContactA": reflect.ValueOf(physics.SetContactA), + "SetContactAAngDelta": reflect.ValueOf(physics.SetContactAAngDelta), + "SetContactADelta": reflect.ValueOf(physics.SetContactADelta), + "SetContactAOff": reflect.ValueOf(physics.SetContactAOff), + "SetContactAPoint": reflect.ValueOf(physics.SetContactAPoint), + "SetContactB": reflect.ValueOf(physics.SetContactB), + "SetContactBAngDelta": reflect.ValueOf(physics.SetContactBAngDelta), + "SetContactBDelta": reflect.ValueOf(physics.SetContactBDelta), + "SetContactBOff": reflect.ValueOf(physics.SetContactBOff), + "SetContactBPoint": reflect.ValueOf(physics.SetContactBPoint), + "SetContactNorm": reflect.ValueOf(physics.SetContactNorm), + "SetContactPointIdx": reflect.ValueOf(physics.SetContactPointIdx), + "SetDynamicAcc": reflect.ValueOf(physics.SetDynamicAcc), + "SetDynamicAngAcc": reflect.ValueOf(physics.SetDynamicAngAcc), + "SetDynamicAngDelta": reflect.ValueOf(physics.SetDynamicAngDelta), + "SetDynamicAngVel": reflect.ValueOf(physics.SetDynamicAngVel), + "SetDynamicBody": reflect.ValueOf(physics.SetDynamicBody), + "SetDynamicDelta": reflect.ValueOf(physics.SetDynamicDelta), + "SetDynamicForce": reflect.ValueOf(physics.SetDynamicForce), + "SetDynamicPos": reflect.ValueOf(physics.SetDynamicPos), + "SetDynamicQuat": reflect.ValueOf(physics.SetDynamicQuat), + "SetDynamicTorque": reflect.ValueOf(physics.SetDynamicTorque), + "SetDynamicVel": reflect.ValueOf(physics.SetDynamicVel), + "SetJointAngularDoFN": reflect.ValueOf(physics.SetJointAngularDoFN), + "SetJointAxis": reflect.ValueOf(physics.SetJointAxis), + "SetJointAxisDoF": reflect.ValueOf(physics.SetJointAxisDoF), + "SetJointCAngDelta": reflect.ValueOf(physics.SetJointCAngDelta), + "SetJointCDelta": reflect.ValueOf(physics.SetJointCDelta), + "SetJointCForce": reflect.ValueOf(physics.SetJointCForce), + "SetJointCPos": reflect.ValueOf(physics.SetJointCPos), + "SetJointCQuat": reflect.ValueOf(physics.SetJointCQuat), + "SetJointCTorque": reflect.ValueOf(physics.SetJointCTorque), + "SetJointChild": reflect.ValueOf(physics.SetJointChild), + "SetJointControl": reflect.ValueOf(physics.SetJointControl), + "SetJointControlForce": reflect.ValueOf(physics.SetJointControlForce), + "SetJointDoF": reflect.ValueOf(physics.SetJointDoF), + "SetJointDoFIndex": reflect.ValueOf(physics.SetJointDoFIndex), + "SetJointEnabled": reflect.ValueOf(physics.SetJointEnabled), + "SetJointLinearDoFN": reflect.ValueOf(physics.SetJointLinearDoFN), + "SetJointPAngDelta": reflect.ValueOf(physics.SetJointPAngDelta), + "SetJointPDelta": reflect.ValueOf(physics.SetJointPDelta), + "SetJointPForce": reflect.ValueOf(physics.SetJointPForce), + "SetJointPPos": reflect.ValueOf(physics.SetJointPPos), + "SetJointPQuat": reflect.ValueOf(physics.SetJointPQuat), + "SetJointPTorque": reflect.ValueOf(physics.SetJointPTorque), + "SetJointParent": reflect.ValueOf(physics.SetJointParent), + "SetJointTargetPos": reflect.ValueOf(physics.SetJointTargetPos), + "SetJointTargetVel": reflect.ValueOf(physics.SetJointTargetVel), + "SetJointType": reflect.ValueOf(physics.SetJointType), + "ShapePairContacts": reflect.ValueOf(physics.ShapePairContacts), + "ShapesN": reflect.ValueOf(physics.ShapesN), + "ShapesValues": reflect.ValueOf(physics.ShapesValues), + "Sphere": reflect.ValueOf(physics.Sphere), + "SphereSDF": reflect.ValueOf(physics.SphereSDF), + "StepBodyContactDeltas": reflect.ValueOf(physics.StepBodyContactDeltas), + "StepBodyContacts": reflect.ValueOf(physics.StepBodyContacts), + "StepBodyDeltas": reflect.ValueOf(physics.StepBodyDeltas), + "StepBodyJointDeltas": reflect.ValueOf(physics.StepBodyJointDeltas), + "StepInit": reflect.ValueOf(physics.StepInit), + "StepIntegrateBodies": reflect.ValueOf(physics.StepIntegrateBodies), + "StepJointForces": reflect.ValueOf(physics.StepJointForces), + "StepSolveJoints": reflect.ValueOf(physics.StepSolveJoints), + "StepsToMsec": reflect.ValueOf(physics.StepsToMsec), + "SyncFromGPU": reflect.ValueOf(physics.SyncFromGPU), + "TensorStrides": reflect.ValueOf(&physics.TensorStrides).Elem(), + "ToGPU": reflect.ValueOf(physics.ToGPU), + "ToGPUTensorStrides": reflect.ValueOf(physics.ToGPUTensorStrides), + "UseGPU": reflect.ValueOf(&physics.UseGPU).Elem(), + "VelocityAtPoint": reflect.ValueOf(physics.VelocityAtPoint), + "WorldsCollide": reflect.ValueOf(physics.WorldsCollide), + + // type definitions + "BBox": reflect.ValueOf((*physics.BBox)(nil)), + "BodyVars": reflect.ValueOf((*physics.BodyVars)(nil)), + "ContactVars": reflect.ValueOf((*physics.ContactVars)(nil)), + "DynamicVars": reflect.ValueOf((*physics.DynamicVars)(nil)), + "GPUVars": reflect.ValueOf((*physics.GPUVars)(nil)), + "GeomData": reflect.ValueOf((*physics.GeomData)(nil)), + "JointControlVars": reflect.ValueOf((*physics.JointControlVars)(nil)), + "JointDoFVars": reflect.ValueOf((*physics.JointDoFVars)(nil)), + "JointTypes": reflect.ValueOf((*physics.JointTypes)(nil)), + "JointVars": reflect.ValueOf((*physics.JointVars)(nil)), + "PhysParams": reflect.ValueOf((*physics.PhysParams)(nil)), + "Shapes": reflect.ValueOf((*physics.Shapes)(nil)), + "State": reflect.ValueOf((*physics.State)(nil)), + "World": reflect.ValueOf((*physics.World)(nil)), + } +} diff --git a/yaegilab/labsymbols/make b/yaegilab/labsymbols/make index f1a5b976..d658834e 100755 --- a/yaegilab/labsymbols/make +++ b/yaegilab/labsymbols/make @@ -6,5 +6,5 @@ command extract { } } -extract plot plot/plots plotcore tensorcore lab +extract physics physics/phyxyz plot plot/plots plotcore tensorcore lab From 81619bf74ec8b0cb450d9207f597396ec8a1cbd3 Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Thu, 25 Dec 2025 06:43:07 +0100 Subject: [PATCH 54/97] physics: world replicas, object ids --- docs/content/physics.md | 32 ++++- physics/body.go | 20 ++++ physics/body.goal | 20 ++++ physics/enumgen.go | 10 +- physics/shaders/CollisionBroad.wgsl | 82 ++++++------- physics/shaders/CollisionNarrow.wgsl | 82 ++++++------- physics/shaders/DynamicsCurToNext.wgsl | 79 ++++++------- physics/shaders/ForcesFromJoints.wgsl | 79 ++++++------- physics/shaders/InitDynamics.wgsl | 79 ++++++------- physics/shaders/StepBodyContactDeltas.wgsl | 79 ++++++------- physics/shaders/StepBodyContacts.wgsl | 82 ++++++------- physics/shaders/StepBodyJointDeltas.wgsl | 79 ++++++------- physics/shaders/StepIntegrateBodies.wgsl | 79 ++++++------- physics/shaders/StepJointForces.wgsl | 79 ++++++------- physics/shaders/StepSolveJoints.wgsl | 79 ++++++------- physics/typegen.go | 4 +- physics/world.go | 130 ++++++++++++--------- physics/world.goal | 130 ++++++++++++--------- 18 files changed, 672 insertions(+), 552 deletions(-) diff --git a/docs/content/physics.md b/docs/content/physics.md index 101349a3..020c0f65 100644 --- a/docs/content/physics.md +++ b/docs/content/physics.md @@ -98,13 +98,17 @@ As discussed in [[GoSL]], to run equivalent code on the GPU and the CPU (i.e., s The basic element is a _body_, which is a rigid physical entity with a specific shape, mass, position and orientation. Call [[doc:physics.World]] `NewBody` to create a new one. There are (currently) only standard geometric [[#shapes]] available (arbitrary triangular meshes and soft bodies could be supported as needed in the future, based on existing newton-physics code). -By itself, a body is static. To make a body that is subject to forces and can be connected to other bodies via [[#joints]], use `NewDynamic`, which creates and additional set of data to implement the dynamic equations of the physics solver. The initial position and orientation of the body can be restored via the `InitState` method. +By itself, a body is static. To make a body that is subject to forces and can be connected to other bodies via [[#joints]], use `NewDynamic`, which creates an additional set of data to implement the dynamic equations of the physics solver. The initial position and orientation of a dynamic body can be restored via the `InitState` method. To optimize the collision detection computation, it is important to organize bodies into `World` and `Group` elements: -* World: Use different world indexes for separate collections of bodies that only interact amongst themselves, and global bodies that have a -1 index. By default everything goes in world = 0. -* Group: typically just use -1 for all static bodies (non-dynamic), which can interact with any dynamic body, but not with any other static body. And use 1 for all dynamic bodies, which can interact with each-other and static bodies. -There is a special constraint where the parent and child on a same joint do not collide, as this often happens and would lead to weird behavior. +* World: Use different world indexes for separate collections of bodies that only interact amongst themselves, and global bodies that have a -1 index. By default everything goes in world = 0. See [[#parallel worlds]] for more info. + +* Group: by default this is set to -1 for all static bodies (non-dynamic), which can interact with any dynamic body, but not with any other static body, and to 1 for all dynamic bodies, which can interact with each other and static bodies. To make dynamic bodies that don't interact, assign them increasing group numbers. + +There is also a special constraint where the parent and child on a same joint do not collide, as this often happens and would lead to weird behavior. + +There is also an `Object` index for each body, that is used for external manipulation and control purposes, but does not affect collision or physics. ## Shapes @@ -134,10 +138,28 @@ The supported [[doc:physics.JointTypes]] include the following (DoF = degrees-of Use `NewJoint*` with _dynamic_ body indexes to create joints (e.g., `NewJointPrismatic` etc). Each joint can be positioned with a relative offset and orientation relative to the _parent_ and _child_ elements. The parent index can be set to -1 to anchor a child body in an arbitrary and fixed position within the overall world. -## World viewer +## Phyxyz viewer Typically, bodies are created using the enhanced functions in the [[doc:physics/phyxyz]] package, which provides a [[doc:physics/phyxyz.View]] wrapper for physics bodies. This wrapper has a default `Color` setting to provide simple color coding of bodies, and supports `NewView` and `InitView` functions that allow arbitrary visualization dynamics to be associated with each body (textures, meshes, dynamic updating etc). +## Parallel worlds + +The compute efficiency of the GPU goes up with the more elements that are processed in parallel, amortizing the memory transfer overhead and leveraging the parallel cores. Furthermore, in AI applications for example, models can be trained in parallel on different instances of the same environment, with each instance having its own random initial starting point and trajectory over time. All of these instances can be simulated in one `physics.World` by using the `World` index on the bodies, with the shared static environment living in World -1, and the elements of each instance (e.g., a simulated robot) living in its own separate world. + +The `NewBody` and `NewDynamic` methods automatically use the `World.CurrentWorld` index by default, or you can directly use `SetBodyWorld` to assign a specific world index. + +The `ReplicateWorld` method creates N replicas of an existing world, including all associated joints. This can only be called once, as it records the start and N-per-world of each such replicated world, which allows the `phyxyz` viewer to efficiently view a specific world. Thus, under this scenario, you create world 0 and then replicate it, then modify the initial positions and orientations accordingly, using `PositionObject`, as described next. The object numbers are also replicated so uniquely indexing a specific object instance requires specifying the world and object indexes. + +The phyxyz viewer can display a specific world, or all worlds. + +## Manipulating objects + +TODO. + +## Sensors + +TODO. + ## Physics solver algorithms This section provides a brief overview of different physics solver algorithms, and motivates why we're using XPBD (see [[@MacklinMullerChentanez16]] and [[@MullerMacklinChentanezEtAl20]] for full info). See [[@CollinsChandVanderkopEtAl21]] for a recent review of relevant software and approaches. There are two main categories of mathematical problems that these engines solve: diff --git a/physics/body.go b/physics/body.go index ee3dd834..7cf9050a 100644 --- a/physics/body.go +++ b/physics/body.go @@ -29,6 +29,7 @@ const ( // BodyWorld partitions bodies into different worlds for // collision detection: Global bodies = -1 can collide with // everything; otherwise only items within the same world collide. + // NewBody uses [World.CurrentWorld] to initialize. BodyWorld // BodyGroup partitions bodies within worlds into different groups @@ -45,6 +46,14 @@ const ( // does not need to be handled here. BodyGroup + // BodyObject identifies bodies that all belong to the same overall + // physical object (e.g., a robot or simulated animal). It has no + // implications for collision or physics simulation. Instead, + // it is used for external manipulation on entire objects at a time, + // e.g. via PositionObject. When using world replicas, the object + // indexes are copied, so objects are indexed by world and object id. + BodyObject + // BodyHSize is the size of the object (values depend on shape type). BodyHSizeX BodyHSizeY @@ -165,6 +174,17 @@ func GetBodyGroup(idx int32) int32 { return int32(math.Float32bits(Bodies.Value(int(idx), int(BodyGroup)))) } +// SetBodyObject sets the [BodyObject] index for given body. +// Object indexes are used for external manipulation of multiple +// bodies in a coherent manner. +func SetBodyObject(idx, w int32) { + Bodies.Set(math.Float32frombits(uint32(w)), int(idx), int(BodyObject)) +} + +func GetBodyObject(idx int32) int32 { + return int32(math.Float32bits(Bodies.Value(int(idx), int(BodyObject)))) +} + func BodyHSize(idx int32) math32.Vector3 { return math32.Vec3(Bodies.Value(int(idx), int(BodyHSizeX)), Bodies.Value(int(idx), int(BodyHSizeY)), Bodies.Value(int(idx), int(BodyHSizeZ))) } diff --git a/physics/body.goal b/physics/body.goal index fe10b405..1de95034 100644 --- a/physics/body.goal +++ b/physics/body.goal @@ -27,6 +27,7 @@ const ( // BodyWorld partitions bodies into different worlds for // collision detection: Global bodies = -1 can collide with // everything; otherwise only items within the same world collide. + // NewBody uses [World.CurrentWorld] to initialize. BodyWorld // BodyGroup partitions bodies within worlds into different groups @@ -43,6 +44,14 @@ const ( // does not need to be handled here. BodyGroup + // BodyObject identifies bodies that all belong to the same overall + // physical object (e.g., a robot or simulated animal). It has no + // implications for collision or physics simulation. Instead, + // it is used for external manipulation on entire objects at a time, + // e.g. via PositionObject. When using world replicas, the object + // indexes are copied, so objects are indexed by world and object id. + BodyObject + // BodyHSize is the size of the object (values depend on shape type). BodyHSizeX BodyHSizeY @@ -163,6 +172,17 @@ func GetBodyGroup(idx int32) int32 { return int32(math.Float32bits(Bodies[idx, BodyGroup])) } +// SetBodyObject sets the [BodyObject] index for given body. +// Object indexes are used for external manipulation of multiple +// bodies in a coherent manner. +func SetBodyObject(idx, w int32) { + Bodies[idx, BodyObject] = math.Float32frombits(uint32(w)) +} + +func GetBodyObject(idx int32) int32 { + return int32(math.Float32bits(Bodies[idx, BodyObject])) +} + func BodyHSize(idx int32) math32.Vector3 { return math32.Vec3(Bodies[idx, BodyHSizeX], Bodies[idx, BodyHSizeY], Bodies[idx, BodyHSizeZ]) } diff --git a/physics/enumgen.go b/physics/enumgen.go index 8dbd4d46..6d85c704 100644 --- a/physics/enumgen.go +++ b/physics/enumgen.go @@ -6,20 +6,20 @@ import ( "cogentcore.org/core/enums" ) -var _BodyVarsValues = []BodyVars{0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42} +var _BodyVarsValues = []BodyVars{0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43} // BodyVarsN is the highest valid value for type BodyVars, plus one. // //gosl:start -const BodyVarsN BodyVars = 43 +const BodyVarsN BodyVars = 44 //gosl:end -var _BodyVarsValueMap = map[string]BodyVars{`BodyShape`: 0, `BodyDynamic`: 1, `BodyWorld`: 2, `BodyGroup`: 3, `BodyHSizeX`: 4, `BodyHSizeY`: 5, `BodyHSizeZ`: 6, `BodyThick`: 7, `BodyMass`: 8, `BodyInvMass`: 9, `BodyBounce`: 10, `BodyFriction`: 11, `BodyFrictionTortion`: 12, `BodyFrictionRolling`: 13, `BodyPosX`: 14, `BodyPosY`: 15, `BodyPosZ`: 16, `BodyQuatX`: 17, `BodyQuatY`: 18, `BodyQuatZ`: 19, `BodyQuatW`: 20, `BodyComX`: 21, `BodyComY`: 22, `BodyComZ`: 23, `BodyInertiaXX`: 24, `BodyInertiaYX`: 25, `BodyInertiaZX`: 26, `BodyInertiaXY`: 27, `BodyInertiaYY`: 28, `BodyInertiaZY`: 29, `BodyInertiaXZ`: 30, `BodyInertiaYZ`: 31, `BodyInertiaZZ`: 32, `BodyInvInertiaXX`: 33, `BodyInvInertiaYX`: 34, `BodyInvInertiaZX`: 35, `BodyInvInertiaXY`: 36, `BodyInvInertiaYY`: 37, `BodyInvInertiaZY`: 38, `BodyInvInertiaXZ`: 39, `BodyInvInertiaYZ`: 40, `BodyInvInertiaZZ`: 41, `BodyRadius`: 42} +var _BodyVarsValueMap = map[string]BodyVars{`BodyShape`: 0, `BodyDynamic`: 1, `BodyWorld`: 2, `BodyGroup`: 3, `BodyObject`: 4, `BodyHSizeX`: 5, `BodyHSizeY`: 6, `BodyHSizeZ`: 7, `BodyThick`: 8, `BodyMass`: 9, `BodyInvMass`: 10, `BodyBounce`: 11, `BodyFriction`: 12, `BodyFrictionTortion`: 13, `BodyFrictionRolling`: 14, `BodyPosX`: 15, `BodyPosY`: 16, `BodyPosZ`: 17, `BodyQuatX`: 18, `BodyQuatY`: 19, `BodyQuatZ`: 20, `BodyQuatW`: 21, `BodyComX`: 22, `BodyComY`: 23, `BodyComZ`: 24, `BodyInertiaXX`: 25, `BodyInertiaYX`: 26, `BodyInertiaZX`: 27, `BodyInertiaXY`: 28, `BodyInertiaYY`: 29, `BodyInertiaZY`: 30, `BodyInertiaXZ`: 31, `BodyInertiaYZ`: 32, `BodyInertiaZZ`: 33, `BodyInvInertiaXX`: 34, `BodyInvInertiaYX`: 35, `BodyInvInertiaZX`: 36, `BodyInvInertiaXY`: 37, `BodyInvInertiaYY`: 38, `BodyInvInertiaZY`: 39, `BodyInvInertiaXZ`: 40, `BodyInvInertiaYZ`: 41, `BodyInvInertiaZZ`: 42, `BodyRadius`: 43} -var _BodyVarsDescMap = map[BodyVars]string{0: `BodyShape is the shape type of the object, as a Shapes type.`, 1: `BodyDynamic is the index into Dynamics for this body, which is -1 for static bodies. Use this to get current Pos and Quat values for a dynamic body.`, 2: `BodyWorld partitions bodies into different worlds for collision detection: Global bodies = -1 can collide with everything; otherwise only items within the same world collide.`, 3: `BodyGroup partitions bodies within worlds into different groups for collision detection. 0 does not collide with anything. Negative numbers are global within a world, except they don't collide amongst themselves (all non-dynamic bodies should go in -1 because they don't collide amongst each-other, but do potentially collide with dynamics). Positive numbers only collide amongst themselves, and with negative groups, but not other positive groups. This is for more special-purpose dynamics: in general use 1 for all dynamic bodies. There is an automatic constraint that the two objects within a single joint do not collide with each other, so this does not need to be handled here.`, 4: `BodyHSize is the size of the object (values depend on shape type).`, 5: ``, 6: ``, 7: `BodyThick is the thickness of the body, as a hollow shape. If 0, then it is a solid shape (default).`, 8: `BodyMass is the mass of the object.`, 9: `BodyInvMass is 1/mass of the object or 0 if no mass.`, 10: `BodyBounce specifies the COR or coefficient of restitution (0..1), which determines how elastic the collision is, i.e., final velocity / initial velocity.`, 11: `BodyFriction is the standard coefficient for linear friction (mu).`, 12: `BodyFrictionTortion is resistance to spinning at the contact point.`, 13: `BodyFrictionRolling is resistance to rolling motion at contact.`, 14: `3D position of body (structural center).`, 15: ``, 16: ``, 17: `Quaternion rotation of body.`, 18: ``, 19: ``, 20: ``, 21: `Relative center-of-mass offset from 3D position of body.`, 22: ``, 23: ``, 24: `Inertia 3x3 matrix (column matrix organization, r,c labels).`, 25: ``, 26: ``, 27: ``, 28: ``, 29: ``, 30: ``, 31: ``, 32: ``, 33: `InvInertia inverse inertia 3x3 matrix (column matrix organization, r,c labels).`, 34: ``, 35: ``, 36: ``, 37: ``, 38: ``, 39: ``, 40: ``, 41: ``, 42: `radius for broadphase collision`} +var _BodyVarsDescMap = map[BodyVars]string{0: `BodyShape is the shape type of the object, as a Shapes type.`, 1: `BodyDynamic is the index into Dynamics for this body, which is -1 for static bodies. Use this to get current Pos and Quat values for a dynamic body.`, 2: `BodyWorld partitions bodies into different worlds for collision detection: Global bodies = -1 can collide with everything; otherwise only items within the same world collide. NewBody uses [World.CurrentWorld] to initialize.`, 3: `BodyGroup partitions bodies within worlds into different groups for collision detection. 0 does not collide with anything. Negative numbers are global within a world, except they don't collide amongst themselves (all non-dynamic bodies should go in -1 because they don't collide amongst each-other, but do potentially collide with dynamics). Positive numbers only collide amongst themselves, and with negative groups, but not other positive groups. This is for more special-purpose dynamics: in general use 1 for all dynamic bodies. There is an automatic constraint that the two objects within a single joint do not collide with each other, so this does not need to be handled here.`, 4: `BodyObject identifies bodies that all belong to the same overall physical object (e.g., a robot or simulated animal). It has no implications for collision or physics simulation. Instead, it is used for external manipulation on entire objects at a time, e.g. via PositionObject. When using world replicas, the object indexes are copied, so objects are indexed by world and object id.`, 5: `BodyHSize is the size of the object (values depend on shape type).`, 6: ``, 7: ``, 8: `BodyThick is the thickness of the body, as a hollow shape. If 0, then it is a solid shape (default).`, 9: `BodyMass is the mass of the object.`, 10: `BodyInvMass is 1/mass of the object or 0 if no mass.`, 11: `BodyBounce specifies the COR or coefficient of restitution (0..1), which determines how elastic the collision is, i.e., final velocity / initial velocity.`, 12: `BodyFriction is the standard coefficient for linear friction (mu).`, 13: `BodyFrictionTortion is resistance to spinning at the contact point.`, 14: `BodyFrictionRolling is resistance to rolling motion at contact.`, 15: `3D position of body (structural center).`, 16: ``, 17: ``, 18: `Quaternion rotation of body.`, 19: ``, 20: ``, 21: ``, 22: `Relative center-of-mass offset from 3D position of body.`, 23: ``, 24: ``, 25: `Inertia 3x3 matrix (column matrix organization, r,c labels).`, 26: ``, 27: ``, 28: ``, 29: ``, 30: ``, 31: ``, 32: ``, 33: ``, 34: `InvInertia inverse inertia 3x3 matrix (column matrix organization, r,c labels).`, 35: ``, 36: ``, 37: ``, 38: ``, 39: ``, 40: ``, 41: ``, 42: ``, 43: `radius for broadphase collision`} -var _BodyVarsMap = map[BodyVars]string{0: `BodyShape`, 1: `BodyDynamic`, 2: `BodyWorld`, 3: `BodyGroup`, 4: `BodyHSizeX`, 5: `BodyHSizeY`, 6: `BodyHSizeZ`, 7: `BodyThick`, 8: `BodyMass`, 9: `BodyInvMass`, 10: `BodyBounce`, 11: `BodyFriction`, 12: `BodyFrictionTortion`, 13: `BodyFrictionRolling`, 14: `BodyPosX`, 15: `BodyPosY`, 16: `BodyPosZ`, 17: `BodyQuatX`, 18: `BodyQuatY`, 19: `BodyQuatZ`, 20: `BodyQuatW`, 21: `BodyComX`, 22: `BodyComY`, 23: `BodyComZ`, 24: `BodyInertiaXX`, 25: `BodyInertiaYX`, 26: `BodyInertiaZX`, 27: `BodyInertiaXY`, 28: `BodyInertiaYY`, 29: `BodyInertiaZY`, 30: `BodyInertiaXZ`, 31: `BodyInertiaYZ`, 32: `BodyInertiaZZ`, 33: `BodyInvInertiaXX`, 34: `BodyInvInertiaYX`, 35: `BodyInvInertiaZX`, 36: `BodyInvInertiaXY`, 37: `BodyInvInertiaYY`, 38: `BodyInvInertiaZY`, 39: `BodyInvInertiaXZ`, 40: `BodyInvInertiaYZ`, 41: `BodyInvInertiaZZ`, 42: `BodyRadius`} +var _BodyVarsMap = map[BodyVars]string{0: `BodyShape`, 1: `BodyDynamic`, 2: `BodyWorld`, 3: `BodyGroup`, 4: `BodyObject`, 5: `BodyHSizeX`, 6: `BodyHSizeY`, 7: `BodyHSizeZ`, 8: `BodyThick`, 9: `BodyMass`, 10: `BodyInvMass`, 11: `BodyBounce`, 12: `BodyFriction`, 13: `BodyFrictionTortion`, 14: `BodyFrictionRolling`, 15: `BodyPosX`, 16: `BodyPosY`, 17: `BodyPosZ`, 18: `BodyQuatX`, 19: `BodyQuatY`, 20: `BodyQuatZ`, 21: `BodyQuatW`, 22: `BodyComX`, 23: `BodyComY`, 24: `BodyComZ`, 25: `BodyInertiaXX`, 26: `BodyInertiaYX`, 27: `BodyInertiaZX`, 28: `BodyInertiaXY`, 29: `BodyInertiaYY`, 30: `BodyInertiaZY`, 31: `BodyInertiaXZ`, 32: `BodyInertiaYZ`, 33: `BodyInertiaZZ`, 34: `BodyInvInertiaXX`, 35: `BodyInvInertiaYX`, 36: `BodyInvInertiaZX`, 37: `BodyInvInertiaXY`, 38: `BodyInvInertiaYY`, 39: `BodyInvInertiaZY`, 40: `BodyInvInertiaXZ`, 41: `BodyInvInertiaYZ`, 42: `BodyInvInertiaZZ`, 43: `BodyRadius`} // String returns the string representation of this BodyVars value. func (i BodyVars) String() string { return enums.String(i, _BodyVarsMap) } diff --git a/physics/shaders/CollisionBroad.wgsl b/physics/shaders/CollisionBroad.wgsl index d9ee0666..d8a47ac4 100644 --- a/physics/shaders/CollisionBroad.wgsl +++ b/physics/shaders/CollisionBroad.wgsl @@ -49,50 +49,52 @@ const BodyShape: BodyVars = 0; const BodyDynamic: BodyVars = 1; const BodyWorld: BodyVars = 2; const BodyGroup: BodyVars = 3; -const BodyHSizeX: BodyVars = 4; -const BodyHSizeY: BodyVars = 5; -const BodyHSizeZ: BodyVars = 6; -const BodyThick: BodyVars = 7; -const BodyMass: BodyVars = 8; -const BodyInvMass: BodyVars = 9; -const BodyBounce: BodyVars = 10; -const BodyFriction: BodyVars = 11; -const BodyFrictionTortion: BodyVars = 12; -const BodyFrictionRolling: BodyVars = 13; -const BodyPosX: BodyVars = 14; -const BodyPosY: BodyVars = 15; -const BodyPosZ: BodyVars = 16; -const BodyQuatX: BodyVars = 17; -const BodyQuatY: BodyVars = 18; -const BodyQuatZ: BodyVars = 19; -const BodyQuatW: BodyVars = 20; -const BodyComX: BodyVars = 21; -const BodyComY: BodyVars = 22; -const BodyComZ: BodyVars = 23; -const BodyInertiaXX: BodyVars = 24; -const BodyInertiaYX: BodyVars = 25; -const BodyInertiaZX: BodyVars = 26; -const BodyInertiaXY: BodyVars = 27; -const BodyInertiaYY: BodyVars = 28; -const BodyInertiaZY: BodyVars = 29; -const BodyInertiaXZ: BodyVars = 30; -const BodyInertiaYZ: BodyVars = 31; -const BodyInertiaZZ: BodyVars = 32; -const BodyInvInertiaXX: BodyVars = 33; -const BodyInvInertiaYX: BodyVars = 34; -const BodyInvInertiaZX: BodyVars = 35; -const BodyInvInertiaXY: BodyVars = 36; -const BodyInvInertiaYY: BodyVars = 37; -const BodyInvInertiaZY: BodyVars = 38; -const BodyInvInertiaXZ: BodyVars = 39; -const BodyInvInertiaYZ: BodyVars = 40; -const BodyInvInertiaZZ: BodyVars = 41; -const BodyRadius: BodyVars = 42; +const BodyObject: BodyVars = 4; +const BodyHSizeX: BodyVars = 5; +const BodyHSizeY: BodyVars = 6; +const BodyHSizeZ: BodyVars = 7; +const BodyThick: BodyVars = 8; +const BodyMass: BodyVars = 9; +const BodyInvMass: BodyVars = 10; +const BodyBounce: BodyVars = 11; +const BodyFriction: BodyVars = 12; +const BodyFrictionTortion: BodyVars = 13; +const BodyFrictionRolling: BodyVars = 14; +const BodyPosX: BodyVars = 15; +const BodyPosY: BodyVars = 16; +const BodyPosZ: BodyVars = 17; +const BodyQuatX: BodyVars = 18; +const BodyQuatY: BodyVars = 19; +const BodyQuatZ: BodyVars = 20; +const BodyQuatW: BodyVars = 21; +const BodyComX: BodyVars = 22; +const BodyComY: BodyVars = 23; +const BodyComZ: BodyVars = 24; +const BodyInertiaXX: BodyVars = 25; +const BodyInertiaYX: BodyVars = 26; +const BodyInertiaZX: BodyVars = 27; +const BodyInertiaXY: BodyVars = 28; +const BodyInertiaYY: BodyVars = 29; +const BodyInertiaZY: BodyVars = 30; +const BodyInertiaXZ: BodyVars = 31; +const BodyInertiaYZ: BodyVars = 32; +const BodyInertiaZZ: BodyVars = 33; +const BodyInvInertiaXX: BodyVars = 34; +const BodyInvInertiaYX: BodyVars = 35; +const BodyInvInertiaZX: BodyVars = 36; +const BodyInvInertiaXY: BodyVars = 37; +const BodyInvInertiaYY: BodyVars = 38; +const BodyInvInertiaZY: BodyVars = 39; +const BodyInvInertiaXZ: BodyVars = 40; +const BodyInvInertiaYZ: BodyVars = 41; +const BodyInvInertiaZZ: BodyVars = 42; +const BodyRadius: BodyVars = 43; fn GetBodyShape(idx: i32) -> Shapes { return Shapes(bitcast(Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyShape))])); } fn GetBodyDynamic(idx: i32) -> i32 { - return i32(bitcast(Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyDynamic))])); + return i32(bitcast(Bodies[Index2D(TensorStrides[0], TensorStrides[1], + u32(idx), u32(BodyDynamic))])); } fn BodyHSize(idx: i32) -> vec3 { return vec3(Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyHSizeX))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyHSizeY))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyHSizeZ))]); diff --git a/physics/shaders/CollisionNarrow.wgsl b/physics/shaders/CollisionNarrow.wgsl index 3a40db63..302d4d74 100644 --- a/physics/shaders/CollisionNarrow.wgsl +++ b/physics/shaders/CollisionNarrow.wgsl @@ -51,50 +51,52 @@ const BodyShape: BodyVars = 0; const BodyDynamic: BodyVars = 1; const BodyWorld: BodyVars = 2; const BodyGroup: BodyVars = 3; -const BodyHSizeX: BodyVars = 4; -const BodyHSizeY: BodyVars = 5; -const BodyHSizeZ: BodyVars = 6; -const BodyThick: BodyVars = 7; -const BodyMass: BodyVars = 8; -const BodyInvMass: BodyVars = 9; -const BodyBounce: BodyVars = 10; -const BodyFriction: BodyVars = 11; -const BodyFrictionTortion: BodyVars = 12; -const BodyFrictionRolling: BodyVars = 13; -const BodyPosX: BodyVars = 14; -const BodyPosY: BodyVars = 15; -const BodyPosZ: BodyVars = 16; -const BodyQuatX: BodyVars = 17; -const BodyQuatY: BodyVars = 18; -const BodyQuatZ: BodyVars = 19; -const BodyQuatW: BodyVars = 20; -const BodyComX: BodyVars = 21; -const BodyComY: BodyVars = 22; -const BodyComZ: BodyVars = 23; -const BodyInertiaXX: BodyVars = 24; -const BodyInertiaYX: BodyVars = 25; -const BodyInertiaZX: BodyVars = 26; -const BodyInertiaXY: BodyVars = 27; -const BodyInertiaYY: BodyVars = 28; -const BodyInertiaZY: BodyVars = 29; -const BodyInertiaXZ: BodyVars = 30; -const BodyInertiaYZ: BodyVars = 31; -const BodyInertiaZZ: BodyVars = 32; -const BodyInvInertiaXX: BodyVars = 33; -const BodyInvInertiaYX: BodyVars = 34; -const BodyInvInertiaZX: BodyVars = 35; -const BodyInvInertiaXY: BodyVars = 36; -const BodyInvInertiaYY: BodyVars = 37; -const BodyInvInertiaZY: BodyVars = 38; -const BodyInvInertiaXZ: BodyVars = 39; -const BodyInvInertiaYZ: BodyVars = 40; -const BodyInvInertiaZZ: BodyVars = 41; -const BodyRadius: BodyVars = 42; +const BodyObject: BodyVars = 4; +const BodyHSizeX: BodyVars = 5; +const BodyHSizeY: BodyVars = 6; +const BodyHSizeZ: BodyVars = 7; +const BodyThick: BodyVars = 8; +const BodyMass: BodyVars = 9; +const BodyInvMass: BodyVars = 10; +const BodyBounce: BodyVars = 11; +const BodyFriction: BodyVars = 12; +const BodyFrictionTortion: BodyVars = 13; +const BodyFrictionRolling: BodyVars = 14; +const BodyPosX: BodyVars = 15; +const BodyPosY: BodyVars = 16; +const BodyPosZ: BodyVars = 17; +const BodyQuatX: BodyVars = 18; +const BodyQuatY: BodyVars = 19; +const BodyQuatZ: BodyVars = 20; +const BodyQuatW: BodyVars = 21; +const BodyComX: BodyVars = 22; +const BodyComY: BodyVars = 23; +const BodyComZ: BodyVars = 24; +const BodyInertiaXX: BodyVars = 25; +const BodyInertiaYX: BodyVars = 26; +const BodyInertiaZX: BodyVars = 27; +const BodyInertiaXY: BodyVars = 28; +const BodyInertiaYY: BodyVars = 29; +const BodyInertiaZY: BodyVars = 30; +const BodyInertiaXZ: BodyVars = 31; +const BodyInertiaYZ: BodyVars = 32; +const BodyInertiaZZ: BodyVars = 33; +const BodyInvInertiaXX: BodyVars = 34; +const BodyInvInertiaYX: BodyVars = 35; +const BodyInvInertiaZX: BodyVars = 36; +const BodyInvInertiaXY: BodyVars = 37; +const BodyInvInertiaYY: BodyVars = 38; +const BodyInvInertiaZY: BodyVars = 39; +const BodyInvInertiaXZ: BodyVars = 40; +const BodyInvInertiaYZ: BodyVars = 41; +const BodyInvInertiaZZ: BodyVars = 42; +const BodyRadius: BodyVars = 43; fn GetBodyShape(idx: i32) -> Shapes { return Shapes(bitcast(Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyShape))])); } fn GetBodyDynamic(idx: i32) -> i32 { - return i32(bitcast(Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyDynamic))])); + return i32(bitcast(Bodies[Index2D(TensorStrides[0], TensorStrides[1], + u32(idx), u32(BodyDynamic))])); } fn BodyHSize(idx: i32) -> vec3 { return vec3(Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyHSizeX))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyHSizeY))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyHSizeZ))]); diff --git a/physics/shaders/DynamicsCurToNext.wgsl b/physics/shaders/DynamicsCurToNext.wgsl index 85cb614e..9df7f5be 100644 --- a/physics/shaders/DynamicsCurToNext.wgsl +++ b/physics/shaders/DynamicsCurToNext.wgsl @@ -33,45 +33,46 @@ const BodyShape: BodyVars = 0; const BodyDynamic: BodyVars = 1; const BodyWorld: BodyVars = 2; const BodyGroup: BodyVars = 3; -const BodyHSizeX: BodyVars = 4; -const BodyHSizeY: BodyVars = 5; -const BodyHSizeZ: BodyVars = 6; -const BodyThick: BodyVars = 7; -const BodyMass: BodyVars = 8; -const BodyInvMass: BodyVars = 9; -const BodyBounce: BodyVars = 10; -const BodyFriction: BodyVars = 11; -const BodyFrictionTortion: BodyVars = 12; -const BodyFrictionRolling: BodyVars = 13; -const BodyPosX: BodyVars = 14; -const BodyPosY: BodyVars = 15; -const BodyPosZ: BodyVars = 16; -const BodyQuatX: BodyVars = 17; -const BodyQuatY: BodyVars = 18; -const BodyQuatZ: BodyVars = 19; -const BodyQuatW: BodyVars = 20; -const BodyComX: BodyVars = 21; -const BodyComY: BodyVars = 22; -const BodyComZ: BodyVars = 23; -const BodyInertiaXX: BodyVars = 24; -const BodyInertiaYX: BodyVars = 25; -const BodyInertiaZX: BodyVars = 26; -const BodyInertiaXY: BodyVars = 27; -const BodyInertiaYY: BodyVars = 28; -const BodyInertiaZY: BodyVars = 29; -const BodyInertiaXZ: BodyVars = 30; -const BodyInertiaYZ: BodyVars = 31; -const BodyInertiaZZ: BodyVars = 32; -const BodyInvInertiaXX: BodyVars = 33; -const BodyInvInertiaYX: BodyVars = 34; -const BodyInvInertiaZX: BodyVars = 35; -const BodyInvInertiaXY: BodyVars = 36; -const BodyInvInertiaYY: BodyVars = 37; -const BodyInvInertiaZY: BodyVars = 38; -const BodyInvInertiaXZ: BodyVars = 39; -const BodyInvInertiaYZ: BodyVars = 40; -const BodyInvInertiaZZ: BodyVars = 41; -const BodyRadius: BodyVars = 42; +const BodyObject: BodyVars = 4; +const BodyHSizeX: BodyVars = 5; +const BodyHSizeY: BodyVars = 6; +const BodyHSizeZ: BodyVars = 7; +const BodyThick: BodyVars = 8; +const BodyMass: BodyVars = 9; +const BodyInvMass: BodyVars = 10; +const BodyBounce: BodyVars = 11; +const BodyFriction: BodyVars = 12; +const BodyFrictionTortion: BodyVars = 13; +const BodyFrictionRolling: BodyVars = 14; +const BodyPosX: BodyVars = 15; +const BodyPosY: BodyVars = 16; +const BodyPosZ: BodyVars = 17; +const BodyQuatX: BodyVars = 18; +const BodyQuatY: BodyVars = 19; +const BodyQuatZ: BodyVars = 20; +const BodyQuatW: BodyVars = 21; +const BodyComX: BodyVars = 22; +const BodyComY: BodyVars = 23; +const BodyComZ: BodyVars = 24; +const BodyInertiaXX: BodyVars = 25; +const BodyInertiaYX: BodyVars = 26; +const BodyInertiaZX: BodyVars = 27; +const BodyInertiaXY: BodyVars = 28; +const BodyInertiaYY: BodyVars = 29; +const BodyInertiaZY: BodyVars = 30; +const BodyInertiaXZ: BodyVars = 31; +const BodyInertiaYZ: BodyVars = 32; +const BodyInertiaZZ: BodyVars = 33; +const BodyInvInertiaXX: BodyVars = 34; +const BodyInvInertiaYX: BodyVars = 35; +const BodyInvInertiaZX: BodyVars = 36; +const BodyInvInertiaXY: BodyVars = 37; +const BodyInvInertiaYY: BodyVars = 38; +const BodyInvInertiaZY: BodyVars = 39; +const BodyInvInertiaXZ: BodyVars = 40; +const BodyInvInertiaYZ: BodyVars = 41; +const BodyInvInertiaZZ: BodyVars = 42; +const BodyRadius: BodyVars = 43; //////// import: "contact.go" alias ContactVars = i32; //enums:enum diff --git a/physics/shaders/ForcesFromJoints.wgsl b/physics/shaders/ForcesFromJoints.wgsl index 575d3687..1ecf6b68 100644 --- a/physics/shaders/ForcesFromJoints.wgsl +++ b/physics/shaders/ForcesFromJoints.wgsl @@ -41,45 +41,46 @@ const BodyShape: BodyVars = 0; const BodyDynamic: BodyVars = 1; const BodyWorld: BodyVars = 2; const BodyGroup: BodyVars = 3; -const BodyHSizeX: BodyVars = 4; -const BodyHSizeY: BodyVars = 5; -const BodyHSizeZ: BodyVars = 6; -const BodyThick: BodyVars = 7; -const BodyMass: BodyVars = 8; -const BodyInvMass: BodyVars = 9; -const BodyBounce: BodyVars = 10; -const BodyFriction: BodyVars = 11; -const BodyFrictionTortion: BodyVars = 12; -const BodyFrictionRolling: BodyVars = 13; -const BodyPosX: BodyVars = 14; -const BodyPosY: BodyVars = 15; -const BodyPosZ: BodyVars = 16; -const BodyQuatX: BodyVars = 17; -const BodyQuatY: BodyVars = 18; -const BodyQuatZ: BodyVars = 19; -const BodyQuatW: BodyVars = 20; -const BodyComX: BodyVars = 21; -const BodyComY: BodyVars = 22; -const BodyComZ: BodyVars = 23; -const BodyInertiaXX: BodyVars = 24; -const BodyInertiaYX: BodyVars = 25; -const BodyInertiaZX: BodyVars = 26; -const BodyInertiaXY: BodyVars = 27; -const BodyInertiaYY: BodyVars = 28; -const BodyInertiaZY: BodyVars = 29; -const BodyInertiaXZ: BodyVars = 30; -const BodyInertiaYZ: BodyVars = 31; -const BodyInertiaZZ: BodyVars = 32; -const BodyInvInertiaXX: BodyVars = 33; -const BodyInvInertiaYX: BodyVars = 34; -const BodyInvInertiaZX: BodyVars = 35; -const BodyInvInertiaXY: BodyVars = 36; -const BodyInvInertiaYY: BodyVars = 37; -const BodyInvInertiaZY: BodyVars = 38; -const BodyInvInertiaXZ: BodyVars = 39; -const BodyInvInertiaYZ: BodyVars = 40; -const BodyInvInertiaZZ: BodyVars = 41; -const BodyRadius: BodyVars = 42; +const BodyObject: BodyVars = 4; +const BodyHSizeX: BodyVars = 5; +const BodyHSizeY: BodyVars = 6; +const BodyHSizeZ: BodyVars = 7; +const BodyThick: BodyVars = 8; +const BodyMass: BodyVars = 9; +const BodyInvMass: BodyVars = 10; +const BodyBounce: BodyVars = 11; +const BodyFriction: BodyVars = 12; +const BodyFrictionTortion: BodyVars = 13; +const BodyFrictionRolling: BodyVars = 14; +const BodyPosX: BodyVars = 15; +const BodyPosY: BodyVars = 16; +const BodyPosZ: BodyVars = 17; +const BodyQuatX: BodyVars = 18; +const BodyQuatY: BodyVars = 19; +const BodyQuatZ: BodyVars = 20; +const BodyQuatW: BodyVars = 21; +const BodyComX: BodyVars = 22; +const BodyComY: BodyVars = 23; +const BodyComZ: BodyVars = 24; +const BodyInertiaXX: BodyVars = 25; +const BodyInertiaYX: BodyVars = 26; +const BodyInertiaZX: BodyVars = 27; +const BodyInertiaXY: BodyVars = 28; +const BodyInertiaYY: BodyVars = 29; +const BodyInertiaZY: BodyVars = 30; +const BodyInertiaXZ: BodyVars = 31; +const BodyInertiaYZ: BodyVars = 32; +const BodyInertiaZZ: BodyVars = 33; +const BodyInvInertiaXX: BodyVars = 34; +const BodyInvInertiaYX: BodyVars = 35; +const BodyInvInertiaZX: BodyVars = 36; +const BodyInvInertiaXY: BodyVars = 37; +const BodyInvInertiaYY: BodyVars = 38; +const BodyInvInertiaZY: BodyVars = 39; +const BodyInvInertiaXZ: BodyVars = 40; +const BodyInvInertiaYZ: BodyVars = 41; +const BodyInvInertiaZZ: BodyVars = 42; +const BodyRadius: BodyVars = 43; //////// import: "contact.go" alias ContactVars = i32; //enums:enum diff --git a/physics/shaders/InitDynamics.wgsl b/physics/shaders/InitDynamics.wgsl index 3fdb41df..39ff9664 100644 --- a/physics/shaders/InitDynamics.wgsl +++ b/physics/shaders/InitDynamics.wgsl @@ -39,45 +39,46 @@ const BodyShape: BodyVars = 0; const BodyDynamic: BodyVars = 1; const BodyWorld: BodyVars = 2; const BodyGroup: BodyVars = 3; -const BodyHSizeX: BodyVars = 4; -const BodyHSizeY: BodyVars = 5; -const BodyHSizeZ: BodyVars = 6; -const BodyThick: BodyVars = 7; -const BodyMass: BodyVars = 8; -const BodyInvMass: BodyVars = 9; -const BodyBounce: BodyVars = 10; -const BodyFriction: BodyVars = 11; -const BodyFrictionTortion: BodyVars = 12; -const BodyFrictionRolling: BodyVars = 13; -const BodyPosX: BodyVars = 14; -const BodyPosY: BodyVars = 15; -const BodyPosZ: BodyVars = 16; -const BodyQuatX: BodyVars = 17; -const BodyQuatY: BodyVars = 18; -const BodyQuatZ: BodyVars = 19; -const BodyQuatW: BodyVars = 20; -const BodyComX: BodyVars = 21; -const BodyComY: BodyVars = 22; -const BodyComZ: BodyVars = 23; -const BodyInertiaXX: BodyVars = 24; -const BodyInertiaYX: BodyVars = 25; -const BodyInertiaZX: BodyVars = 26; -const BodyInertiaXY: BodyVars = 27; -const BodyInertiaYY: BodyVars = 28; -const BodyInertiaZY: BodyVars = 29; -const BodyInertiaXZ: BodyVars = 30; -const BodyInertiaYZ: BodyVars = 31; -const BodyInertiaZZ: BodyVars = 32; -const BodyInvInertiaXX: BodyVars = 33; -const BodyInvInertiaYX: BodyVars = 34; -const BodyInvInertiaZX: BodyVars = 35; -const BodyInvInertiaXY: BodyVars = 36; -const BodyInvInertiaYY: BodyVars = 37; -const BodyInvInertiaZY: BodyVars = 38; -const BodyInvInertiaXZ: BodyVars = 39; -const BodyInvInertiaYZ: BodyVars = 40; -const BodyInvInertiaZZ: BodyVars = 41; -const BodyRadius: BodyVars = 42; +const BodyObject: BodyVars = 4; +const BodyHSizeX: BodyVars = 5; +const BodyHSizeY: BodyVars = 6; +const BodyHSizeZ: BodyVars = 7; +const BodyThick: BodyVars = 8; +const BodyMass: BodyVars = 9; +const BodyInvMass: BodyVars = 10; +const BodyBounce: BodyVars = 11; +const BodyFriction: BodyVars = 12; +const BodyFrictionTortion: BodyVars = 13; +const BodyFrictionRolling: BodyVars = 14; +const BodyPosX: BodyVars = 15; +const BodyPosY: BodyVars = 16; +const BodyPosZ: BodyVars = 17; +const BodyQuatX: BodyVars = 18; +const BodyQuatY: BodyVars = 19; +const BodyQuatZ: BodyVars = 20; +const BodyQuatW: BodyVars = 21; +const BodyComX: BodyVars = 22; +const BodyComY: BodyVars = 23; +const BodyComZ: BodyVars = 24; +const BodyInertiaXX: BodyVars = 25; +const BodyInertiaYX: BodyVars = 26; +const BodyInertiaZX: BodyVars = 27; +const BodyInertiaXY: BodyVars = 28; +const BodyInertiaYY: BodyVars = 29; +const BodyInertiaZY: BodyVars = 30; +const BodyInertiaXZ: BodyVars = 31; +const BodyInertiaYZ: BodyVars = 32; +const BodyInertiaZZ: BodyVars = 33; +const BodyInvInertiaXX: BodyVars = 34; +const BodyInvInertiaYX: BodyVars = 35; +const BodyInvInertiaZX: BodyVars = 36; +const BodyInvInertiaXY: BodyVars = 37; +const BodyInvInertiaYY: BodyVars = 38; +const BodyInvInertiaZY: BodyVars = 39; +const BodyInvInertiaXZ: BodyVars = 40; +const BodyInvInertiaYZ: BodyVars = 41; +const BodyInvInertiaZZ: BodyVars = 42; +const BodyRadius: BodyVars = 43; //////// import: "contact.go" alias ContactVars = i32; //enums:enum diff --git a/physics/shaders/StepBodyContactDeltas.wgsl b/physics/shaders/StepBodyContactDeltas.wgsl index 44f767b9..0eede33a 100644 --- a/physics/shaders/StepBodyContactDeltas.wgsl +++ b/physics/shaders/StepBodyContactDeltas.wgsl @@ -47,45 +47,46 @@ const BodyShape: BodyVars = 0; const BodyDynamic: BodyVars = 1; const BodyWorld: BodyVars = 2; const BodyGroup: BodyVars = 3; -const BodyHSizeX: BodyVars = 4; -const BodyHSizeY: BodyVars = 5; -const BodyHSizeZ: BodyVars = 6; -const BodyThick: BodyVars = 7; -const BodyMass: BodyVars = 8; -const BodyInvMass: BodyVars = 9; -const BodyBounce: BodyVars = 10; -const BodyFriction: BodyVars = 11; -const BodyFrictionTortion: BodyVars = 12; -const BodyFrictionRolling: BodyVars = 13; -const BodyPosX: BodyVars = 14; -const BodyPosY: BodyVars = 15; -const BodyPosZ: BodyVars = 16; -const BodyQuatX: BodyVars = 17; -const BodyQuatY: BodyVars = 18; -const BodyQuatZ: BodyVars = 19; -const BodyQuatW: BodyVars = 20; -const BodyComX: BodyVars = 21; -const BodyComY: BodyVars = 22; -const BodyComZ: BodyVars = 23; -const BodyInertiaXX: BodyVars = 24; -const BodyInertiaYX: BodyVars = 25; -const BodyInertiaZX: BodyVars = 26; -const BodyInertiaXY: BodyVars = 27; -const BodyInertiaYY: BodyVars = 28; -const BodyInertiaZY: BodyVars = 29; -const BodyInertiaXZ: BodyVars = 30; -const BodyInertiaYZ: BodyVars = 31; -const BodyInertiaZZ: BodyVars = 32; -const BodyInvInertiaXX: BodyVars = 33; -const BodyInvInertiaYX: BodyVars = 34; -const BodyInvInertiaZX: BodyVars = 35; -const BodyInvInertiaXY: BodyVars = 36; -const BodyInvInertiaYY: BodyVars = 37; -const BodyInvInertiaZY: BodyVars = 38; -const BodyInvInertiaXZ: BodyVars = 39; -const BodyInvInertiaYZ: BodyVars = 40; -const BodyInvInertiaZZ: BodyVars = 41; -const BodyRadius: BodyVars = 42; +const BodyObject: BodyVars = 4; +const BodyHSizeX: BodyVars = 5; +const BodyHSizeY: BodyVars = 6; +const BodyHSizeZ: BodyVars = 7; +const BodyThick: BodyVars = 8; +const BodyMass: BodyVars = 9; +const BodyInvMass: BodyVars = 10; +const BodyBounce: BodyVars = 11; +const BodyFriction: BodyVars = 12; +const BodyFrictionTortion: BodyVars = 13; +const BodyFrictionRolling: BodyVars = 14; +const BodyPosX: BodyVars = 15; +const BodyPosY: BodyVars = 16; +const BodyPosZ: BodyVars = 17; +const BodyQuatX: BodyVars = 18; +const BodyQuatY: BodyVars = 19; +const BodyQuatZ: BodyVars = 20; +const BodyQuatW: BodyVars = 21; +const BodyComX: BodyVars = 22; +const BodyComY: BodyVars = 23; +const BodyComZ: BodyVars = 24; +const BodyInertiaXX: BodyVars = 25; +const BodyInertiaYX: BodyVars = 26; +const BodyInertiaZX: BodyVars = 27; +const BodyInertiaXY: BodyVars = 28; +const BodyInertiaYY: BodyVars = 29; +const BodyInertiaZY: BodyVars = 30; +const BodyInertiaXZ: BodyVars = 31; +const BodyInertiaYZ: BodyVars = 32; +const BodyInertiaZZ: BodyVars = 33; +const BodyInvInertiaXX: BodyVars = 34; +const BodyInvInertiaYX: BodyVars = 35; +const BodyInvInertiaZX: BodyVars = 36; +const BodyInvInertiaXY: BodyVars = 37; +const BodyInvInertiaYY: BodyVars = 38; +const BodyInvInertiaZY: BodyVars = 39; +const BodyInvInertiaXZ: BodyVars = 40; +const BodyInvInertiaYZ: BodyVars = 41; +const BodyInvInertiaZZ: BodyVars = 42; +const BodyRadius: BodyVars = 43; fn BodyCom(idx: i32) -> vec3 { return vec3(Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComX))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComY))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComZ))]); } diff --git a/physics/shaders/StepBodyContacts.wgsl b/physics/shaders/StepBodyContacts.wgsl index 7793471c..c9aa9a76 100644 --- a/physics/shaders/StepBodyContacts.wgsl +++ b/physics/shaders/StepBodyContacts.wgsl @@ -47,47 +47,49 @@ const BodyShape: BodyVars = 0; const BodyDynamic: BodyVars = 1; const BodyWorld: BodyVars = 2; const BodyGroup: BodyVars = 3; -const BodyHSizeX: BodyVars = 4; -const BodyHSizeY: BodyVars = 5; -const BodyHSizeZ: BodyVars = 6; -const BodyThick: BodyVars = 7; -const BodyMass: BodyVars = 8; -const BodyInvMass: BodyVars = 9; -const BodyBounce: BodyVars = 10; -const BodyFriction: BodyVars = 11; -const BodyFrictionTortion: BodyVars = 12; -const BodyFrictionRolling: BodyVars = 13; -const BodyPosX: BodyVars = 14; -const BodyPosY: BodyVars = 15; -const BodyPosZ: BodyVars = 16; -const BodyQuatX: BodyVars = 17; -const BodyQuatY: BodyVars = 18; -const BodyQuatZ: BodyVars = 19; -const BodyQuatW: BodyVars = 20; -const BodyComX: BodyVars = 21; -const BodyComY: BodyVars = 22; -const BodyComZ: BodyVars = 23; -const BodyInertiaXX: BodyVars = 24; -const BodyInertiaYX: BodyVars = 25; -const BodyInertiaZX: BodyVars = 26; -const BodyInertiaXY: BodyVars = 27; -const BodyInertiaYY: BodyVars = 28; -const BodyInertiaZY: BodyVars = 29; -const BodyInertiaXZ: BodyVars = 30; -const BodyInertiaYZ: BodyVars = 31; -const BodyInertiaZZ: BodyVars = 32; -const BodyInvInertiaXX: BodyVars = 33; -const BodyInvInertiaYX: BodyVars = 34; -const BodyInvInertiaZX: BodyVars = 35; -const BodyInvInertiaXY: BodyVars = 36; -const BodyInvInertiaYY: BodyVars = 37; -const BodyInvInertiaZY: BodyVars = 38; -const BodyInvInertiaXZ: BodyVars = 39; -const BodyInvInertiaYZ: BodyVars = 40; -const BodyInvInertiaZZ: BodyVars = 41; -const BodyRadius: BodyVars = 42; +const BodyObject: BodyVars = 4; +const BodyHSizeX: BodyVars = 5; +const BodyHSizeY: BodyVars = 6; +const BodyHSizeZ: BodyVars = 7; +const BodyThick: BodyVars = 8; +const BodyMass: BodyVars = 9; +const BodyInvMass: BodyVars = 10; +const BodyBounce: BodyVars = 11; +const BodyFriction: BodyVars = 12; +const BodyFrictionTortion: BodyVars = 13; +const BodyFrictionRolling: BodyVars = 14; +const BodyPosX: BodyVars = 15; +const BodyPosY: BodyVars = 16; +const BodyPosZ: BodyVars = 17; +const BodyQuatX: BodyVars = 18; +const BodyQuatY: BodyVars = 19; +const BodyQuatZ: BodyVars = 20; +const BodyQuatW: BodyVars = 21; +const BodyComX: BodyVars = 22; +const BodyComY: BodyVars = 23; +const BodyComZ: BodyVars = 24; +const BodyInertiaXX: BodyVars = 25; +const BodyInertiaYX: BodyVars = 26; +const BodyInertiaZX: BodyVars = 27; +const BodyInertiaXY: BodyVars = 28; +const BodyInertiaYY: BodyVars = 29; +const BodyInertiaZY: BodyVars = 30; +const BodyInertiaXZ: BodyVars = 31; +const BodyInertiaYZ: BodyVars = 32; +const BodyInertiaZZ: BodyVars = 33; +const BodyInvInertiaXX: BodyVars = 34; +const BodyInvInertiaYX: BodyVars = 35; +const BodyInvInertiaZX: BodyVars = 36; +const BodyInvInertiaXY: BodyVars = 37; +const BodyInvInertiaYY: BodyVars = 38; +const BodyInvInertiaZY: BodyVars = 39; +const BodyInvInertiaXZ: BodyVars = 40; +const BodyInvInertiaYZ: BodyVars = 41; +const BodyInvInertiaZZ: BodyVars = 42; +const BodyRadius: BodyVars = 43; fn GetBodyDynamic(idx: i32) -> i32 { - return i32(bitcast(Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyDynamic))])); + return i32(bitcast(Bodies[Index2D(TensorStrides[0], TensorStrides[1], + u32(idx), u32(BodyDynamic))])); } fn BodyPos(idx: i32) -> vec3 { return vec3(Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyPosX))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyPosY))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyPosZ))]); diff --git a/physics/shaders/StepBodyJointDeltas.wgsl b/physics/shaders/StepBodyJointDeltas.wgsl index cfbc6de1..ee127e1d 100644 --- a/physics/shaders/StepBodyJointDeltas.wgsl +++ b/physics/shaders/StepBodyJointDeltas.wgsl @@ -43,45 +43,46 @@ const BodyShape: BodyVars = 0; const BodyDynamic: BodyVars = 1; const BodyWorld: BodyVars = 2; const BodyGroup: BodyVars = 3; -const BodyHSizeX: BodyVars = 4; -const BodyHSizeY: BodyVars = 5; -const BodyHSizeZ: BodyVars = 6; -const BodyThick: BodyVars = 7; -const BodyMass: BodyVars = 8; -const BodyInvMass: BodyVars = 9; -const BodyBounce: BodyVars = 10; -const BodyFriction: BodyVars = 11; -const BodyFrictionTortion: BodyVars = 12; -const BodyFrictionRolling: BodyVars = 13; -const BodyPosX: BodyVars = 14; -const BodyPosY: BodyVars = 15; -const BodyPosZ: BodyVars = 16; -const BodyQuatX: BodyVars = 17; -const BodyQuatY: BodyVars = 18; -const BodyQuatZ: BodyVars = 19; -const BodyQuatW: BodyVars = 20; -const BodyComX: BodyVars = 21; -const BodyComY: BodyVars = 22; -const BodyComZ: BodyVars = 23; -const BodyInertiaXX: BodyVars = 24; -const BodyInertiaYX: BodyVars = 25; -const BodyInertiaZX: BodyVars = 26; -const BodyInertiaXY: BodyVars = 27; -const BodyInertiaYY: BodyVars = 28; -const BodyInertiaZY: BodyVars = 29; -const BodyInertiaXZ: BodyVars = 30; -const BodyInertiaYZ: BodyVars = 31; -const BodyInertiaZZ: BodyVars = 32; -const BodyInvInertiaXX: BodyVars = 33; -const BodyInvInertiaYX: BodyVars = 34; -const BodyInvInertiaZX: BodyVars = 35; -const BodyInvInertiaXY: BodyVars = 36; -const BodyInvInertiaYY: BodyVars = 37; -const BodyInvInertiaZY: BodyVars = 38; -const BodyInvInertiaXZ: BodyVars = 39; -const BodyInvInertiaYZ: BodyVars = 40; -const BodyInvInertiaZZ: BodyVars = 41; -const BodyRadius: BodyVars = 42; +const BodyObject: BodyVars = 4; +const BodyHSizeX: BodyVars = 5; +const BodyHSizeY: BodyVars = 6; +const BodyHSizeZ: BodyVars = 7; +const BodyThick: BodyVars = 8; +const BodyMass: BodyVars = 9; +const BodyInvMass: BodyVars = 10; +const BodyBounce: BodyVars = 11; +const BodyFriction: BodyVars = 12; +const BodyFrictionTortion: BodyVars = 13; +const BodyFrictionRolling: BodyVars = 14; +const BodyPosX: BodyVars = 15; +const BodyPosY: BodyVars = 16; +const BodyPosZ: BodyVars = 17; +const BodyQuatX: BodyVars = 18; +const BodyQuatY: BodyVars = 19; +const BodyQuatZ: BodyVars = 20; +const BodyQuatW: BodyVars = 21; +const BodyComX: BodyVars = 22; +const BodyComY: BodyVars = 23; +const BodyComZ: BodyVars = 24; +const BodyInertiaXX: BodyVars = 25; +const BodyInertiaYX: BodyVars = 26; +const BodyInertiaZX: BodyVars = 27; +const BodyInertiaXY: BodyVars = 28; +const BodyInertiaYY: BodyVars = 29; +const BodyInertiaZY: BodyVars = 30; +const BodyInertiaXZ: BodyVars = 31; +const BodyInertiaYZ: BodyVars = 32; +const BodyInertiaZZ: BodyVars = 33; +const BodyInvInertiaXX: BodyVars = 34; +const BodyInvInertiaYX: BodyVars = 35; +const BodyInvInertiaZX: BodyVars = 36; +const BodyInvInertiaXY: BodyVars = 37; +const BodyInvInertiaYY: BodyVars = 38; +const BodyInvInertiaZY: BodyVars = 39; +const BodyInvInertiaXZ: BodyVars = 40; +const BodyInvInertiaYZ: BodyVars = 41; +const BodyInvInertiaZZ: BodyVars = 42; +const BodyRadius: BodyVars = 43; fn BodyCom(idx: i32) -> vec3 { return vec3(Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComX))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComY))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComZ))]); } diff --git a/physics/shaders/StepIntegrateBodies.wgsl b/physics/shaders/StepIntegrateBodies.wgsl index f006940e..73c91053 100644 --- a/physics/shaders/StepIntegrateBodies.wgsl +++ b/physics/shaders/StepIntegrateBodies.wgsl @@ -39,45 +39,46 @@ const BodyShape: BodyVars = 0; const BodyDynamic: BodyVars = 1; const BodyWorld: BodyVars = 2; const BodyGroup: BodyVars = 3; -const BodyHSizeX: BodyVars = 4; -const BodyHSizeY: BodyVars = 5; -const BodyHSizeZ: BodyVars = 6; -const BodyThick: BodyVars = 7; -const BodyMass: BodyVars = 8; -const BodyInvMass: BodyVars = 9; -const BodyBounce: BodyVars = 10; -const BodyFriction: BodyVars = 11; -const BodyFrictionTortion: BodyVars = 12; -const BodyFrictionRolling: BodyVars = 13; -const BodyPosX: BodyVars = 14; -const BodyPosY: BodyVars = 15; -const BodyPosZ: BodyVars = 16; -const BodyQuatX: BodyVars = 17; -const BodyQuatY: BodyVars = 18; -const BodyQuatZ: BodyVars = 19; -const BodyQuatW: BodyVars = 20; -const BodyComX: BodyVars = 21; -const BodyComY: BodyVars = 22; -const BodyComZ: BodyVars = 23; -const BodyInertiaXX: BodyVars = 24; -const BodyInertiaYX: BodyVars = 25; -const BodyInertiaZX: BodyVars = 26; -const BodyInertiaXY: BodyVars = 27; -const BodyInertiaYY: BodyVars = 28; -const BodyInertiaZY: BodyVars = 29; -const BodyInertiaXZ: BodyVars = 30; -const BodyInertiaYZ: BodyVars = 31; -const BodyInertiaZZ: BodyVars = 32; -const BodyInvInertiaXX: BodyVars = 33; -const BodyInvInertiaYX: BodyVars = 34; -const BodyInvInertiaZX: BodyVars = 35; -const BodyInvInertiaXY: BodyVars = 36; -const BodyInvInertiaYY: BodyVars = 37; -const BodyInvInertiaZY: BodyVars = 38; -const BodyInvInertiaXZ: BodyVars = 39; -const BodyInvInertiaYZ: BodyVars = 40; -const BodyInvInertiaZZ: BodyVars = 41; -const BodyRadius: BodyVars = 42; +const BodyObject: BodyVars = 4; +const BodyHSizeX: BodyVars = 5; +const BodyHSizeY: BodyVars = 6; +const BodyHSizeZ: BodyVars = 7; +const BodyThick: BodyVars = 8; +const BodyMass: BodyVars = 9; +const BodyInvMass: BodyVars = 10; +const BodyBounce: BodyVars = 11; +const BodyFriction: BodyVars = 12; +const BodyFrictionTortion: BodyVars = 13; +const BodyFrictionRolling: BodyVars = 14; +const BodyPosX: BodyVars = 15; +const BodyPosY: BodyVars = 16; +const BodyPosZ: BodyVars = 17; +const BodyQuatX: BodyVars = 18; +const BodyQuatY: BodyVars = 19; +const BodyQuatZ: BodyVars = 20; +const BodyQuatW: BodyVars = 21; +const BodyComX: BodyVars = 22; +const BodyComY: BodyVars = 23; +const BodyComZ: BodyVars = 24; +const BodyInertiaXX: BodyVars = 25; +const BodyInertiaYX: BodyVars = 26; +const BodyInertiaZX: BodyVars = 27; +const BodyInertiaXY: BodyVars = 28; +const BodyInertiaYY: BodyVars = 29; +const BodyInertiaZY: BodyVars = 30; +const BodyInertiaXZ: BodyVars = 31; +const BodyInertiaYZ: BodyVars = 32; +const BodyInertiaZZ: BodyVars = 33; +const BodyInvInertiaXX: BodyVars = 34; +const BodyInvInertiaYX: BodyVars = 35; +const BodyInvInertiaZX: BodyVars = 36; +const BodyInvInertiaXY: BodyVars = 37; +const BodyInvInertiaYY: BodyVars = 38; +const BodyInvInertiaZY: BodyVars = 39; +const BodyInvInertiaXZ: BodyVars = 40; +const BodyInvInertiaYZ: BodyVars = 41; +const BodyInvInertiaZZ: BodyVars = 42; +const BodyRadius: BodyVars = 43; fn BodyCom(idx: i32) -> vec3 { return vec3(Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComX))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComY))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComZ))]); } diff --git a/physics/shaders/StepJointForces.wgsl b/physics/shaders/StepJointForces.wgsl index 4e9ab597..455a404c 100644 --- a/physics/shaders/StepJointForces.wgsl +++ b/physics/shaders/StepJointForces.wgsl @@ -45,45 +45,46 @@ const BodyShape: BodyVars = 0; const BodyDynamic: BodyVars = 1; const BodyWorld: BodyVars = 2; const BodyGroup: BodyVars = 3; -const BodyHSizeX: BodyVars = 4; -const BodyHSizeY: BodyVars = 5; -const BodyHSizeZ: BodyVars = 6; -const BodyThick: BodyVars = 7; -const BodyMass: BodyVars = 8; -const BodyInvMass: BodyVars = 9; -const BodyBounce: BodyVars = 10; -const BodyFriction: BodyVars = 11; -const BodyFrictionTortion: BodyVars = 12; -const BodyFrictionRolling: BodyVars = 13; -const BodyPosX: BodyVars = 14; -const BodyPosY: BodyVars = 15; -const BodyPosZ: BodyVars = 16; -const BodyQuatX: BodyVars = 17; -const BodyQuatY: BodyVars = 18; -const BodyQuatZ: BodyVars = 19; -const BodyQuatW: BodyVars = 20; -const BodyComX: BodyVars = 21; -const BodyComY: BodyVars = 22; -const BodyComZ: BodyVars = 23; -const BodyInertiaXX: BodyVars = 24; -const BodyInertiaYX: BodyVars = 25; -const BodyInertiaZX: BodyVars = 26; -const BodyInertiaXY: BodyVars = 27; -const BodyInertiaYY: BodyVars = 28; -const BodyInertiaZY: BodyVars = 29; -const BodyInertiaXZ: BodyVars = 30; -const BodyInertiaYZ: BodyVars = 31; -const BodyInertiaZZ: BodyVars = 32; -const BodyInvInertiaXX: BodyVars = 33; -const BodyInvInertiaYX: BodyVars = 34; -const BodyInvInertiaZX: BodyVars = 35; -const BodyInvInertiaXY: BodyVars = 36; -const BodyInvInertiaYY: BodyVars = 37; -const BodyInvInertiaZY: BodyVars = 38; -const BodyInvInertiaXZ: BodyVars = 39; -const BodyInvInertiaYZ: BodyVars = 40; -const BodyInvInertiaZZ: BodyVars = 41; -const BodyRadius: BodyVars = 42; +const BodyObject: BodyVars = 4; +const BodyHSizeX: BodyVars = 5; +const BodyHSizeY: BodyVars = 6; +const BodyHSizeZ: BodyVars = 7; +const BodyThick: BodyVars = 8; +const BodyMass: BodyVars = 9; +const BodyInvMass: BodyVars = 10; +const BodyBounce: BodyVars = 11; +const BodyFriction: BodyVars = 12; +const BodyFrictionTortion: BodyVars = 13; +const BodyFrictionRolling: BodyVars = 14; +const BodyPosX: BodyVars = 15; +const BodyPosY: BodyVars = 16; +const BodyPosZ: BodyVars = 17; +const BodyQuatX: BodyVars = 18; +const BodyQuatY: BodyVars = 19; +const BodyQuatZ: BodyVars = 20; +const BodyQuatW: BodyVars = 21; +const BodyComX: BodyVars = 22; +const BodyComY: BodyVars = 23; +const BodyComZ: BodyVars = 24; +const BodyInertiaXX: BodyVars = 25; +const BodyInertiaYX: BodyVars = 26; +const BodyInertiaZX: BodyVars = 27; +const BodyInertiaXY: BodyVars = 28; +const BodyInertiaYY: BodyVars = 29; +const BodyInertiaZY: BodyVars = 30; +const BodyInertiaXZ: BodyVars = 31; +const BodyInertiaYZ: BodyVars = 32; +const BodyInertiaZZ: BodyVars = 33; +const BodyInvInertiaXX: BodyVars = 34; +const BodyInvInertiaYX: BodyVars = 35; +const BodyInvInertiaZX: BodyVars = 36; +const BodyInvInertiaXY: BodyVars = 37; +const BodyInvInertiaYY: BodyVars = 38; +const BodyInvInertiaZY: BodyVars = 39; +const BodyInvInertiaXZ: BodyVars = 40; +const BodyInvInertiaYZ: BodyVars = 41; +const BodyInvInertiaZZ: BodyVars = 42; +const BodyRadius: BodyVars = 43; fn BodyCom(idx: i32) -> vec3 { return vec3(Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComX))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComY))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComZ))]); } diff --git a/physics/shaders/StepSolveJoints.wgsl b/physics/shaders/StepSolveJoints.wgsl index 6967f1e4..f84de152 100644 --- a/physics/shaders/StepSolveJoints.wgsl +++ b/physics/shaders/StepSolveJoints.wgsl @@ -45,45 +45,46 @@ const BodyShape: BodyVars = 0; const BodyDynamic: BodyVars = 1; const BodyWorld: BodyVars = 2; const BodyGroup: BodyVars = 3; -const BodyHSizeX: BodyVars = 4; -const BodyHSizeY: BodyVars = 5; -const BodyHSizeZ: BodyVars = 6; -const BodyThick: BodyVars = 7; -const BodyMass: BodyVars = 8; -const BodyInvMass: BodyVars = 9; -const BodyBounce: BodyVars = 10; -const BodyFriction: BodyVars = 11; -const BodyFrictionTortion: BodyVars = 12; -const BodyFrictionRolling: BodyVars = 13; -const BodyPosX: BodyVars = 14; -const BodyPosY: BodyVars = 15; -const BodyPosZ: BodyVars = 16; -const BodyQuatX: BodyVars = 17; -const BodyQuatY: BodyVars = 18; -const BodyQuatZ: BodyVars = 19; -const BodyQuatW: BodyVars = 20; -const BodyComX: BodyVars = 21; -const BodyComY: BodyVars = 22; -const BodyComZ: BodyVars = 23; -const BodyInertiaXX: BodyVars = 24; -const BodyInertiaYX: BodyVars = 25; -const BodyInertiaZX: BodyVars = 26; -const BodyInertiaXY: BodyVars = 27; -const BodyInertiaYY: BodyVars = 28; -const BodyInertiaZY: BodyVars = 29; -const BodyInertiaXZ: BodyVars = 30; -const BodyInertiaYZ: BodyVars = 31; -const BodyInertiaZZ: BodyVars = 32; -const BodyInvInertiaXX: BodyVars = 33; -const BodyInvInertiaYX: BodyVars = 34; -const BodyInvInertiaZX: BodyVars = 35; -const BodyInvInertiaXY: BodyVars = 36; -const BodyInvInertiaYY: BodyVars = 37; -const BodyInvInertiaZY: BodyVars = 38; -const BodyInvInertiaXZ: BodyVars = 39; -const BodyInvInertiaYZ: BodyVars = 40; -const BodyInvInertiaZZ: BodyVars = 41; -const BodyRadius: BodyVars = 42; +const BodyObject: BodyVars = 4; +const BodyHSizeX: BodyVars = 5; +const BodyHSizeY: BodyVars = 6; +const BodyHSizeZ: BodyVars = 7; +const BodyThick: BodyVars = 8; +const BodyMass: BodyVars = 9; +const BodyInvMass: BodyVars = 10; +const BodyBounce: BodyVars = 11; +const BodyFriction: BodyVars = 12; +const BodyFrictionTortion: BodyVars = 13; +const BodyFrictionRolling: BodyVars = 14; +const BodyPosX: BodyVars = 15; +const BodyPosY: BodyVars = 16; +const BodyPosZ: BodyVars = 17; +const BodyQuatX: BodyVars = 18; +const BodyQuatY: BodyVars = 19; +const BodyQuatZ: BodyVars = 20; +const BodyQuatW: BodyVars = 21; +const BodyComX: BodyVars = 22; +const BodyComY: BodyVars = 23; +const BodyComZ: BodyVars = 24; +const BodyInertiaXX: BodyVars = 25; +const BodyInertiaYX: BodyVars = 26; +const BodyInertiaZX: BodyVars = 27; +const BodyInertiaXY: BodyVars = 28; +const BodyInertiaYY: BodyVars = 29; +const BodyInertiaZY: BodyVars = 30; +const BodyInertiaXZ: BodyVars = 31; +const BodyInertiaYZ: BodyVars = 32; +const BodyInertiaZZ: BodyVars = 33; +const BodyInvInertiaXX: BodyVars = 34; +const BodyInvInertiaYX: BodyVars = 35; +const BodyInvInertiaZX: BodyVars = 36; +const BodyInvInertiaXY: BodyVars = 37; +const BodyInvInertiaYY: BodyVars = 38; +const BodyInvInertiaZY: BodyVars = 39; +const BodyInvInertiaXZ: BodyVars = 40; +const BodyInvInertiaYZ: BodyVars = 41; +const BodyInvInertiaZZ: BodyVars = 42; +const BodyRadius: BodyVars = 43; fn BodyCom(idx: i32) -> vec3 { return vec3(Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComX))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComY))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComZ))]); } diff --git a/physics/typegen.go b/physics/typegen.go index ae0b9953..80452820 100644 --- a/physics/typegen.go +++ b/physics/typegen.go @@ -24,7 +24,7 @@ var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.JointVars", var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.JointDoFVars", IDName: "joint-do-f-vars", Doc: "JointDoFVars are joint DoF state variables stored in tensor.Float32,\none for each DoF."}) -var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.PhysParams", IDName: "phys-params", Doc: "PhysParams are the physics parameters", Directives: []types.Directive{{Tool: "gosl", Directive: "start"}}, Fields: []types.Field{{Name: "Iterations", Doc: "Iterations is the number of integration iterations to perform\nwithin each solver step. Muller et al (2020) report that 1 is best."}, {Name: "Dt", Doc: "Dt is the integration stepsize.\nFor highly kinetic situations (e.g., rapidly moving bouncing balls)\n0.0001 is needed to ensure contact registration. Use SubSteps to\naccomplish a target effective read-out step size."}, {Name: "SubSteps", Doc: "SubSteps is the number of integration steps to take per Step()\nfunction call. These sub steps are taken without any sync to/from\nthe GPU and are therefore much faster."}, {Name: "ContactMargin", Doc: "Contact margin is the extra distance for broadphase collision\naround rigid bodies."}, {Name: "ContactRelax", Doc: "ContactRelax is rigid contact relaxation constant.\nHigher values cause errros"}, {Name: "ContactWeighting", Doc: "Contact weighting: balances contact forces?"}, {Name: "Restitution", Doc: "Restitution takes into account bounciness of objects."}, {Name: "JointLinearRelax", Doc: "JointLinearRelax is joint linear relaxation constant."}, {Name: "JointAngularRelax", Doc: "JointAngularRelax is joint angular relaxation constant."}, {Name: "JointLinearComply", Doc: "JointLinearComply is joint linear compliance constant."}, {Name: "JointAngularComply", Doc: "JointAngularComply is joint angular compliance constant."}, {Name: "AngularDamping", Doc: "AngularDamping is damping of angular motion."}, {Name: "SoftRelax", Doc: "SoftRelax is soft-body relaxation constant."}, {Name: "MaxGeomIter", Doc: "MaxGeomIter is number of iterations to perform in shape-based\ngeometry collision computations"}, {Name: "ContactsMax", Doc: "Maximum number of contacts to process at any given point."}, {Name: "Cur", Doc: "Index for the current state (0 or 1, alternates with Next)."}, {Name: "Next", Doc: "Index for the next state (1 or 0, alternates with Cur)."}, {Name: "BodiesN", Doc: "BodiesN is number of rigid bodies."}, {Name: "DynamicsN", Doc: "DynamicsN is number of dynamics bodies."}, {Name: "JointsN", Doc: "JointsN is number of joints."}, {Name: "JointDoFsN", Doc: "JointDoFsN is number of joint DoFs."}, {Name: "BodyJointsMax", Doc: "BodyJointsMax is max number of joints per body + 1 for actual n."}, {Name: "BodyCollidePairsN", Doc: "BodyCollidePairsN is the total number of pre-compiled collision pairs\nto examine."}, {Name: "pad"}, {Name: "Gravity", Doc: "Gravity is the gravity acceleration function"}}}) +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.PhysParams", IDName: "phys-params", Doc: "PhysParams are the physics parameters", Directives: []types.Directive{{Tool: "gosl", Directive: "start"}}, Fields: []types.Field{{Name: "Iterations", Doc: "Iterations is the number of integration iterations to perform\nwithin each solver step. Muller et al (2020) report that 1 is best."}, {Name: "Dt", Doc: "Dt is the integration stepsize.\nFor highly kinetic situations (e.g., rapidly moving bouncing balls)\n0.0001 is needed to ensure contact registration. Use SubSteps to\naccomplish a target effective read-out step size."}, {Name: "SubSteps", Doc: "SubSteps is the number of integration steps to take per Step()\nfunction call. These sub steps are taken without any sync to/from\nthe GPU and are therefore much faster."}, {Name: "ContactMargin", Doc: "Contact margin is the extra distance for broadphase collision\naround rigid bodies. This can make some joints potentially unstable if > 0"}, {Name: "ContactRelax", Doc: "ContactRelax is rigid contact relaxation constant.\nHigher values cause errros"}, {Name: "ContactWeighting", Doc: "Contact weighting: balances contact forces?"}, {Name: "Restitution", Doc: "Restitution takes into account bounciness of objects."}, {Name: "JointLinearRelax", Doc: "JointLinearRelax is joint linear relaxation constant."}, {Name: "JointAngularRelax", Doc: "JointAngularRelax is joint angular relaxation constant."}, {Name: "JointLinearComply", Doc: "JointLinearComply is joint linear compliance constant."}, {Name: "JointAngularComply", Doc: "JointAngularComply is joint angular compliance constant."}, {Name: "AngularDamping", Doc: "AngularDamping is damping of angular motion."}, {Name: "SoftRelax", Doc: "SoftRelax is soft-body relaxation constant."}, {Name: "MaxGeomIter", Doc: "MaxGeomIter is number of iterations to perform in shape-based\ngeometry collision computations"}, {Name: "ContactsMax", Doc: "Maximum number of contacts to process at any given point."}, {Name: "Cur", Doc: "Index for the current state (0 or 1, alternates with Next)."}, {Name: "Next", Doc: "Index for the next state (1 or 0, alternates with Cur)."}, {Name: "BodiesN", Doc: "BodiesN is number of rigid bodies."}, {Name: "DynamicsN", Doc: "DynamicsN is number of dynamics bodies."}, {Name: "JointsN", Doc: "JointsN is number of joints."}, {Name: "JointDoFsN", Doc: "JointDoFsN is number of joint DoFs."}, {Name: "BodyJointsMax", Doc: "BodyJointsMax is max number of joints per body + 1 for actual n."}, {Name: "BodyCollidePairsN", Doc: "BodyCollidePairsN is the total number of pre-compiled collision pairs\nto examine."}, {Name: "pad"}, {Name: "Gravity", Doc: "Gravity is the gravity acceleration function"}}}) var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.GeomData", IDName: "geom-data", Doc: "GeomData contains all geometric data for narrow-phase collision.", Directives: []types.Directive{{Tool: "gosl", Directive: "start"}}, Fields: []types.Field{{Name: "BodyIdx"}, {Name: "Shape"}, {Name: "MinSize", Doc: "MinSize is the min of the Size dimensions."}, {Name: "Thick", Doc: "Thickness of shape."}, {Name: "Radius", Doc: "Radius is the effective radius for sphere-like elements (Sphere, Capsule, Cone)"}, {Name: "Size"}, {Name: "WbR", Doc: "World-to-Body transform\nPosition (R) (i.e., BodyPos)"}, {Name: "WbQ", Doc: "Quaternion (Q) (i.e., BodyQuat)"}, {Name: "BwR", Doc: "Body-to-World transform (inverse)\nPosition (R)"}, {Name: "BwQ", Doc: "Quaternion (Q)"}}}) @@ -32,4 +32,4 @@ var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.Shapes", IDN var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.State", IDName: "state", Doc: "State contains the basic physical state including position, orientation, velocity.\nThese are only the values that can be either relative or absolute -- other physical\nstate values such as Mass should go in Rigid.", Methods: []types.Method{{Name: "MoveOnAxis", Doc: "MoveOnAxis moves (translates) the specified distance on the specified local axis,\nrelative to the current rotation orientation.\nThe axis is normalized prior to aplying the distance factor.\nSets the LinVel to motion vector.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z", "dist"}}, {Name: "MoveOnAxisAbs", Doc: "MoveOnAxisAbs moves (translates) the specified distance on the specified local axis,\nin absolute X,Y,Z coordinates (does not apply the Quat rotation factor.\nThe axis is normalized prior to aplying the distance factor.\nSets the LinVel to motion vector.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z", "dist"}}, {Name: "SetEulerRotation", Doc: "SetEulerRotation sets the rotation in Euler angles (degrees).", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z"}}, {Name: "EulerRotation", Doc: "EulerRotation returns the current rotation in Euler angles (degrees).", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Returns: []string{"Vector3"}}, {Name: "SetAxisRotation", Doc: "SetAxisRotation sets rotation from local axis and angle in degrees.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z", "angle"}}, {Name: "RotateOnAxis", Doc: "RotateOnAxis rotates around the specified local axis the specified angle in degrees.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z", "angle"}}, {Name: "RotateEuler", Doc: "RotateEuler rotates by given Euler angles (in degrees) relative to existing rotation.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z"}}}, Fields: []types.Field{{Name: "Pos", Doc: "Pos is the position of center of mass of object."}, {Name: "Quat", Doc: "Quat is the rotation specified as a quaternion."}, {Name: "LinVel", Doc: "LinVel is the linear velocity."}, {Name: "AngVel", Doc: "AngVel is the angular velocity."}}}) -var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.World", IDName: "world", Doc: "World contains and manages all of the physics elements.", Directives: []types.Directive{{Tool: "go", Directive: "generate", Args: []string{"core", "generate", "-add-types", "-gosl"}}}, Fields: []types.Field{{Name: "GPU", Doc: "GPU determines whether to use GPU (else CPU)."}, {Name: "Params", Doc: "Params are global parameters."}, {Name: "Bodies", Doc: "Bodies are the rigid body elements (dynamic and static),\nspecifying the constant, non-dynamic properties,\nwhich is initial state for dynamics.\n[body][BodyVarsN]"}, {Name: "Joints", Doc: "Joints is a list of permanent joints connecting bodies,\nwhich do not change (no dynamic variables).\n[joint][JointVarsN]"}, {Name: "JointDoFs", Doc: "JointDoFs is a list of joint DoF parameters, allocated per joint.\n[dof][JointDoFVars]"}, {Name: "BodyJoints", Doc: "BodyJoints is a list of joint indexes for each dynamic body, for aggregating.\n[dyn body][parent, child][Params.BodyJointsMax]"}, {Name: "BodyCollidePairs", Doc: "BodyCollidePairs are pairs of Body indexes that could potentially collide\nbased on precomputed collision logic, using World, Group, and Joint indexes.\n[BodyCollidePairsN][2]"}, {Name: "Dynamics", Doc: "Dynamics are the dynamic rigid body elements: these actually move.\nThe first set of variables are for initial values, and the second current.\n[body][cur/next][DynamicVarsN]"}, {Name: "BroadContactsN", Doc: "BroadContactsN has number of points of broad contact\nbetween bodies. [1]"}, {Name: "BroadContacts", Doc: "BroadContacts are the results of broad-phase contact processing,\nestablishing possible points of contact between bodies.\n[ContactsMax][BroadContactVarsN]"}, {Name: "ContactsN", Doc: "ContactsN has number of points of narrow (final) contact\nbetween bodies. [1]"}, {Name: "Contacts", Doc: "Contacts are the results of narrow-phase contact processing,\nwhere only actual contacts with fully-specified values are present.\n[ContactsMax][ContactVarsN]"}, {Name: "JointControls", Doc: "JointControls are dynamic joint control inputs, per joint DoF\n(in correspondence with [JointDoFs]). This can be uploaded to the\nGPU at every step.\n[dof][JointControlVarsN]"}}}) +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.World", IDName: "world", Doc: "World contains and manages all of the physics elements.", Directives: []types.Directive{{Tool: "go", Directive: "generate", Args: []string{"core", "generate", "-add-types", "-gosl"}}}, Fields: []types.Field{{Name: "GPU", Doc: "GPU determines whether to use GPU (else CPU)."}, {Name: "Params", Doc: "Params are global parameters."}, {Name: "CurrentWorld", Doc: "CurrentWorld is the [BodyWorld] value to use when creating new bodies.\nSet to -1 to create global elements that interact with everything,\nwhile 0 and positive numbers only interact amongst themselves."}, {Name: "WorldReplicasStart", Doc: "WorldReplicasStart is the starting body index for replicated world bodies,\nwhich is needed for viewers to efficiently select a specific world to view.\nThis is the start of the World=0 first instance."}, {Name: "WorldReplicasN", Doc: "WorldReplicasN is the number of body elements within each set of\nreplicated world bodies, which is needed for viewers to efficiently select\na specific world to view."}, {Name: "CurrentObject", Doc: "CurrentObject is the [BodyObject] value to use when creating new bodies.\nGenerally just increment when starting a new object. When using world replicas,\nthe object indexes are copied, so objects are indexed by world and object id."}, {Name: "Bodies", Doc: "Bodies are the rigid body elements (dynamic and static),\nspecifying the constant, non-dynamic properties,\nwhich is initial state for dynamics.\n[body][BodyVarsN]"}, {Name: "Joints", Doc: "Joints is a list of permanent joints connecting bodies,\nwhich do not change (no dynamic variables).\n[joint][JointVarsN]"}, {Name: "JointDoFs", Doc: "JointDoFs is a list of joint DoF parameters, allocated per joint.\n[dof][JointDoFVars]"}, {Name: "BodyJoints", Doc: "BodyJoints is a list of joint indexes for each dynamic body, for aggregating.\n[dyn body][parent, child][Params.BodyJointsMax]"}, {Name: "BodyCollidePairs", Doc: "BodyCollidePairs are pairs of Body indexes that could potentially collide\nbased on precomputed collision logic, using World, Group, and Joint indexes.\n[BodyCollidePairsN][2]"}, {Name: "Dynamics", Doc: "Dynamics are the dynamic rigid body elements: these actually move.\nThe first set of variables are for initial values, and the second current.\n[body][cur/next][DynamicVarsN]"}, {Name: "BroadContactsN", Doc: "BroadContactsN has number of points of broad contact\nbetween bodies. [1]"}, {Name: "BroadContacts", Doc: "BroadContacts are the results of broad-phase contact processing,\nestablishing possible points of contact between bodies.\n[ContactsMax][BroadContactVarsN]"}, {Name: "ContactsN", Doc: "ContactsN has number of points of narrow (final) contact\nbetween bodies. [1]"}, {Name: "Contacts", Doc: "Contacts are the results of narrow-phase contact processing,\nwhere only actual contacts with fully-specified values are present.\n[ContactsMax][ContactVarsN]"}, {Name: "JointControls", Doc: "JointControls are dynamic joint control inputs, per joint DoF\n(in correspondence with [JointDoFs]). This can be uploaded to the\nGPU at every step.\n[dof][JointControlVarsN]"}}}) diff --git a/physics/world.go b/physics/world.go index 19569b52..e1f8cf48 100644 --- a/physics/world.go +++ b/physics/world.go @@ -21,6 +21,26 @@ type World struct { // Params are global parameters. Params []PhysParams + // CurrentWorld is the [BodyWorld] value to use when creating new bodies. + // Set to -1 to create global elements that interact with everything, + // while 0 and positive numbers only interact amongst themselves. + CurrentWorld int + + // WorldReplicasStart is the starting body index for replicated world bodies, + // which is needed for viewers to efficiently select a specific world to view. + // This is the start of the World=0 first instance. + WorldReplicasStart int32 + + // WorldReplicasN is the number of body elements within each set of + // replicated world bodies, which is needed for viewers to efficiently select + // a specific world to view. + WorldReplicasN int32 + + // CurrentObject is the [BodyObject] value to use when creating new bodies. + // Generally just increment when starting a new object. When using world replicas, + // the object indexes are copied, so objects are indexed by world and object id. + CurrentObject int + // Bodies are the rigid body elements (dynamic and static), // specifying the constant, non-dynamic properties, // which is initial state for dynamics. @@ -76,68 +96,70 @@ type World struct { } func NewWorld() *World { - wl := &World{} - wl.Init() - return wl + ph := &World{} + ph.Init() + return ph } // Init makes initial vars. Called in NewWorld. // Must call Config once configured. -func (wl *World) Init() { - wl.GPU = true - wl.Params = make([]PhysParams, 1) - wl.Params[0].Defaults() - wl.Reset() +func (ph *World) Init() { + ph.GPU = true + ph.Params = make([]PhysParams, 1) + ph.Params[0].Defaults() + ph.Reset() } // Reset resets all data to empty: starting over. -func (wl *World) Reset() { - wl.Bodies = tensor.NewFloat32(0, int(BodyVarsN)) - wl.Joints = tensor.NewFloat32(0, int(JointVarsN)) - wl.JointDoFs = tensor.NewFloat32(0, int(JointDoFVarsN)) - wl.BodyJoints = tensor.NewInt32(0, 2, 2) - wl.BodyCollidePairs = tensor.NewInt32(0, 2) - wl.Dynamics = tensor.NewFloat32(0, 2, int(DynamicVarsN)) - wl.BroadContactsN = tensor.NewInt32(1) - wl.BroadContacts = tensor.NewFloat32(0, int(ContactVarsN)) - wl.ContactsN = tensor.NewInt32(1) - wl.Contacts = tensor.NewFloat32(0, int(ContactVarsN)) - wl.JointControls = tensor.NewFloat32(0, int(JointControlVarsN)) - wl.SetAsCurrentVars() +func (ph *World) Reset() { + ph.Bodies = tensor.NewFloat32(0, int(BodyVarsN)) + ph.Joints = tensor.NewFloat32(0, int(JointVarsN)) + ph.JointDoFs = tensor.NewFloat32(0, int(JointDoFVarsN)) + ph.BodyJoints = tensor.NewInt32(0, 2, 2) + ph.BodyCollidePairs = tensor.NewInt32(0, 2) + ph.Dynamics = tensor.NewFloat32(0, 2, int(DynamicVarsN)) + ph.BroadContactsN = tensor.NewInt32(1) + ph.BroadContacts = tensor.NewFloat32(0, int(ContactVarsN)) + ph.ContactsN = tensor.NewInt32(1) + ph.Contacts = tensor.NewFloat32(0, int(ContactVarsN)) + ph.JointControls = tensor.NewFloat32(0, int(JointControlVarsN)) + ph.SetAsCurrentVars() } // NewBody adds a new body with given parameters. Returns the index. // Use this for Static elements; NewDynamic for dynamic elements. -func (wl *World) NewBody(shape Shapes, size, pos math32.Vector3, rot math32.Quat) int32 { - sizes := wl.Bodies.ShapeSizes() +func (ph *World) NewBody(shape Shapes, size, pos math32.Vector3, rot math32.Quat) int32 { + sizes := ph.Bodies.ShapeSizes() idx := int32(sizes[0]) - wl.Bodies.SetShapeSizes(int(idx+1), int(BodyVarsN)) - wl.Params[0].BodiesN = idx + 1 + ph.Bodies.SetShapeSizes(int(idx+1), int(BodyVarsN)) + ph.Params[0].BodiesN = idx + 1 SetBodyShape(idx, shape) SetBodyDynamic(idx, -1) SetBodyHSize(idx, size) SetBodyPos(idx, pos) SetBodyQuat(idx, rot) - SetBodyGroup(idx, -1) // assume static - wl.SetMass(idx, shape, size, 0) // assume static + SetBodyGroup(idx, -1) // assume static + SetBodyWorld(idx, int32(ph.CurrentWorld)) + SetBodyObject(idx, int32(ph.CurrentObject)) + ph.SetMass(idx, shape, size, 0) // assume static return idx } // NewDynamic adds a new dynamic body with given parameters. Returns the index. // Shape cannot be [Plane]. -func (wl *World) NewDynamic(shape Shapes, mass float32, size, pos math32.Vector3, rot math32.Quat) (bodyIdx, dynIdx int32) { +func (ph *World) NewDynamic(shape Shapes, mass float32, size, pos math32.Vector3, rot math32.Quat) (bodyIdx, dynIdx int32) { if shape == Plane { panic("physics.NewDynamic: shape cannot be Plane") } - bodyIdx = wl.NewBody(shape, size, pos, rot) - sizes := wl.Dynamics.ShapeSizes() + bodyIdx = ph.NewBody(shape, size, pos, rot) + sizes := ph.Dynamics.ShapeSizes() dynIdx = int32(sizes[0]) - wl.Dynamics.SetShapeSizes(int(dynIdx+1), 2, int(DynamicVarsN)) - wl.Params[0].DynamicsN = dynIdx + 1 + ph.Dynamics.SetShapeSizes(int(dynIdx+1), 2, int(DynamicVarsN)) + ph.Params[0].DynamicsN = dynIdx + 1 SetDynamicBody(dynIdx, bodyIdx) SetBodyDynamic(bodyIdx, dynIdx) SetBodyGroup(bodyIdx, 1) // dynamic - wl.SetMass(bodyIdx, shape, size, mass) + ph.SetMass(bodyIdx, shape, size, mass) return } @@ -145,42 +167,42 @@ func (wl *World) NewDynamic(shape Shapes, mass float32, size, pos math32.Vector3 // processed in the code (on the GPU). If this was not the setter of // the current variables, then the parameter variables are copied up // to the GPU. -func (wl *World) SetAsCurrent() { - isCur := (Bodies == wl.Bodies) - wl.SetAsCurrentVars() +func (ph *World) SetAsCurrent() { + isCur := (Bodies == ph.Bodies) + ph.SetAsCurrentVars() if GPUInitialized && !isCur { - wl.ToGPUInfra() + ph.ToGPUInfra() } } // SetAsCurrentVars sets these as the current global values that are // processed in the code (on the GPU). -func (wl *World) SetAsCurrentVars() { - Params = wl.Params - Bodies = wl.Bodies - Joints = wl.Joints - JointDoFs = wl.JointDoFs - BodyJoints = wl.BodyJoints - BodyCollidePairs = wl.BodyCollidePairs - Dynamics = wl.Dynamics - BroadContactsN = wl.BroadContactsN - BroadContacts = wl.BroadContacts - ContactsN = wl.ContactsN - Contacts = wl.Contacts - JointControls = wl.JointControls +func (ph *World) SetAsCurrentVars() { + Params = ph.Params + Bodies = ph.Bodies + Joints = ph.Joints + JointDoFs = ph.JointDoFs + BodyJoints = ph.BodyJoints + BodyCollidePairs = ph.BodyCollidePairs + Dynamics = ph.Dynamics + BroadContactsN = ph.BroadContactsN + BroadContacts = ph.BroadContacts + ContactsN = ph.ContactsN + Contacts = ph.Contacts + JointControls = ph.JointControls } // GPUInit initializes the GPU and transfers Infra. // Should have already called SetAsCurrent (needed for CPU and GPU). -func (wl *World) GPUInit() { +func (ph *World) GPUInit() { GPUInit() - UseGPU = wl.GPU - wl.ToGPUInfra() + UseGPU = ph.GPU + ph.ToGPUInfra() } // ToGPUInfra copies all the infrastructure for these filters up to // the GPU. This is done in GPUInit, and if current switched. -func (wl *World) ToGPUInfra() { +func (ph *World) ToGPUInfra() { ToGPUTensorStrides() ToGPU(ParamsVar, BodiesVar, JointsVar, JointDoFsVar, BodyJointsVar, BodyCollidePairsVar, DynamicsVar, BroadContactsNVar, BroadContactsVar, ContactsNVar, ContactsVar, JointControlsVar) } diff --git a/physics/world.goal b/physics/world.goal index 0bd61b84..9d73107c 100644 --- a/physics/world.goal +++ b/physics/world.goal @@ -19,6 +19,26 @@ type World struct { // Params are global parameters. Params []PhysParams + // CurrentWorld is the [BodyWorld] value to use when creating new bodies. + // Set to -1 to create global elements that interact with everything, + // while 0 and positive numbers only interact amongst themselves. + CurrentWorld int + + // WorldReplicasStart is the starting body index for replicated world bodies, + // which is needed for viewers to efficiently select a specific world to view. + // This is the start of the World=0 first instance. + WorldReplicasStart int32 + + // WorldReplicasN is the number of body elements within each set of + // replicated world bodies, which is needed for viewers to efficiently select + // a specific world to view. + WorldReplicasN int32 + + // CurrentObject is the [BodyObject] value to use when creating new bodies. + // Generally just increment when starting a new object. When using world replicas, + // the object indexes are copied, so objects are indexed by world and object id. + CurrentObject int + // Bodies are the rigid body elements (dynamic and static), // specifying the constant, non-dynamic properties, // which is initial state for dynamics. @@ -74,68 +94,70 @@ type World struct { } func NewWorld() *World { - wl := &World{} - wl.Init() - return wl + ph := &World{} + ph.Init() + return ph } // Init makes initial vars. Called in NewWorld. // Must call Config once configured. -func (wl *World) Init() { - wl.GPU = true - wl.Params = make([]PhysParams, 1) - wl.Params[0].Defaults() - wl.Reset() +func (ph *World) Init() { + ph.GPU = true + ph.Params = make([]PhysParams, 1) + ph.Params[0].Defaults() + ph.Reset() } // Reset resets all data to empty: starting over. -func (wl *World) Reset() { - wl.Bodies = tensor.NewFloat32(0, int(BodyVarsN)) - wl.Joints = tensor.NewFloat32(0, int(JointVarsN)) - wl.JointDoFs = tensor.NewFloat32(0, int(JointDoFVarsN)) - wl.BodyJoints = tensor.NewInt32(0, 2, 2) - wl.BodyCollidePairs = tensor.NewInt32(0, 2) - wl.Dynamics = tensor.NewFloat32(0, 2, int(DynamicVarsN)) - wl.BroadContactsN = tensor.NewInt32(1) - wl.BroadContacts = tensor.NewFloat32(0, int(ContactVarsN)) - wl.ContactsN = tensor.NewInt32(1) - wl.Contacts = tensor.NewFloat32(0, int(ContactVarsN)) - wl.JointControls = tensor.NewFloat32(0, int(JointControlVarsN)) - wl.SetAsCurrentVars() +func (ph *World) Reset() { + ph.Bodies = tensor.NewFloat32(0, int(BodyVarsN)) + ph.Joints = tensor.NewFloat32(0, int(JointVarsN)) + ph.JointDoFs = tensor.NewFloat32(0, int(JointDoFVarsN)) + ph.BodyJoints = tensor.NewInt32(0, 2, 2) + ph.BodyCollidePairs = tensor.NewInt32(0, 2) + ph.Dynamics = tensor.NewFloat32(0, 2, int(DynamicVarsN)) + ph.BroadContactsN = tensor.NewInt32(1) + ph.BroadContacts = tensor.NewFloat32(0, int(ContactVarsN)) + ph.ContactsN = tensor.NewInt32(1) + ph.Contacts = tensor.NewFloat32(0, int(ContactVarsN)) + ph.JointControls = tensor.NewFloat32(0, int(JointControlVarsN)) + ph.SetAsCurrentVars() } // NewBody adds a new body with given parameters. Returns the index. // Use this for Static elements; NewDynamic for dynamic elements. -func (wl *World) NewBody(shape Shapes, size, pos math32.Vector3, rot math32.Quat) int32 { - sizes := wl.Bodies.ShapeSizes() +func (ph *World) NewBody(shape Shapes, size, pos math32.Vector3, rot math32.Quat) int32 { + sizes := ph.Bodies.ShapeSizes() idx := int32(sizes[0]) - wl.Bodies.SetShapeSizes(int(idx+1), int(BodyVarsN)) - wl.Params[0].BodiesN = idx + 1 + ph.Bodies.SetShapeSizes(int(idx+1), int(BodyVarsN)) + ph.Params[0].BodiesN = idx + 1 SetBodyShape(idx, shape) SetBodyDynamic(idx, -1) SetBodyHSize(idx, size) SetBodyPos(idx, pos) SetBodyQuat(idx, rot) - SetBodyGroup(idx, -1) // assume static - wl.SetMass(idx, shape, size, 0) // assume static + SetBodyGroup(idx, -1) // assume static + SetBodyWorld(idx, int32(ph.CurrentWorld)) + SetBodyObject(idx, int32(ph.CurrentObject)) + ph.SetMass(idx, shape, size, 0) // assume static return idx } // NewDynamic adds a new dynamic body with given parameters. Returns the index. // Shape cannot be [Plane]. -func (wl *World) NewDynamic(shape Shapes, mass float32, size, pos math32.Vector3, rot math32.Quat) (bodyIdx, dynIdx int32) { +func (ph *World) NewDynamic(shape Shapes, mass float32, size, pos math32.Vector3, rot math32.Quat) (bodyIdx, dynIdx int32) { if shape == Plane { panic("physics.NewDynamic: shape cannot be Plane") } - bodyIdx = wl.NewBody(shape, size, pos, rot) - sizes := wl.Dynamics.ShapeSizes() + bodyIdx = ph.NewBody(shape, size, pos, rot) + sizes := ph.Dynamics.ShapeSizes() dynIdx = int32(sizes[0]) - wl.Dynamics.SetShapeSizes(int(dynIdx+1), 2, int(DynamicVarsN)) - wl.Params[0].DynamicsN = dynIdx + 1 + ph.Dynamics.SetShapeSizes(int(dynIdx+1), 2, int(DynamicVarsN)) + ph.Params[0].DynamicsN = dynIdx + 1 SetDynamicBody(dynIdx, bodyIdx) SetBodyDynamic(bodyIdx, dynIdx) SetBodyGroup(bodyIdx, 1) // dynamic - wl.SetMass(bodyIdx, shape, size, mass) + ph.SetMass(bodyIdx, shape, size, mass) return } @@ -143,42 +165,42 @@ func (wl *World) NewDynamic(shape Shapes, mass float32, size, pos math32.Vector3 // processed in the code (on the GPU). If this was not the setter of // the current variables, then the parameter variables are copied up // to the GPU. -func (wl *World) SetAsCurrent() { - isCur := (Bodies == wl.Bodies) - wl.SetAsCurrentVars() +func (ph *World) SetAsCurrent() { + isCur := (Bodies == ph.Bodies) + ph.SetAsCurrentVars() if GPUInitialized && !isCur { - wl.ToGPUInfra() + ph.ToGPUInfra() } } // SetAsCurrentVars sets these as the current global values that are // processed in the code (on the GPU). -func (wl *World) SetAsCurrentVars() { - Params = wl.Params - Bodies = wl.Bodies - Joints = wl.Joints - JointDoFs = wl.JointDoFs - BodyJoints = wl.BodyJoints - BodyCollidePairs = wl.BodyCollidePairs - Dynamics = wl.Dynamics - BroadContactsN = wl.BroadContactsN - BroadContacts = wl.BroadContacts - ContactsN = wl.ContactsN - Contacts = wl.Contacts - JointControls = wl.JointControls +func (ph *World) SetAsCurrentVars() { + Params = ph.Params + Bodies = ph.Bodies + Joints = ph.Joints + JointDoFs = ph.JointDoFs + BodyJoints = ph.BodyJoints + BodyCollidePairs = ph.BodyCollidePairs + Dynamics = ph.Dynamics + BroadContactsN = ph.BroadContactsN + BroadContacts = ph.BroadContacts + ContactsN = ph.ContactsN + Contacts = ph.Contacts + JointControls = ph.JointControls } // GPUInit initializes the GPU and transfers Infra. // Should have already called SetAsCurrent (needed for CPU and GPU). -func (wl *World) GPUInit() { +func (ph *World) GPUInit() { GPUInit() - UseGPU = wl.GPU - wl.ToGPUInfra() + UseGPU = ph.GPU + ph.ToGPUInfra() } // ToGPUInfra copies all the infrastructure for these filters up to // the GPU. This is done in GPUInit, and if current switched. -func (wl *World) ToGPUInfra() { +func (ph *World) ToGPUInfra() { ToGPUTensorStrides() ToGPU(ParamsVar, BodiesVar, JointsVar, JointDoFsVar, BodyJointsVar, BodyCollidePairsVar, DynamicsVar, BroadContactsNVar, BroadContactsVar, ContactsNVar, ContactsVar, JointControlsVar) } From 46a88dd5780282bde8c3eeeae5c4b8d5b4bfbe32 Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Fri, 26 Dec 2025 10:23:23 +0100 Subject: [PATCH 55/97] physics: start on Builder for replicating worlds, rename physics.World -> Model, phyxyz.World -> Scene -- World is untenable. --- docs/content/physics.md | 26 +- physics/bbox.go | 63 ----- physics/body.go | 16 +- physics/body.goal | 16 +- physics/builder/body.go | 77 ++++++ physics/builder/builder.go | 13 + physics/builder/joint.go | 49 ++++ physics/builder/object.go | 10 + physics/{state.go => builder/pose.go} | 122 ++++----- physics/builder/world.go | 17 ++ physics/config.go | 46 ++-- physics/config.goal | 46 ++-- physics/contact.go | 8 +- physics/contact.goal | 8 +- physics/dynamics.go | 2 +- physics/dynamics.goal | 2 +- physics/enumgen.go | 4 +- physics/examples/balls/balls.go | 16 +- physics/examples/pendula/pendula.go | 12 +- physics/group.go | 244 ------------------ physics/joint.go | 68 ++--- physics/joint.goal | 68 ++--- physics/{world.go => model.go} | 128 ++++----- physics/{world.goal => model.goal} | 126 ++++----- physics/phyxyz/editor.go | 54 ++-- physics/phyxyz/typegen.go | 25 +- physics/phyxyz/view.go | 16 +- physics/phyxyz/world.go | 112 +++----- physics/shaders/CollisionBroad.wgsl | 2 +- physics/shaders/CollisionNarrow.wgsl | 2 +- physics/shaders/DynamicsCurToNext.wgsl | 2 +- physics/shaders/ForcesFromJoints.wgsl | 2 +- physics/shaders/InitDynamics.wgsl | 2 +- physics/shaders/StepBodyContactDeltas.wgsl | 2 +- physics/shaders/StepBodyContacts.wgsl | 2 +- physics/shaders/StepBodyJointDeltas.wgsl | 2 +- physics/shaders/StepIntegrateBodies.wgsl | 2 +- physics/shaders/StepJointForces.wgsl | 2 +- physics/shaders/StepSolveJoints.wgsl | 2 +- physics/step.go | 30 +-- physics/step.goal | 30 +-- physics/typegen.go | 8 +- .../cogentcore_org-lab-physics-phyxyz.go | 13 +- .../labsymbols/cogentcore_org-lab-physics.go | 10 +- 44 files changed, 670 insertions(+), 837 deletions(-) delete mode 100644 physics/bbox.go create mode 100644 physics/builder/body.go create mode 100644 physics/builder/builder.go create mode 100644 physics/builder/joint.go create mode 100644 physics/builder/object.go rename physics/{state.go => builder/pose.go} (52%) create mode 100644 physics/builder/world.go delete mode 100644 physics/group.go rename physics/{world.go => model.go} (68%) rename physics/{world.goal => model.goal} (68%) diff --git a/docs/content/physics.md b/docs/content/physics.md index 020c0f65..96954256 100644 --- a/docs/content/physics.md +++ b/docs/content/physics.md @@ -30,26 +30,26 @@ params.NPendula = 2 ed.SetUserParams(¶ms) ed.SetConfigFunc(func() { - ph := ed.Physics - wr := ed.World + ml := ed.Model + sc := ed.Scene hsz := math32.Vec3(0.05, .2, 0.05) mass := float32(0.1) stY := 4*hsz.Y x := -hsz.Y rleft := math32.NewQuatAxisAngle(math32.Vec3(0, 0, 1), -math32.Pi/2) - pb := wr.NewDynamic(ph, "top", physics.Capsule, "blue", mass, hsz, math32.Vec3(x, stY, 0), rleft) + pb := sc.NewDynamic(ml, "top", physics.Capsule, "blue", mass, hsz, math32.Vec3(x, stY, 0), rleft) pb.SetBodyGroup(1) // no collide across groups - ji := pb.NewJointRevolute(ph, nil, math32.Vec3(0, stY, 0), math32.Vec3(0, hsz.Y, 0), math32.Vec3(0, 0, 1)) + ji := pb.NewJointRevolute(ml, nil, math32.Vec3(0, stY, 0), math32.Vec3(0, hsz.Y, 0), math32.Vec3(0, 0, 1)) physics.SetJointTargetPos(ji, 0, 0, 0) physics.SetJointTargetVel(ji, 0, 0, 0) for i := 1; i < params.NPendula; i++ { clr := colors.Names[i%len(colors.Names)] x = -float32(i)*hsz.Y*2 - hsz.Y - cb := wr.NewDynamic(ph, "child", physics.Capsule, clr, mass, hsz, math32.Vec3(x, stY, 0), rleft) + cb := sc.NewDynamic(ml, "child", physics.Capsule, clr, mass, hsz, math32.Vec3(x, stY, 0), rleft) cb.SetBodyGroup(1+i) - ji = cb.NewJointRevolute(ph, pb, math32.Vec3(0, -hsz.Y, 0), math32.Vec3(0, hsz.Y, 0), math32.Vec3(0, 0, 1)) + ji = cb.NewJointRevolute(ml, pb, math32.Vec3(0, -hsz.Y, 0), math32.Vec3(0, hsz.Y, 0), math32.Vec3(0, 0, 1)) physics.SetJointTargetPos(ji, 0, 0, 0) physics.SetJointTargetVel(ji, 0, 0, 0) pb = cb @@ -57,11 +57,11 @@ ed.SetConfigFunc(func() { }) ``` -The [[doc:physics/phyxyz/Editor]] widget provides the [[doc:physics/World]] and [[doc:physics/phyxyz/World]] elements, and the `ConfigFunc` function that configures the physics elements. Stepping through these elements in order: +The [[doc:physics/phyxyz/Editor]] widget provides the [[doc:physics/Model]] and [[doc:physics/phyxyz/Scene]] elements, and the `ConfigFunc` function that configures the physics elements. Stepping through these elements in order: ```go rleft := math32.NewQuatAxisAngle(math32.Vec3(0, 0, 1), -math32.Pi/2) - pb := wr.NewDynamic(ph, "top", physics.Box, "blue", mass, hsz, math32.Vec3(x, stY, 0), rleft) + pb := sc.NewDynamic(ml, "top", physics.Box, "blue", mass, hsz, math32.Vec3(x, stY, 0), rleft) ``` The `math32.Quat` quaternion provides all the rotational math used in `xyz` and `physics`, and the `rleft` instance represents a -90 degree rotation about the Z (depth axis), which is what causes the pendulum to start in a horizontal orientation. @@ -75,7 +75,7 @@ The `NewDynamic` method adds a new dynamic body element with a default visualiza The `Group` property of a body can be set to fine-tune collision logic. Positive-numbered groups only collide with each other and any negative-numbered groups, while negative-numbered groups only collide with positive numbered and not within the group. 0 means it doesn't collide with anything. With the crazy dynamics that emerge with multiple arms, it is good to let them all pass through each other. ```go - ji := pb.NewJointRevolute(ph, nil, math32.Vec3(0, stY, 0), math32.Vec3(0, hsz.Y, 0), math32.Vec3(0, 0, 1)) + ji := pb.NewJointRevolute(ml, nil, math32.Vec3(0, stY, 0), math32.Vec3(0, hsz.Y, 0), math32.Vec3(0, 0, 1)) physics.SetJointTargetPos(ji, 0, 0, 0) physics.SetJointTargetVel(ji, 0, 0, 0) ``` @@ -96,7 +96,7 @@ As discussed in [[GoSL]], to run equivalent code on the GPU and the CPU (i.e., s ## Bodies and Dynamics -The basic element is a _body_, which is a rigid physical entity with a specific shape, mass, position and orientation. Call [[doc:physics.World]] `NewBody` to create a new one. There are (currently) only standard geometric [[#shapes]] available (arbitrary triangular meshes and soft bodies could be supported as needed in the future, based on existing newton-physics code). +The basic element is a _body_, which is a rigid physical entity with a specific shape, mass, position and orientation. Call [[doc:physics.Model]] `NewBody` to create a new one. There are (currently) only standard geometric [[#shapes]] available (arbitrary triangular meshes and soft bodies could be supported as needed in the future, based on existing newton-physics code). By itself, a body is static. To make a body that is subject to forces and can be connected to other bodies via [[#joints]], use `NewDynamic`, which creates an additional set of data to implement the dynamic equations of the physics solver. The initial position and orientation of a dynamic body can be restored via the `InitState` method. @@ -144,9 +144,11 @@ Typically, bodies are created using the enhanced functions in the [[doc:physics/ ## Parallel worlds -The compute efficiency of the GPU goes up with the more elements that are processed in parallel, amortizing the memory transfer overhead and leveraging the parallel cores. Furthermore, in AI applications for example, models can be trained in parallel on different instances of the same environment, with each instance having its own random initial starting point and trajectory over time. All of these instances can be simulated in one `physics.World` by using the `World` index on the bodies, with the shared static environment living in World -1, and the elements of each instance (e.g., a simulated robot) living in its own separate world. +TODO: switch over to builder here. -The `NewBody` and `NewDynamic` methods automatically use the `World.CurrentWorld` index by default, or you can directly use `SetBodyWorld` to assign a specific world index. +The compute efficiency of the GPU goes up with the more elements that are processed in parallel, amortizing the memory transfer overhead and leveraging the parallel cores. Furthermore, in AI applications for example, models can be trained in parallel on different instances of the same environment, with each instance having its own random initial starting point and trajectory over time. All of these instances can be simulated in one `physics.Model` by using the `World` index on the bodies, with the shared static environment living in World -1, and the elements of each instance (e.g., a simulated robot) living in its own separate world. + +The `NewBody` and `NewDynamic` methods automatically use the `Model.CurrentWorld` index by default, or you can directly use `SetBodyWorld` to assign a specific world index. The `ReplicateWorld` method creates N replicas of an existing world, including all associated joints. This can only be called once, as it records the start and N-per-world of each such replicated world, which allows the `phyxyz` viewer to efficiently view a specific world. Thus, under this scenario, you create world 0 and then replicate it, then modify the initial positions and orientations accordingly, using `PositionObject`, as described next. The object numbers are also replicated so uniquely indexing a specific object instance requires specifying the world and object indexes. diff --git a/physics/bbox.go b/physics/bbox.go deleted file mode 100644 index 9addc98e..00000000 --- a/physics/bbox.go +++ /dev/null @@ -1,63 +0,0 @@ -// Copyright (c) 2019, Cogent Core. All rights reserved. -// Use of this source code is governed by a BSD-style -// license that can be found in the LICENSE file. - -package physics - -import "cogentcore.org/core/math32" - -// BBox contains bounding box and other gross object properties -type BBox struct { - - // bounding box in world coords (Axis-Aligned Bounding Box = AABB) - BBox math32.Box3 - - // velocity-projected bounding box in world coords: extend BBox to include future position of moving bodies -- collision must be made on this basis - VelBBox math32.Box3 - - // bounding sphere in local coords - BSphere math32.Sphere - - // area - Area float32 - - // volume - Volume float32 -} - -// SetBounds sets BBox from min, max and updates other factors based on that -func (bb *BBox) SetBounds(min, max math32.Vector3) { - bb.BBox.Set(&min, &max) - bb.UpdateFromBBox() -} - -// UpdateFromBBox updates other values from BBox -func (bb *BBox) UpdateFromBBox() { - bb.BSphere.SetFromBox(bb.BBox) - sz := bb.BBox.Size() - bb.Area = 2*sz.X + 2*sz.Y + 2*sz.Z - bb.Volume = sz.X * sz.Y * sz.Z -} - -// XForm transforms bounds with given quat and position offset to convert to world coords -func (bb *BBox) XForm(q math32.Quat, pos math32.Vector3) { - bb.BBox = bb.BBox.MulQuat(q).Translate(pos) - bb.BSphere.Translate(pos) -} - -// VelProject computes the velocity-projected bounding box for given velocity and step size -func (bb *BBox) VelProject(vel math32.Vector3, step float32) { - eb := bb.BBox.Translate(vel.MulScalar(step)) - bb.VelBBox = bb.BBox - bb.VelBBox.ExpandByBox(eb) -} - -// VelNilProject is for static items -- just copy the BBox -func (bb *BBox) VelNilProject() { - bb.VelBBox = bb.BBox -} - -// IntersectsVelBox returns true if two velocity-projected bounding boxes intersect -func (bb *BBox) IntersectsVelBox(oth *BBox) bool { - return bb.VelBBox.IntersectsBox(oth.VelBBox) -} diff --git a/physics/body.go b/physics/body.go index 7cf9050a..e154199f 100644 --- a/physics/body.go +++ b/physics/body.go @@ -39,9 +39,9 @@ const ( // in -1 because they don't collide amongst each-other, but do // potentially collide with dynamics). // Positive numbers only collide amongst themselves, and with - // negative groups, but not other positive groups. This is for - // more special-purpose dynamics: in general use 1 for all dynamic - // bodies. There is an automatic constraint that the two objects + // negative groups, but not other positive groups. To avoid + // unwanted collisions, put bodies into separate groups. + // There is an automatic constraint that the two objects // within a single joint do not collide with each other, so this // does not need to be handled here. BodyGroup @@ -54,7 +54,9 @@ const ( // indexes are copied, so objects are indexed by world and object id. BodyObject - // BodyHSize is the size of the object (values depend on shape type). + // BodyHSize is the half-size (e.g., radius) of the body. + // Values depend on shape type: X is generally radius, + // Y is half-height. BodyHSizeX BodyHSizeY BodyHSizeZ @@ -161,9 +163,9 @@ func GetBodyWorld(idx int32) int32 { // in -1 because they don't collide amongst each-other, but do // potentially collide with dynamics). // Positive numbers only collide amongst themselves, and with -// negative groups, but not other positive groups. This is for -// more special-purpose dynamics: in general use 1 for all dynamic -// bodies. There is an automatic constraint that the two objects +// negative groups, but not other positive groups. To avoid +// unwanted collisions, put bodies into separate groups. +// There is an automatic constraint that the two objects // within a single joint do not collide with each other, so this // does not need to be handled here. func SetBodyGroup(idx, w int32) { diff --git a/physics/body.goal b/physics/body.goal index 1de95034..e6b23aa2 100644 --- a/physics/body.goal +++ b/physics/body.goal @@ -37,9 +37,9 @@ const ( // in -1 because they don't collide amongst each-other, but do // potentially collide with dynamics). // Positive numbers only collide amongst themselves, and with - // negative groups, but not other positive groups. This is for - // more special-purpose dynamics: in general use 1 for all dynamic - // bodies. There is an automatic constraint that the two objects + // negative groups, but not other positive groups. To avoid + // unwanted collisions, put bodies into separate groups. + // There is an automatic constraint that the two objects // within a single joint do not collide with each other, so this // does not need to be handled here. BodyGroup @@ -52,7 +52,9 @@ const ( // indexes are copied, so objects are indexed by world and object id. BodyObject - // BodyHSize is the size of the object (values depend on shape type). + // BodyHSize is the half-size (e.g., radius) of the body. + // Values depend on shape type: X is generally radius, + // Y is half-height. BodyHSizeX BodyHSizeY BodyHSizeZ @@ -159,9 +161,9 @@ func GetBodyWorld(idx int32) int32 { // in -1 because they don't collide amongst each-other, but do // potentially collide with dynamics). // Positive numbers only collide amongst themselves, and with -// negative groups, but not other positive groups. This is for -// more special-purpose dynamics: in general use 1 for all dynamic -// bodies. There is an automatic constraint that the two objects +// negative groups, but not other positive groups. To avoid +// unwanted collisions, put bodies into separate groups. +// There is an automatic constraint that the two objects // within a single joint do not collide with each other, so this // does not need to be handled here. func SetBodyGroup(idx, w int32) { diff --git a/physics/builder/body.go b/physics/builder/body.go new file mode 100644 index 00000000..25e78219 --- /dev/null +++ b/physics/builder/body.go @@ -0,0 +1,77 @@ +// Copyright (c) 2025, Cogent Core. All rights reserved. +// Use of this source code is governed by a BSD-style +// license that can be found in the LICENSE file. + +package builder + +import ( + "cogentcore.org/core/math32" + "cogentcore.org/lab/physics" + "cogentcore.org/lab/physics/phyxyz" +) + +// Body is a rigid body. +type Body struct { + // Shape of the body. + Shape physics.Shapes + + // Dynamic makes this a dynamic body. + Dynamic bool + + // Group partitions bodies within worlds into different groups + // for collision detection. 0 does not collide with anything. + // Negative numbers are global within a world, except they don't + // collide amongst themselves (all non-dynamic bodies should go + // in -1 because they don't collide amongst each-other, but do + // potentially collide with dynamics). + // Positive numbers only collide amongst themselves, and with + // negative groups, but not other positive groups. To avoid + // unwanted collisions, put bodies into separate groups. + // There is an automatic constraint that the two objects + // within a single joint do not collide with each other, so this + // does not need to be handled here. + Group int + + // HSize is the half-size (e.g., radius) of the body. + // Values depend on shape type: X is generally radius, + // Y is half-height. + HSize math32.Vector3 + + // Thick is the thickness of the body, as a hollow shape. + // If 0, then it is a solid shape (default). + Thick float32 + + // Mass of the object. Only relevant for Dynamic bodies. + Mass float32 + + // Bounce specifies the COR or coefficient of restitution (0..1), + // which determines how elastic the collision is, + // i.e., final velocity / initial velocity. + Bounce float32 + + // Friction is the standard coefficient for linear friction (mu). + Friction float32 + + // FrictionTortion is resistance to spinning at the contact point. + FrictionTortion float32 + + // FrictionRolling is resistance to rolling motion at contact. + FrictionRolling float32 + + // Pose has the position and rotation. + Pose Pose + + // Com is the center-of-mass offset from the Pose.Pos. + Com math32.Vector3 + + // View is the view element for this Body (optional). + View *phyxyz.View + + // Index is the index of this body in the physics.World, + // once built. + Index int32 + + // DynamicIndex is the index of this dynamic body in + // the physics.World, once built. + DynamicIndex int32 +} diff --git a/physics/builder/builder.go b/physics/builder/builder.go new file mode 100644 index 00000000..db289e8c --- /dev/null +++ b/physics/builder/builder.go @@ -0,0 +1,13 @@ +// Copyright (c) 2025, Cogent Core. All rights reserved. +// Use of this source code is governed by a BSD-style +// license that can be found in the LICENSE file. + +package builder + +// Builder is the global container of [physics.Model] elements, +// organized into worlds that are independently updated. +type Builder struct { + + // Worlds are the independent world elements. + Worlds []World +} diff --git a/physics/builder/joint.go b/physics/builder/joint.go new file mode 100644 index 00000000..4fb579c6 --- /dev/null +++ b/physics/builder/joint.go @@ -0,0 +1,49 @@ +// Copyright (c) 2025, Cogent Core. All rights reserved. +// Use of this source code is governed by a BSD-style +// license that can be found in the LICENSE file. + +package builder + +import ( + "cogentcore.org/core/math32" + "cogentcore.org/core/math32/minmax" + "cogentcore.org/lab/physics" +) + +// Joint describes a joint between two bodies. +type Joint struct { + // Parent is index within an Object for parent body. + Parent int + + // Parent is index within an Object for parent body. + Child int + + // Type is the type of the joint. + Type physics.JointTypes + + // PPose is the parent position and orientation of the joint + // in the parent's body-centered coordinates. + PPose Pose + + // CPose is the child position and orientation of the joint + // in the parent's body-centered coordinates. + CPose Pose + + // LinearDoFN is the number of linear degrees of freedom (3 max). + LinearDoFN int + + // AngularDoFN is the number of linear degrees of freedom (3 max). + AngularDoFN int + + // DoFs are the degrees-of-freedom for this joint. + DoFs []DoF +} + +// DoF is a degree-of-freedom for a [Joint]. +type DoF struct { + // Axis is the axis of articulation. + Axis math32.Vector3 + + // Limit has the limits for motion of this DoF. + Limit minmax.F32 +} diff --git a/physics/builder/object.go b/physics/builder/object.go new file mode 100644 index 00000000..034ace8c --- /dev/null +++ b/physics/builder/object.go @@ -0,0 +1,10 @@ +// Copyright (c) 2025, Cogent Core. All rights reserved. +// Use of this source code is governed by a BSD-style +// license that can be found in the LICENSE file. + +package builder + +type Object struct { + Bodies []Body + Joints []Joint +} diff --git a/physics/state.go b/physics/builder/pose.go similarity index 52% rename from physics/state.go rename to physics/builder/pose.go index 3b2f4ec2..09033875 100644 --- a/physics/state.go +++ b/physics/builder/pose.go @@ -2,85 +2,45 @@ // Use of this source code is governed by a BSD-style // license that can be found in the LICENSE file. -package physics +package builder import ( - "math" - + "cogentcore.org/core/core" + "cogentcore.org/core/icons" "cogentcore.org/core/math32" + "cogentcore.org/core/tree" ) -// State contains the basic physical state including position, orientation, velocity. -// These are only the values that can be either relative or absolute -- other physical -// state values such as Mass should go in Rigid. -type State struct { +// Pose represents the 3D position and rotation. +type Pose struct { // Pos is the position of center of mass of object. Pos math32.Vector3 // Quat is the rotation specified as a quaternion. Quat math32.Quat - - // LinVel is the linear velocity. - LinVel math32.Vector3 - - // AngVel is the angular velocity. - AngVel math32.Vector3 } // Defaults sets defaults only if current values are nil -func (ps *State) Defaults() { +func (ps *Pose) Defaults() { if ps.Quat.IsNil() { ps.Quat.SetIdentity() } } -//////// State updates +//////// Pose updates // FromRel sets state from relative values compared to a parent state -func (ps *State) FromRel(rel, par *State) { +func (ps *Pose) FromRel(rel, par *Pose) { ps.Quat = rel.Quat.Mul(par.Quat) ps.Pos = par.Quat.MulVector(rel.Pos).Add(par.Pos) - ps.LinVel = rel.Quat.MulVector(rel.LinVel).Add(par.LinVel) - ps.AngVel = rel.Quat.MulVector(rel.AngVel).Add(par.AngVel) -} - -// AngMotionMax is maximum angular motion that can be taken per update -const AngMotionMax = math.Pi / 4 - -// StepByAngVel steps the Quat rotation from angular velocity -func (ps *State) StepByAngVel(step float32) { - ang := math32.Sqrt(ps.AngVel.Dot(ps.AngVel)) - - // limit the angular motion - if ang*step > AngMotionMax { - ang = AngMotionMax / step - } - var axis math32.Vector3 - if ang < 0.001 { - // use Taylor's expansions of sync function - axis = ps.AngVel.MulScalar(0.5*step - (step*step*step)*0.020833333333*ang*ang) - } else { - // sync(fAngle) = sin(c*fAngle)/t - axis = ps.AngVel.MulScalar(math32.Sin(0.5*ang*step) / ang) - } - var dq math32.Quat - dq.SetFromAxisAngle(axis, ang*step) - ps.Quat = dq.Mul(ps.Quat) - ps.Quat.Normalize() -} - -// StepByLinVel steps the Pos from the linear velocity -func (ps *State) StepByLinVel(step float32) { - ps.Pos = ps.Pos.Add(ps.LinVel.MulScalar(step)) } //////// Moving // Move moves (translates) Pos by given amount, and sets the LinVel to the given // delta -- this can be useful for Scripted motion to track movement. -func (ps *State) Move(delta math32.Vector3) { - ps.LinVel = delta +func (ps *Pose) Move(delta math32.Vector3) { ps.Pos.SetAdd(delta) } @@ -88,68 +48,98 @@ func (ps *State) Move(delta math32.Vector3) { // relative to the current rotation orientation. // The axis is normalized prior to aplying the distance factor. // Sets the LinVel to motion vector. -func (ps *State) MoveOnAxis(x, y, z, dist float32) { //types:add - ps.LinVel = ps.Quat.MulVector(math32.Vec3(x, y, z).Normal()).MulScalar(dist) - ps.Pos.SetAdd(ps.LinVel) +func (ps *Pose) MoveOnAxis(x, y, z, dist float32) { //types:add + delta := ps.Quat.MulVector(math32.Vec3(x, y, z).Normal()).MulScalar(dist) + ps.Pos.SetAdd(delta) } // MoveOnAxisAbs moves (translates) the specified distance on the specified local axis, // in absolute X,Y,Z coordinates (does not apply the Quat rotation factor. // The axis is normalized prior to aplying the distance factor. // Sets the LinVel to motion vector. -func (ps *State) MoveOnAxisAbs(x, y, z, dist float32) { //types:add - ps.LinVel = math32.Vec3(x, y, z).Normal().MulScalar(dist) - ps.Pos.SetAdd(ps.LinVel) +func (ps *Pose) MoveOnAxisAbs(x, y, z, dist float32) { //types:add + delta := math32.Vec3(x, y, z).Normal().MulScalar(dist) + ps.Pos.SetAdd(delta) } //////// Rotating // SetEulerRotation sets the rotation in Euler angles (degrees). -func (ps *State) SetEulerRotation(x, y, z float32) { //types:add +func (ps *Pose) SetEulerRotation(x, y, z float32) { //types:add ps.Quat.SetFromEuler(math32.Vec3(x, y, z).MulScalar(math32.DegToRadFactor)) } // SetEulerRotationRad sets the rotation in Euler angles (radians). -func (ps *State) SetEulerRotationRad(x, y, z float32) { +func (ps *Pose) SetEulerRotationRad(x, y, z float32) { ps.Quat.SetFromEuler(math32.Vec3(x, y, z)) } // EulerRotation returns the current rotation in Euler angles (degrees). -func (ps *State) EulerRotation() math32.Vector3 { //types:add +func (ps *Pose) EulerRotation() math32.Vector3 { //types:add return ps.Quat.ToEuler().MulScalar(math32.RadToDegFactor) } // EulerRotationRad returns the current rotation in Euler angles (radians). -func (ps *State) EulerRotationRad() math32.Vector3 { +func (ps *Pose) EulerRotationRad() math32.Vector3 { return ps.Quat.ToEuler() } // SetAxisRotation sets rotation from local axis and angle in degrees. -func (ps *State) SetAxisRotation(x, y, z, angle float32) { //types:add +func (ps *Pose) SetAxisRotation(x, y, z, angle float32) { //types:add ps.Quat.SetFromAxisAngle(math32.Vec3(x, y, z), math32.DegToRad(angle)) } // SetAxisRotationRad sets rotation from local axis and angle in radians. -func (ps *State) SetAxisRotationRad(x, y, z, angle float32) { +func (ps *Pose) SetAxisRotationRad(x, y, z, angle float32) { ps.Quat.SetFromAxisAngle(math32.Vec3(x, y, z), angle) } // RotateOnAxis rotates around the specified local axis the specified angle in degrees. -func (ps *State) RotateOnAxis(x, y, z, angle float32) { //types:add +func (ps *Pose) RotateOnAxis(x, y, z, angle float32) { //types:add ps.Quat.SetMul(math32.NewQuatAxisAngle(math32.Vec3(x, y, z), math32.DegToRad(angle))) } // RotateOnAxisRad rotates around the specified local axis the specified angle in radians. -func (ps *State) RotateOnAxisRad(x, y, z, angle float32) { +func (ps *Pose) RotateOnAxisRad(x, y, z, angle float32) { ps.Quat.SetMul(math32.NewQuatAxisAngle(math32.Vec3(x, y, z), angle)) } // RotateEuler rotates by given Euler angles (in degrees) relative to existing rotation. -func (ps *State) RotateEuler(x, y, z float32) { //types:add +func (ps *Pose) RotateEuler(x, y, z float32) { //types:add ps.Quat.SetMul(math32.NewQuatEuler(math32.Vec3(x, y, z).MulScalar(math32.DegToRadFactor))) } // RotateEulerRad rotates by given Euler angles (in radians) relative to existing rotation. -func (ps *State) RotateEulerRad(x, y, z, angle float32) { +func (ps *Pose) RotateEulerRad(x, y, z, angle float32) { ps.Quat.SetMul(math32.NewQuatEuler(math32.Vec3(x, y, z))) } + +// MakePoseToolbar returns a toolbar function for physics state updates, +// calling the given updt function after making the change. +func MakePoseToolbar(ps *Pose, updt func()) func(p *tree.Plan) { + return func(p *tree.Plan) { + tree.Add(p, func(w *core.FuncButton) { + w.SetFunc(ps.SetEulerRotation).SetAfterFunc(updt).SetIcon(icons.Rotate90DegreesCcw) + }) + tree.Add(p, func(w *core.FuncButton) { + w.SetFunc(ps.SetAxisRotation).SetAfterFunc(updt).SetIcon(icons.Rotate90DegreesCcw) + }) + tree.Add(p, func(w *core.FuncButton) { + w.SetFunc(ps.RotateEuler).SetAfterFunc(updt).SetIcon(icons.Rotate90DegreesCcw) + }) + tree.Add(p, func(w *core.FuncButton) { + w.SetFunc(ps.RotateOnAxis).SetAfterFunc(updt).SetIcon(icons.Rotate90DegreesCcw) + }) + tree.Add(p, func(w *core.FuncButton) { + w.SetFunc(ps.EulerRotation).SetAfterFunc(updt).SetShowReturn(true).SetIcon(icons.Rotate90DegreesCcw) + }) + tree.Add(p, func(w *core.Separator) {}) + tree.Add(p, func(w *core.FuncButton) { + w.SetFunc(ps.MoveOnAxis).SetAfterFunc(updt).SetIcon(icons.MoveItem) + }) + tree.Add(p, func(w *core.FuncButton) { + w.SetFunc(ps.MoveOnAxisAbs).SetAfterFunc(updt).SetIcon(icons.MoveItem) + }) + + } +} diff --git a/physics/builder/world.go b/physics/builder/world.go new file mode 100644 index 00000000..4d9dc9c4 --- /dev/null +++ b/physics/builder/world.go @@ -0,0 +1,17 @@ +// Copyright (c) 2025, Cogent Core. All rights reserved. +// Use of this source code is governed by a BSD-style +// license that can be found in the LICENSE file. + +package builder + +// World is one world within the Builder. +type World struct { + // World is the world index. + World int + + // Objects are the objects within the world. + // Each object is a coherent collection of bodies, typically + // connected by joints. This is an organizational convenience + // for positioning elements as well. + Objects []Object +} diff --git a/physics/config.go b/physics/config.go index 8c691649..1e0071c3 100644 --- a/physics/config.go +++ b/physics/config.go @@ -14,20 +14,20 @@ import ( // Config does final configuration prior to running // after everything has been added. Does SetAsCurrent, GPUInit. -func (wl *World) Config() { - wl.ConfigJoints() - wl.ConfigBodyCollidePairs() - wl.SetMaxContacts() - wl.SetAsCurrent() - wl.ConfigBodies() - wl.GPUInit() - wl.InitState() +func (ml *Model) Config() { + ml.ConfigJoints() + ml.ConfigBodyCollidePairs() + ml.SetMaxContacts() + ml.SetAsCurrent() + ml.ConfigBodies() + ml.GPUInit() + ml.InitState() } // ConfigJoints does all of the initialization associated with joints. -func (wl *World) ConfigJoints() { +func (ml *Model) ConfigJoints() { // accumulate parent and child joints per dynamic - params := &wl.Params[0] + params := &ml.Params[0] nj := params.JointsN nd := params.DynamicsN @@ -54,43 +54,43 @@ func (wl *World) ConfigJoints() { if maxi == 0 { maxi = 1 } - wl.BodyJoints.SetShapeSizes(int(nd), 2, maxi+1) + ml.BodyJoints.SetShapeSizes(int(nd), 2, maxi+1) for di := range nd { np := int32(len(bjp[di])) - wl.BodyJoints.Set(np, int(di), int(0), int(0)) + ml.BodyJoints.Set(np, int(di), int(0), int(0)) for i, ji := range bjp[di] { - wl.BodyJoints.Set(ji, int(di), int(0), int(1+i)) + ml.BodyJoints.Set(ji, int(di), int(0), int(1+i)) } nc := int32(len(bjc[di])) - wl.BodyJoints.Set(nc, int(di), int(1), int(0)) + ml.BodyJoints.Set(nc, int(di), int(1), int(0)) for i, ji := range bjc[di] { - wl.BodyJoints.Set(ji, int(di), int(1), int(1+i)) + ml.BodyJoints.Set(ji, int(di), int(1), int(1+i)) } } if nj == 0 { - wl.Joints = tensor.NewFloat32(1, int(JointVarsN)) - wl.JointDoFs = tensor.NewFloat32(1, int(JointDoFVarsN)) - wl.JointControls = tensor.NewFloat32(1, int(JointControlVarsN)) + ml.Joints = tensor.NewFloat32(1, int(JointVarsN)) + ml.JointDoFs = tensor.NewFloat32(1, int(JointDoFVarsN)) + ml.JointControls = tensor.NewFloat32(1, int(JointControlVarsN)) } } // ConfigBodies updates computed body values from current values. // Call if body params (mass, size) change. -func (wl *World) ConfigBodies() { - params := &wl.Params[0] +func (ml *Model) ConfigBodies() { + params := &ml.Params[0] nb := params.BodiesN for bi := range nb { shape := GetBodyShape(bi) size := BodyHSize(bi) mass := Bodies.Value(int(bi), int(BodyMass)) - wl.SetMass(bi, shape, size, mass) + ml.SetMass(bi, shape, size, mass) } } // InitState initializes the simulation state. -func (wl *World) InitState() { +func (ml *Model) InitState() { params := GetParams(0) - wl.ToGPUInfra() + ml.ToGPUInfra() RunInitDynamics(int(params.DynamicsN)) RunDone(DynamicsVar) } diff --git a/physics/config.goal b/physics/config.goal index e26cc841..64a02e8d 100644 --- a/physics/config.goal +++ b/physics/config.goal @@ -12,20 +12,20 @@ import ( // Config does final configuration prior to running // after everything has been added. Does SetAsCurrent, GPUInit. -func (wl *World) Config() { - wl.ConfigJoints() - wl.ConfigBodyCollidePairs() - wl.SetMaxContacts() - wl.SetAsCurrent() - wl.ConfigBodies() - wl.GPUInit() - wl.InitState() +func (ml *Model) Config() { + ml.ConfigJoints() + ml.ConfigBodyCollidePairs() + ml.SetMaxContacts() + ml.SetAsCurrent() + ml.ConfigBodies() + ml.GPUInit() + ml.InitState() } // ConfigJoints does all of the initialization associated with joints. -func (wl *World) ConfigJoints() { +func (ml *Model) ConfigJoints() { // accumulate parent and child joints per dynamic - params := &wl.Params[0] + params := &ml.Params[0] nj := params.JointsN nd := params.DynamicsN @@ -52,43 +52,43 @@ func (wl *World) ConfigJoints() { if maxi == 0 { maxi = 1 } - wl.BodyJoints.SetShapeSizes(int(nd), 2, maxi+1) + ml.BodyJoints.SetShapeSizes(int(nd), 2, maxi+1) for di := range nd { np := int32(len(bjp[di])) - wl.BodyJoints[di, 0, 0] = np + ml.BodyJoints[di, 0, 0] = np for i, ji := range bjp[di] { - wl.BodyJoints[di, 0, 1+i] = ji + ml.BodyJoints[di, 0, 1+i] = ji } nc := int32(len(bjc[di])) - wl.BodyJoints[di, 1, 0] = nc + ml.BodyJoints[di, 1, 0] = nc for i, ji := range bjc[di] { - wl.BodyJoints[di, 1, 1+i] = ji + ml.BodyJoints[di, 1, 1+i] = ji } } if nj == 0 { - wl.Joints = tensor.NewFloat32(1, int(JointVarsN)) - wl.JointDoFs = tensor.NewFloat32(1, int(JointDoFVarsN)) - wl.JointControls = tensor.NewFloat32(1, int(JointControlVarsN)) + ml.Joints = tensor.NewFloat32(1, int(JointVarsN)) + ml.JointDoFs = tensor.NewFloat32(1, int(JointDoFVarsN)) + ml.JointControls = tensor.NewFloat32(1, int(JointControlVarsN)) } } // ConfigBodies updates computed body values from current values. // Call if body params (mass, size) change. -func (wl *World) ConfigBodies() { - params := &wl.Params[0] +func (ml *Model) ConfigBodies() { + params := &ml.Params[0] nb := params.BodiesN for bi := range nb { shape := GetBodyShape(bi) size := BodyHSize(bi) mass := Bodies[bi, BodyMass] - wl.SetMass(bi, shape, size, mass) + ml.SetMass(bi, shape, size, mass) } } // InitState initializes the simulation state. -func (wl *World) InitState() { +func (ml *Model) InitState() { params := GetParams(0) - wl.ToGPUInfra() + ml.ToGPUInfra() RunInitDynamics(int(params.DynamicsN)) RunDone(DynamicsVar) } diff --git a/physics/contact.go b/physics/contact.go index 48992e30..55fb8787 100644 --- a/physics/contact.go +++ b/physics/contact.go @@ -1,6 +1,6 @@ // Code generated by "goal build"; DO NOT EDIT. //line contact.goal:1 -// Copyright (c) 2019, Cogent Core. All rights reserved. +// Copyright (c) 2025, Cogent Core. All rights reserved. // Use of this source code is governed by a BSD-style // license that can be found in the LICENSE file. @@ -707,7 +707,7 @@ func ContactConstraint(err float32, q0A, q0B math32.Quat, mInvA, mInvB float32, // IsChildDynamic returns true if dic is a direct child // on any joint where dip is the parent. -func (wl *World) IsChildDynamic(dip, dic int32) bool { +func (wl *Model) IsChildDynamic(dip, dic int32) bool { if dip < 0 || dic < 0 { return false } @@ -728,7 +728,7 @@ func (wl *World) IsChildDynamic(dip, dic int32) bool { // based on world and group settings and not being direct parent // child relationship within a joint. Result has A with lower shape type, // so that shapes are in a canonical order. -func (wl *World) ConfigBodyCollidePairs() { +func (wl *Model) ConfigBodyCollidePairs() { params := &wl.Params[0] nb := params.BodiesN nalc := int(nb) * 10 @@ -787,7 +787,7 @@ func (wl *World) ConfigBodyCollidePairs() { // SetMaxContacts computes [Params.MaxContacts] based on current list of // [BodyCollidePairs]. -func (wl *World) SetMaxContacts() { +func (wl *Model) SetMaxContacts() { params := &wl.Params[0] n := int32(0) diff --git a/physics/contact.goal b/physics/contact.goal index bd2ef035..87f40ac9 100644 --- a/physics/contact.goal +++ b/physics/contact.goal @@ -1,4 +1,4 @@ -// Copyright (c) 2019, Cogent Core. All rights reserved. +// Copyright (c) 2025, Cogent Core. All rights reserved. // Use of this source code is governed by a BSD-style // license that can be found in the LICENSE file. @@ -706,7 +706,7 @@ func ContactConstraint(err float32, q0A, q0B math32.Quat, mInvA, mInvB float32, // IsChildDynamic returns true if dic is a direct child // on any joint where dip is the parent. -func (wl *World) IsChildDynamic(dip, dic int32) bool { +func (wl *Model) IsChildDynamic(dip, dic int32) bool { if dip < 0 || dic < 0 { return false } @@ -727,7 +727,7 @@ func (wl *World) IsChildDynamic(dip, dic int32) bool { // based on world and group settings and not being direct parent // child relationship within a joint. Result has A with lower shape type, // so that shapes are in a canonical order. -func (wl *World) ConfigBodyCollidePairs() { +func (wl *Model) ConfigBodyCollidePairs() { params := &wl.Params[0] nb := params.BodiesN nalc := int(nb) * 10 @@ -786,7 +786,7 @@ func (wl *World) ConfigBodyCollidePairs() { // SetMaxContacts computes [Params.MaxContacts] based on current list of // [BodyCollidePairs]. -func (wl *World) SetMaxContacts() { +func (wl *Model) SetMaxContacts() { params := &wl.Params[0] n := int32(0) diff --git a/physics/dynamics.go b/physics/dynamics.go index 0d77dfbb..09447d45 100644 --- a/physics/dynamics.go +++ b/physics/dynamics.go @@ -195,7 +195,7 @@ func SetDynamicAngDelta(idx, cni int32, angDelta math32.Vector3) { // SetMass sets the mass of given body object (only relevant for dynamics), // including a default inertia tensor based on solid shape of given size. -func (wl *World) SetMass(idx int32, shape Shapes, size math32.Vector3, mass float32) { +func (ml *Model) SetMass(idx int32, shape Shapes, size math32.Vector3, mass float32) { Bodies.Set(shape.Radius(size), int(idx), int(BodyRadius)) Bodies.Set(mass, int(idx), int(BodyMass)) invm := mass diff --git a/physics/dynamics.goal b/physics/dynamics.goal index 4faa4bf7..313bc999 100644 --- a/physics/dynamics.goal +++ b/physics/dynamics.goal @@ -193,7 +193,7 @@ func SetDynamicAngDelta(idx, cni int32, angDelta math32.Vector3) { // SetMass sets the mass of given body object (only relevant for dynamics), // including a default inertia tensor based on solid shape of given size. -func (wl *World) SetMass(idx int32, shape Shapes, size math32.Vector3, mass float32) { +func (ml *Model) SetMass(idx int32, shape Shapes, size math32.Vector3, mass float32) { Bodies[idx, BodyRadius] = shape.Radius(size) Bodies[idx, BodyMass] = mass invm := mass diff --git a/physics/enumgen.go b/physics/enumgen.go index 6d85c704..bbfd2c9f 100644 --- a/physics/enumgen.go +++ b/physics/enumgen.go @@ -17,7 +17,7 @@ const BodyVarsN BodyVars = 44 var _BodyVarsValueMap = map[string]BodyVars{`BodyShape`: 0, `BodyDynamic`: 1, `BodyWorld`: 2, `BodyGroup`: 3, `BodyObject`: 4, `BodyHSizeX`: 5, `BodyHSizeY`: 6, `BodyHSizeZ`: 7, `BodyThick`: 8, `BodyMass`: 9, `BodyInvMass`: 10, `BodyBounce`: 11, `BodyFriction`: 12, `BodyFrictionTortion`: 13, `BodyFrictionRolling`: 14, `BodyPosX`: 15, `BodyPosY`: 16, `BodyPosZ`: 17, `BodyQuatX`: 18, `BodyQuatY`: 19, `BodyQuatZ`: 20, `BodyQuatW`: 21, `BodyComX`: 22, `BodyComY`: 23, `BodyComZ`: 24, `BodyInertiaXX`: 25, `BodyInertiaYX`: 26, `BodyInertiaZX`: 27, `BodyInertiaXY`: 28, `BodyInertiaYY`: 29, `BodyInertiaZY`: 30, `BodyInertiaXZ`: 31, `BodyInertiaYZ`: 32, `BodyInertiaZZ`: 33, `BodyInvInertiaXX`: 34, `BodyInvInertiaYX`: 35, `BodyInvInertiaZX`: 36, `BodyInvInertiaXY`: 37, `BodyInvInertiaYY`: 38, `BodyInvInertiaZY`: 39, `BodyInvInertiaXZ`: 40, `BodyInvInertiaYZ`: 41, `BodyInvInertiaZZ`: 42, `BodyRadius`: 43} -var _BodyVarsDescMap = map[BodyVars]string{0: `BodyShape is the shape type of the object, as a Shapes type.`, 1: `BodyDynamic is the index into Dynamics for this body, which is -1 for static bodies. Use this to get current Pos and Quat values for a dynamic body.`, 2: `BodyWorld partitions bodies into different worlds for collision detection: Global bodies = -1 can collide with everything; otherwise only items within the same world collide. NewBody uses [World.CurrentWorld] to initialize.`, 3: `BodyGroup partitions bodies within worlds into different groups for collision detection. 0 does not collide with anything. Negative numbers are global within a world, except they don't collide amongst themselves (all non-dynamic bodies should go in -1 because they don't collide amongst each-other, but do potentially collide with dynamics). Positive numbers only collide amongst themselves, and with negative groups, but not other positive groups. This is for more special-purpose dynamics: in general use 1 for all dynamic bodies. There is an automatic constraint that the two objects within a single joint do not collide with each other, so this does not need to be handled here.`, 4: `BodyObject identifies bodies that all belong to the same overall physical object (e.g., a robot or simulated animal). It has no implications for collision or physics simulation. Instead, it is used for external manipulation on entire objects at a time, e.g. via PositionObject. When using world replicas, the object indexes are copied, so objects are indexed by world and object id.`, 5: `BodyHSize is the size of the object (values depend on shape type).`, 6: ``, 7: ``, 8: `BodyThick is the thickness of the body, as a hollow shape. If 0, then it is a solid shape (default).`, 9: `BodyMass is the mass of the object.`, 10: `BodyInvMass is 1/mass of the object or 0 if no mass.`, 11: `BodyBounce specifies the COR or coefficient of restitution (0..1), which determines how elastic the collision is, i.e., final velocity / initial velocity.`, 12: `BodyFriction is the standard coefficient for linear friction (mu).`, 13: `BodyFrictionTortion is resistance to spinning at the contact point.`, 14: `BodyFrictionRolling is resistance to rolling motion at contact.`, 15: `3D position of body (structural center).`, 16: ``, 17: ``, 18: `Quaternion rotation of body.`, 19: ``, 20: ``, 21: ``, 22: `Relative center-of-mass offset from 3D position of body.`, 23: ``, 24: ``, 25: `Inertia 3x3 matrix (column matrix organization, r,c labels).`, 26: ``, 27: ``, 28: ``, 29: ``, 30: ``, 31: ``, 32: ``, 33: ``, 34: `InvInertia inverse inertia 3x3 matrix (column matrix organization, r,c labels).`, 35: ``, 36: ``, 37: ``, 38: ``, 39: ``, 40: ``, 41: ``, 42: ``, 43: `radius for broadphase collision`} +var _BodyVarsDescMap = map[BodyVars]string{0: `BodyShape is the shape type of the object, as a Shapes type.`, 1: `BodyDynamic is the index into Dynamics for this body, which is -1 for static bodies. Use this to get current Pos and Quat values for a dynamic body.`, 2: `BodyWorld partitions bodies into different worlds for collision detection: Global bodies = -1 can collide with everything; otherwise only items within the same world collide. NewBody uses [World.CurrentWorld] to initialize.`, 3: `BodyGroup partitions bodies within worlds into different groups for collision detection. 0 does not collide with anything. Negative numbers are global within a world, except they don't collide amongst themselves (all non-dynamic bodies should go in -1 because they don't collide amongst each-other, but do potentially collide with dynamics). Positive numbers only collide amongst themselves, and with negative groups, but not other positive groups. To avoid unwanted collisions, put bodies into separate groups. There is an automatic constraint that the two objects within a single joint do not collide with each other, so this does not need to be handled here.`, 4: `BodyObject identifies bodies that all belong to the same overall physical object (e.g., a robot or simulated animal). It has no implications for collision or physics simulation. Instead, it is used for external manipulation on entire objects at a time, e.g. via PositionObject. When using world replicas, the object indexes are copied, so objects are indexed by world and object id.`, 5: `BodyHSize is the half-size (e.g., radius) of the body. Values depend on shape type: X is generally radius, Y is half-height.`, 6: ``, 7: ``, 8: `BodyThick is the thickness of the body, as a hollow shape. If 0, then it is a solid shape (default).`, 9: `BodyMass is the mass of the object.`, 10: `BodyInvMass is 1/mass of the object or 0 if no mass.`, 11: `BodyBounce specifies the COR or coefficient of restitution (0..1), which determines how elastic the collision is, i.e., final velocity / initial velocity.`, 12: `BodyFriction is the standard coefficient for linear friction (mu).`, 13: `BodyFrictionTortion is resistance to spinning at the contact point.`, 14: `BodyFrictionRolling is resistance to rolling motion at contact.`, 15: `3D position of body (structural center).`, 16: ``, 17: ``, 18: `Quaternion rotation of body.`, 19: ``, 20: ``, 21: ``, 22: `Relative center-of-mass offset from 3D position of body.`, 23: ``, 24: ``, 25: `Inertia 3x3 matrix (column matrix organization, r,c labels).`, 26: ``, 27: ``, 28: ``, 29: ``, 30: ``, 31: ``, 32: ``, 33: ``, 34: `InvInertia inverse inertia 3x3 matrix (column matrix organization, r,c labels).`, 35: ``, 36: ``, 37: ``, 38: ``, 39: ``, 40: ``, 41: ``, 42: ``, 43: `radius for broadphase collision`} var _BodyVarsMap = map[BodyVars]string{0: `BodyShape`, 1: `BodyDynamic`, 2: `BodyWorld`, 3: `BodyGroup`, 4: `BodyObject`, 5: `BodyHSizeX`, 6: `BodyHSizeY`, 7: `BodyHSizeZ`, 8: `BodyThick`, 9: `BodyMass`, 10: `BodyInvMass`, 11: `BodyBounce`, 12: `BodyFriction`, 13: `BodyFrictionTortion`, 14: `BodyFrictionRolling`, 15: `BodyPosX`, 16: `BodyPosY`, 17: `BodyPosZ`, 18: `BodyQuatX`, 19: `BodyQuatY`, 20: `BodyQuatZ`, 21: `BodyQuatW`, 22: `BodyComX`, 23: `BodyComY`, 24: `BodyComZ`, 25: `BodyInertiaXX`, 26: `BodyInertiaYX`, 27: `BodyInertiaZX`, 28: `BodyInertiaXY`, 29: `BodyInertiaYY`, 30: `BodyInertiaZY`, 31: `BodyInertiaXZ`, 32: `BodyInertiaYZ`, 33: `BodyInertiaZZ`, 34: `BodyInvInertiaXX`, 35: `BodyInvInertiaYX`, 36: `BodyInvInertiaZX`, 37: `BodyInvInertiaXY`, 38: `BodyInvInertiaYY`, 39: `BodyInvInertiaZY`, 40: `BodyInvInertiaXZ`, 41: `BodyInvInertiaYZ`, 42: `BodyInvInertiaZZ`, 43: `BodyRadius`} @@ -295,7 +295,7 @@ const JointVarsN JointVars = 50 var _JointVarsValueMap = map[string]JointVars{`JointType`: 0, `JointEnabled`: 1, `JointParent`: 2, `JointChild`: 3, `JointPPosX`: 4, `JointPPosY`: 5, `JointPPosZ`: 6, `JointPQuatX`: 7, `JointPQuatY`: 8, `JointPQuatZ`: 9, `JointPQuatW`: 10, `JointCPosX`: 11, `JointCPosY`: 12, `JointCPosZ`: 13, `JointCQuatX`: 14, `JointCQuatY`: 15, `JointCQuatZ`: 16, `JointCQuatW`: 17, `JointLinearDoFN`: 18, `JointAngularDoFN`: 19, `JointDoF1`: 20, `JointDoF2`: 21, `JointDoF3`: 22, `JointDoF4`: 23, `JointDoF5`: 24, `JointDoF6`: 25, `JointPForceX`: 26, `JointPForceY`: 27, `JointPForceZ`: 28, `JointPTorqueX`: 29, `JointPTorqueY`: 30, `JointPTorqueZ`: 31, `JointCForceX`: 32, `JointCForceY`: 33, `JointCForceZ`: 34, `JointCTorqueX`: 35, `JointCTorqueY`: 36, `JointCTorqueZ`: 37, `JointPDeltaX`: 38, `JointPDeltaY`: 39, `JointPDeltaZ`: 40, `JointPAngDeltaX`: 41, `JointPAngDeltaY`: 42, `JointPAngDeltaZ`: 43, `JointCDeltaX`: 44, `JointCDeltaY`: 45, `JointCDeltaZ`: 46, `JointCAngDeltaX`: 47, `JointCAngDeltaY`: 48, `JointCAngDeltaZ`: 49} -var _JointVarsDescMap = map[JointVars]string{0: `JointType (as an int32 from bits).`, 1: `JointEnabled allows joints to be dynamically enabled.`, 2: `JointParent is the dynamic body index for parent body. Can be -1 for a fixed parent for absolute anchor.`, 3: `JointChild is the dynamic body index for child body.`, 4: `relative position of joint, in parent frame.`, 5: ``, 6: ``, 7: `relative orientation of joint, in parent frame.`, 8: ``, 9: ``, 10: ``, 11: `relative position of joint, in child frame.`, 12: ``, 13: ``, 14: `relative orientation of joint, in child frame.`, 15: ``, 16: ``, 17: ``, 18: `JointLinearDoFN is the number of linear degrees-of-freedom for the joint.`, 19: `JointAngularDoFN is the number of angular degrees-of-freedom for the joint.`, 20: `indexes in JointDoFs for each DoF`, 21: ``, 22: ``, 23: `angular starts here for Free, Distance, D6`, 24: ``, 25: ``, 26: `Computed parent joint force value.`, 27: ``, 28: ``, 29: `Computed parent joint torque value.`, 30: ``, 31: ``, 32: `Computed child joint force value.`, 33: ``, 34: ``, 35: `Computed child joint torque value.`, 36: ``, 37: ``, 38: `Computed parent joint delta value.`, 39: ``, 40: ``, 41: `Computed parent joint angdelta value.`, 42: ``, 43: ``, 44: `Computed child joint delta value.`, 45: ``, 46: ``, 47: `Computed child joint angdelta value.`, 48: ``, 49: ``} +var _JointVarsDescMap = map[JointVars]string{0: `JointType (as an int32 from bits).`, 1: `JointEnabled allows joints to be dynamically enabled.`, 2: `JointParent is the dynamic body index for parent body. Can be -1 for a fixed parent for absolute anchor.`, 3: `JointChild is the dynamic body index for child body.`, 4: `relative position of joint, in parent frame. This is prior to parent body rotation.`, 5: ``, 6: ``, 7: `relative orientation of joint, in parent frame. This is prior to parent body rotation.`, 8: ``, 9: ``, 10: ``, 11: `relative position of joint, in child frame. This is prior to child body rotation.`, 12: ``, 13: ``, 14: `relative orientation of joint, in child frame. This is prior to parent body rotation.`, 15: ``, 16: ``, 17: ``, 18: `JointLinearDoFN is the number of linear degrees-of-freedom for the joint.`, 19: `JointAngularDoFN is the number of angular degrees-of-freedom for the joint.`, 20: `indexes in JointDoFs for each DoF`, 21: ``, 22: ``, 23: `angular starts here for Free, Distance, D6`, 24: ``, 25: ``, 26: `Computed parent joint force value.`, 27: ``, 28: ``, 29: `Computed parent joint torque value.`, 30: ``, 31: ``, 32: `Computed child joint force value.`, 33: ``, 34: ``, 35: `Computed child joint torque value.`, 36: ``, 37: ``, 38: `Computed parent joint delta value.`, 39: ``, 40: ``, 41: `Computed parent joint angdelta value.`, 42: ``, 43: ``, 44: `Computed child joint delta value.`, 45: ``, 46: ``, 47: `Computed child joint angdelta value.`, 48: ``, 49: ``} var _JointVarsMap = map[JointVars]string{0: `JointType`, 1: `JointEnabled`, 2: `JointParent`, 3: `JointChild`, 4: `JointPPosX`, 5: `JointPPosY`, 6: `JointPPosZ`, 7: `JointPQuatX`, 8: `JointPQuatY`, 9: `JointPQuatZ`, 10: `JointPQuatW`, 11: `JointCPosX`, 12: `JointCPosY`, 13: `JointCPosZ`, 14: `JointCQuatX`, 15: `JointCQuatY`, 16: `JointCQuatZ`, 17: `JointCQuatW`, 18: `JointLinearDoFN`, 19: `JointAngularDoFN`, 20: `JointDoF1`, 21: `JointDoF2`, 22: `JointDoF3`, 23: `JointDoF4`, 24: `JointDoF5`, 25: `JointDoF6`, 26: `JointPForceX`, 27: `JointPForceY`, 28: `JointPForceZ`, 29: `JointPTorqueX`, 30: `JointPTorqueY`, 31: `JointPTorqueZ`, 32: `JointCForceX`, 33: `JointCForceY`, 34: `JointCForceZ`, 35: `JointCTorqueX`, 36: `JointCTorqueY`, 37: `JointCTorqueZ`, 38: `JointPDeltaX`, 39: `JointPDeltaY`, 40: `JointPDeltaZ`, 41: `JointPAngDeltaX`, 42: `JointPAngDeltaY`, 43: `JointPAngDeltaZ`, 44: `JointCDeltaX`, 45: `JointCDeltaY`, 46: `JointCDeltaZ`, 47: `JointCAngDeltaX`, 48: `JointCAngDeltaY`, 49: `JointCAngDeltaZ`} diff --git a/physics/examples/balls/balls.go b/physics/examples/balls/balls.go index ff329243..4dd50d3d 100644 --- a/physics/examples/balls/balls.go +++ b/physics/examples/balls/balls.go @@ -71,23 +71,23 @@ func main() { ed.SetUserParams(bs) ed.SetConfigFunc(func() { - ph := ed.Physics - wr := ed.World + ml := ed.Model + sc := ed.Scene rot := math32.NewQuat(0, 0, 0, 1) - wr.NewBody(ph, "floor", physics.Plane, "#D0D0D080", math32.Vec3(0, 0, 0), + sc.NewBody(ml, "floor", physics.Plane, "#D0D0D080", math32.Vec3(0, 0, 0), math32.Vec3(0, 0, 0), rot) hw := bs.Width / 2 hd := bs.Depth / 2 hh := bs.Height / 2 ht := bs.Thick / 2 - wr.NewBody(ph, "back-wall", physics.Box, "#0000FFA0", math32.Vec3(hw, hh, ht), + sc.NewBody(ml, "back-wall", physics.Box, "#0000FFA0", math32.Vec3(hw, hh, ht), math32.Vec3(0, hh, -hd), rot) - wr.NewBody(ph, "left-wall", physics.Box, "#FF0000A0", math32.Vec3(ht, hh, hd), + sc.NewBody(ml, "left-wall", physics.Box, "#FF0000A0", math32.Vec3(ht, hh, hd), math32.Vec3(-hw, hh, 0), rot) - wr.NewBody(ph, "right-wall", physics.Box, "#00FF00A0", math32.Vec3(ht, hh, hd), + sc.NewBody(ml, "right-wall", physics.Box, "#00FF00A0", math32.Vec3(ht, hh, hd), math32.Vec3(hw, hh, 0), rot) - wr.NewBody(ph, "front-wall", physics.Box, "#FFFF00A0", math32.Vec3(hw, hh, ht), + sc.NewBody(ml, "front-wall", physics.Box, "#FFFF00A0", math32.Vec3(hw, hh, ht), math32.Vec3(0, hh, hd), rot) box := bs.Width * .9 @@ -97,7 +97,7 @@ func main() { x := rand.Float32()*box - 0.5*box z := rand.Float32()*box - 0.5*box clr := colors.Names[i%len(colors.Names)] - bl := wr.NewDynamic(ph, "ball", physics.Sphere, clr, bs.Mass, math32.Vec3(size, size, size), + bl := sc.NewDynamic(ml, "ball", physics.Sphere, clr, bs.Mass, math32.Vec3(size, size, size), math32.Vec3(x, size+ht, z), rot) if !bs.Collide { physics.SetBodyGroup(bl.Index, int32(i+1)) // only collide within same group diff --git a/physics/examples/pendula/pendula.go b/physics/examples/pendula/pendula.go index 975e92a8..3fc6d96e 100644 --- a/physics/examples/pendula/pendula.go +++ b/physics/examples/pendula/pendula.go @@ -84,8 +84,8 @@ func main() { var botJoint int32 ed.SetConfigFunc(func() { - ph := ed.Physics - wr := ed.World + ml := ed.Model + sc := ed.Scene rot := math32.NewQuat(0, 0, 0, 1) rleft := math32.NewQuatAxisAngle(math32.Vec3(0, 0, 1), -math32.Pi/2) @@ -101,14 +101,14 @@ func main() { x = 0 y -= ps.HSize.Y } - pb := wr.NewDynamic(ph, "top", physics.Capsule, clr, ps.Mass, ps.HSize, math32.Vec3(x, y, 0), rleft) + pb := sc.NewDynamic(ml, "top", physics.Capsule, clr, ps.Mass, ps.HSize, math32.Vec3(x, y, 0), rleft) if !ps.Collide { pb.SetBodyGroup(1) } targ := math32.DegToRad(float32(ps.TargetDegFromVert)) - ji := pb.NewJointRevolute(ph, nil, math32.Vec3(0, stY, 0), math32.Vec3(0, ps.HSize.Y, 0), math32.Vec3(0, 0, 1)) + ji := pb.NewJointRevolute(ml, nil, math32.Vec3(0, stY, 0), math32.Vec3(0, ps.HSize.Y, 0), math32.Vec3(0, 0, 1)) physics.SetJointTargetPos(ji, 0, targ, ps.Stiff) physics.SetJointTargetVel(ji, 0, 0, ps.Damp) @@ -120,11 +120,11 @@ func main() { y = stY + x x = 0 } - cb := wr.NewDynamic(ph, "child", physics.Capsule, clr, ps.Mass, ps.HSize, math32.Vec3(x, y, 0), rleft) + cb := sc.NewDynamic(ml, "child", physics.Capsule, clr, ps.Mass, ps.HSize, math32.Vec3(x, y, 0), rleft) if !ps.Collide { cb.SetBodyGroup(1 + i) } - ji = cb.NewJointRevolute(ph, pb, math32.Vec3(0, -ps.HSize.Y, 0), math32.Vec3(0, ps.HSize.Y, 0), math32.Vec3(0, 0, 1)) + ji = cb.NewJointRevolute(ml, pb, math32.Vec3(0, -ps.HSize.Y, 0), math32.Vec3(0, ps.HSize.Y, 0), math32.Vec3(0, 0, 1)) physics.SetJointTargetPos(ji, 0, targ, ps.Stiff) physics.SetJointTargetVel(ji, 0, 0, ps.Damp) pb = cb diff --git a/physics/group.go b/physics/group.go deleted file mode 100644 index 11a2df13..00000000 --- a/physics/group.go +++ /dev/null @@ -1,244 +0,0 @@ -// Copyright (c) 2019, Cogent Core. All rights reserved. -// Use of this source code is governed by a BSD-style -// license that can be found in the LICENSE file. - -package physics - -// Group is a container of bodies or other groups. -// It should be used strategically to partition the space -// and its BBox is used to optimize tree-based collision detection. -// Use a group for the top-level World node. -// type Group struct { -// NodeBase -// } -// -// func (gp *Group) Update() { -// gp.RunUpdaters() -// gp.WalkDown(func(n tree.Node) bool { -// if n.AsTree().This == gp.This { -// return tree.Continue -// } -// n.(Node).Update() -// return tree.Continue -// }) -// } -// -// func (gp *Group) InitAbs(par *NodeBase) { -// gp.InitAbsBase(par) -// } -// -// func (gp *Group) RelToAbs(par *NodeBase) { -// gp.RelToAbsBase(par) // yes we can move groups -// } -// -// func (gp *Group) Step(step float32) { -// // groups do NOT update physics -// } -// -// func (gp *Group) GroupBBox() { -// hasDyn := false -// gp.BBox.BBox.SetEmpty() -// gp.BBox.VelBBox.SetEmpty() -// for _, kid := range gp.Children { -// n, nb := AsNode(kid) -// if n == nil { -// continue -// } -// gp.BBox.BBox.ExpandByBox(nb.BBox.BBox) -// gp.BBox.VelBBox.ExpandByBox(nb.BBox.VelBBox) -// if nb.Dynamic { -// hasDyn = true -// } -// } -// gp.SetDynamic(hasDyn) -// } -// -// // WorldDynGroupBBox does a GroupBBox on all dynamic nodes -// func (gp *Group) WorldDynGroupBBox() { -// gp.WalkDownPost(func(tn tree.Node) bool { -// n, nb := AsNode(tn) -// if n == nil { -// return false -// } -// if !nb.Dynamic { -// return false -// } -// return true -// }, func(tn tree.Node) bool { -// n, nb := AsNode(tn) -// if n == nil { -// return false -// } -// if !nb.Dynamic { -// return false -// } -// n.GroupBBox() -// return true -// }) -// } -// -// // WorldInit does the full tree InitAbs and GroupBBox updates -// func (gp *Group) WorldInit() { -// gp.Update() -// gp.WalkDown(func(tn tree.Node) bool { -// n, _ := AsNode(tn) -// if n == nil { -// return false -// } -// _, pi := AsNode(tn.AsTree().Parent) -// n.InitAbs(pi) -// return true -// }) -// -// gp.WalkDownPost(func(tn tree.Node) bool { -// n, _ := AsNode(tn) -// if n == nil { -// return false -// } -// return true -// }, func(tn tree.Node) bool { -// n, _ := AsNode(tn) -// if n == nil { -// return false -// } -// n.GroupBBox() -// return true -// }) -// -// } -// -// // WorldRelToAbs does a full RelToAbs update for all Dynamic groups, for -// // Scripted mode updates with manual updating of Rel values. -// func (gp *Group) WorldRelToAbs() { -// gp.WalkDown(func(tn tree.Node) bool { -// n, nb := AsNode(tn) -// if n == nil { -// return false // going into a different type of thing, bail -// } -// if !nb.Dynamic { -// return false -// } -// _, pi := AsNode(tn.AsTree().Parent) -// n.RelToAbs(pi) -// return true -// }) -// -// gp.WorldDynGroupBBox() -// } -// -// // WorldStep does a full Step update for all Dynamic nodes, for -// // either physics or scripted mode, based on current velocities. -// func (gp *Group) WorldStep(step float32) { -// gp.WalkDown(func(tn tree.Node) bool { -// n, nb := AsNode(tn) -// if n == nil { -// return false // going into a different type of thing, bail -// } -// if !nb.Dynamic { -// return false -// } -// n.Step(step) -// return true -// }) -// -// gp.WorldDynGroupBBox() -// } -// -// const ( -// // DynsTopGps is passed to WorldCollide when all dynamic objects are in separate top groups -// DynsTopGps = true -// -// // DynsSubGps is passed to WorldCollide when all dynamic objects are in separate groups under top -// // level (i.e., one level deeper) -// DynsSubGps -// ) -// -// // WorldCollide does first pass filtering step of collision detection -// // based on separate dynamic vs. dynamic and dynamic vs. static groups. -// // If dynTop is true, then each Dynamic group is separate at the top level -- -// // otherwise they are organized at the next group level. -// // Contacts are organized by dynamic group, when non-nil, for easier -// // processing. -// func (gp *Group) WorldCollide(dynTop bool) []Contacts { -// var stats []Node -// var dyns []Node -// for _, kid := range gp.Children { -// n, nb := AsNode(kid) -// if n == nil { -// continue -// } -// if nb.Dynamic { -// dyns = append(dyns, n) -// } else { -// stats = append(stats, n) -// } -// } -// -// var sdyns []Node -// if !dynTop { -// for _, d := range dyns { -// for _, dk := range d.AsTree().Children { -// nii, _ := AsNode(dk) -// if nii == nil { -// continue -// } -// sdyns = append(sdyns, nii) -// } -// } -// dyns = sdyns -// } -// -// var cts []Contacts -// for i, d := range dyns { -// var dct Contacts -// for _, s := range stats { -// cc := BodyVelBBoxIntersects(d, s) -// dct = append(dct, cc...) -// } -// for di := 0; di < i; di++ { -// od := dyns[di] -// cc := BodyVelBBoxIntersects(d, od) -// dct = append(dct, cc...) -// } -// if len(dct) > 0 { -// cts = append(cts, dct) -// } -// } -// return cts -// } -// -// // BodyPoint contains a Body and a Point on that body -// type BodyPoint struct { -// Body Body -// Point math32.Vector3 -// } -// -// // RayBodyIntersections returns a list of bodies whose bounding box intersects -// // with the given ray, with the point of intersection -// func (gp *Group) RayBodyIntersections(ray math32.Ray) []*BodyPoint { -// var bs []*BodyPoint -// gp.WalkDown(func(k tree.Node) bool { -// nii, ni := AsNode(k) -// if nii == nil { -// return false // going into a different type of thing, bail -// } -// pt, has := ray.IntersectBox(ni.BBox.BBox) -// if !has { -// return false -// } -// bd := nii.AsBody() -// if bd == nil { -// return true -// } -// bs = append(bs, &BodyPoint{bd, pt}) -// return false -// }) -// -// sort.Slice(bs, func(i, j int) bool { -// di := bs[i].Point.DistanceTo(ray.Origin) -// dj := bs[j].Point.DistanceTo(ray.Origin) -// return di < dj -// }) -// -// return bs -//} diff --git a/physics/joint.go b/physics/joint.go index c7d3f2ad..aa3f5285 100644 --- a/physics/joint.go +++ b/physics/joint.go @@ -63,22 +63,26 @@ const ( JointChild // relative position of joint, in parent frame. + // This is prior to parent body rotation. JointPPosX JointPPosY JointPPosZ // relative orientation of joint, in parent frame. + // This is prior to parent body rotation. JointPQuatX JointPQuatY JointPQuatZ JointPQuatW // relative position of joint, in child frame. + // This is prior to child body rotation. JointCPosX JointCPosY JointCPosZ // relative orientation of joint, in child frame. + // This is prior to parent body rotation. JointCQuatX JointCQuatY JointCQuatZ @@ -367,13 +371,13 @@ func SetJointDoF(idx, dof int32, vr JointDoFVars, value float32) { //gosl:end -func (wl *World) JointDefaults(idx int32) { +func (ml *Model) JointDefaults(idx int32) { rot := math32.NewQuat(0, 0, 0, 1) SetJointPQuat(idx, rot) SetJointCQuat(idx, rot) } -func (wl *World) JointDoFDefaults(didx int32) { +func (ml *Model) JointDoFDefaults(didx int32) { JointDoFs.Set(-JointLimitUnlimited, int(didx), int(JointLimitLower)) JointDoFs.Set(JointLimitUnlimited, int(didx), int(JointLimitUpper)) JointControls.Set(1, int(didx), int(JointTargetDamp)) @@ -384,8 +388,8 @@ func (wl *World) JointDoFDefaults(didx int32) { // Use -1 for parent to add a world-anchored joint. // ppos, cpos are the relative positions from the parent, child. // Sets relative rotation matricies to identity by default. -func (wl *World) NewJointFixed(parent, child int32, ppos, cpos math32.Vector3) int32 { - idx := wl.newJoint(Fixed, parent, child, ppos, cpos) +func (ml *Model) NewJointFixed(parent, child int32, ppos, cpos math32.Vector3) int32 { + idx := ml.newJoint(Fixed, parent, child, ppos, cpos) return idx } @@ -396,10 +400,10 @@ func (wl *World) NewJointFixed(parent, child int32, ppos, cpos math32.Vector3) i // Sets relative rotation matricies to identity by default. // axis is the axis of articulation for the joint. // Use [SetJointDoF] to set the remaining DoF parameters. -func (wl *World) NewJointPrismatic(parent, child int32, ppos, cpos, axis math32.Vector3) int32 { - idx := wl.newJoint(Prismatic, parent, child, ppos, cpos) +func (ml *Model) NewJointPrismatic(parent, child int32, ppos, cpos, axis math32.Vector3) int32 { + idx := ml.newJoint(Prismatic, parent, child, ppos, cpos) SetJointLinearDoFN(idx, 1) - wl.newJointDoF(idx, 0, axis) + ml.newJointDoF(idx, 0, axis) return idx } @@ -410,10 +414,10 @@ func (wl *World) NewJointPrismatic(parent, child int32, ppos, cpos, axis math32. // Sets relative rotation matricies to identity by default. // axis is the axis of articulation for the joint. // Use [SetJointDoF] to set the remaining DoF parameters. -func (wl *World) NewJointRevolute(parent, child int32, ppos, cpos, axis math32.Vector3) int32 { - idx := wl.newJoint(Revolute, parent, child, ppos, cpos) +func (ml *Model) NewJointRevolute(parent, child int32, ppos, cpos, axis math32.Vector3) int32 { + idx := ml.newJoint(Revolute, parent, child, ppos, cpos) SetJointAngularDoFN(idx, 1) - wl.newJointDoF(idx, 0, axis) + ml.newJointDoF(idx, 0, axis) return idx } @@ -423,13 +427,13 @@ func (wl *World) NewJointRevolute(parent, child int32, ppos, cpos, axis math32.V // ppos, cpos are the relative positions from the parent, child. // Sets relative rotation matricies to identity by default. // Use [SetJointDoF] to set the remaining DoF parameters. -func (wl *World) NewJointBall(parent, child int32, ppos, cpos math32.Vector3) int32 { - idx := wl.newJoint(Ball, parent, child, ppos, cpos) +func (ml *Model) NewJointBall(parent, child int32, ppos, cpos math32.Vector3) int32 { + idx := ml.newJoint(Ball, parent, child, ppos, cpos) SetJointAngularDoFN(idx, 3) for d := range math32.W { axis := math32.Vector3{} axis.SetDim(d, 1) - wl.newJointDoF(idx, int32(d), axis) + ml.newJointDoF(idx, int32(d), axis) } return idx } @@ -441,19 +445,19 @@ func (wl *World) NewJointBall(parent, child int32, ppos, cpos math32.Vector3) in // ppos, cpos are the relative positions from the parent, child. // Sets relative rotation matricies to identity by default. // Use [SetJointDoF] to set the remaining DoF parameters. -func (wl *World) NewJointDistance(parent, child int32, ppos, cpos math32.Vector3, minDist, maxDist float32) int32 { - idx := wl.newJoint(Distance, parent, child, ppos, cpos) +func (ml *Model) NewJointDistance(parent, child int32, ppos, cpos math32.Vector3, minDist, maxDist float32) int32 { + idx := ml.newJoint(Distance, parent, child, ppos, cpos) SetJointLinearDoFN(idx, 3) SetJointAngularDoFN(idx, 3) for d := range math32.W { axis := math32.Vector3{} axis.SetDim(d, 1) - wl.newJointDoF(idx, int32(d), axis) + ml.newJointDoF(idx, int32(d), axis) } for d := range math32.W { axis := math32.Vector3{} axis.SetDim(d, 1) - wl.newJointDoF(idx, int32(d), axis) + ml.newJointDoF(idx, int32(d), axis) } // only on the X linear axis SetJointDoF(idx, 0, JointLimitLower, minDist) @@ -468,19 +472,19 @@ func (wl *World) NewJointDistance(parent, child int32, ppos, cpos math32.Vector3 // ppos, cpos are the relative positions from the parent, child. // Sets relative rotation matricies to identity by default. // Use [SetJointDoF] to set the remaining DoF parameters. -func (wl *World) NewJointFree(parent, child int32, ppos, cpos math32.Vector3) int32 { - idx := wl.newJoint(Distance, parent, child, ppos, cpos) +func (ml *Model) NewJointFree(parent, child int32, ppos, cpos math32.Vector3) int32 { + idx := ml.newJoint(Distance, parent, child, ppos, cpos) SetJointLinearDoFN(idx, 0) SetJointAngularDoFN(idx, 0) for d := range math32.W { axis := math32.Vector3{} axis.SetDim(d, 1) - wl.newJointDoF(idx, int32(d), axis) + ml.newJointDoF(idx, int32(d), axis) } for d := range math32.W { axis := math32.Vector3{} axis.SetDim(d, 1) - wl.newJointDoF(idx, int32(d), axis) + ml.newJointDoF(idx, int32(d), axis) } return idx } @@ -490,12 +494,12 @@ func (wl *World) NewJointFree(parent, child int32, ppos, cpos math32.Vector3) in // Use -1 for parent to add a world-anchored joint. // ppos, cpos are the relative positions from the parent, child. // Sets relative rotation matricies to identity by default. -func (wl *World) newJoint(joint JointTypes, parent, child int32, ppos, cpos math32.Vector3) int32 { - sizes := wl.Joints.ShapeSizes() +func (ml *Model) newJoint(joint JointTypes, parent, child int32, ppos, cpos math32.Vector3) int32 { + sizes := ml.Joints.ShapeSizes() idx := int32(sizes[0]) - wl.Params[0].JointsN = idx + 1 - wl.Joints.SetShapeSizes(int(idx+1), int(JointVarsN)) - wl.JointDefaults(idx) + ml.Params[0].JointsN = idx + 1 + ml.Joints.SetShapeSizes(int(idx+1), int(JointVarsN)) + ml.JointDefaults(idx) SetJointType(idx, joint) SetJointEnabled(idx, true) SetJointParent(idx, parent) @@ -507,13 +511,13 @@ func (wl *World) newJoint(joint JointTypes, parent, child int32, ppos, cpos math // newJointDoF adds new JointDoFs and JointControls entries // initialized to detfaults. Returns index. -func (wl *World) newJointDoF(jidx, dof int32, axis math32.Vector3) int32 { - sizes := wl.JointDoFs.ShapeSizes() +func (ml *Model) newJointDoF(jidx, dof int32, axis math32.Vector3) int32 { + sizes := ml.JointDoFs.ShapeSizes() didx := int32(sizes[0]) - wl.JointDoFs.SetShapeSizes(int(didx+1), int(JointDoFVarsN)) - wl.JointControls.SetShapeSizes(int(didx+1), int(JointControlVarsN)) - wl.Params[0].JointDoFsN = didx + 1 - wl.JointDoFDefaults(didx) + ml.JointDoFs.SetShapeSizes(int(didx+1), int(JointDoFVarsN)) + ml.JointControls.SetShapeSizes(int(didx+1), int(JointControlVarsN)) + ml.Params[0].JointDoFsN = didx + 1 + ml.JointDoFDefaults(didx) SetJointDoFIndex(jidx, dof, didx) SetJointAxis(jidx, dof, axis) return didx diff --git a/physics/joint.goal b/physics/joint.goal index 630059d5..79a8d7b6 100644 --- a/physics/joint.goal +++ b/physics/joint.goal @@ -61,22 +61,26 @@ const ( JointChild // relative position of joint, in parent frame. + // This is prior to parent body rotation. JointPPosX JointPPosY JointPPosZ // relative orientation of joint, in parent frame. + // This is prior to parent body rotation. JointPQuatX JointPQuatY JointPQuatZ JointPQuatW // relative position of joint, in child frame. + // This is prior to child body rotation. JointCPosX JointCPosY JointCPosZ // relative orientation of joint, in child frame. + // This is prior to parent body rotation. JointCQuatX JointCQuatY JointCQuatZ @@ -367,13 +371,13 @@ func SetJointDoF(idx, dof int32, vr JointDoFVars, value float32) { //gosl:end -func (wl *World) JointDefaults(idx int32) { +func (ml *Model) JointDefaults(idx int32) { rot := math32.NewQuat(0, 0, 0, 1) SetJointPQuat(idx, rot) SetJointCQuat(idx, rot) } -func (wl *World) JointDoFDefaults(didx int32) { +func (ml *Model) JointDoFDefaults(didx int32) { JointDoFs[didx, JointLimitLower] = -JointLimitUnlimited JointDoFs[didx, JointLimitUpper] = JointLimitUnlimited JointControls[didx, JointTargetDamp] = 1 @@ -384,8 +388,8 @@ func (wl *World) JointDoFDefaults(didx int32) { // Use -1 for parent to add a world-anchored joint. // ppos, cpos are the relative positions from the parent, child. // Sets relative rotation matricies to identity by default. -func (wl *World) NewJointFixed(parent, child int32, ppos, cpos math32.Vector3) int32 { - idx := wl.newJoint(Fixed, parent, child, ppos, cpos) +func (ml *Model) NewJointFixed(parent, child int32, ppos, cpos math32.Vector3) int32 { + idx := ml.newJoint(Fixed, parent, child, ppos, cpos) return idx } @@ -396,10 +400,10 @@ func (wl *World) NewJointFixed(parent, child int32, ppos, cpos math32.Vector3) i // Sets relative rotation matricies to identity by default. // axis is the axis of articulation for the joint. // Use [SetJointDoF] to set the remaining DoF parameters. -func (wl *World) NewJointPrismatic(parent, child int32, ppos, cpos, axis math32.Vector3) int32 { - idx := wl.newJoint(Prismatic, parent, child, ppos, cpos) +func (ml *Model) NewJointPrismatic(parent, child int32, ppos, cpos, axis math32.Vector3) int32 { + idx := ml.newJoint(Prismatic, parent, child, ppos, cpos) SetJointLinearDoFN(idx, 1) - wl.newJointDoF(idx, 0, axis) + ml.newJointDoF(idx, 0, axis) return idx } @@ -410,10 +414,10 @@ func (wl *World) NewJointPrismatic(parent, child int32, ppos, cpos, axis math32. // Sets relative rotation matricies to identity by default. // axis is the axis of articulation for the joint. // Use [SetJointDoF] to set the remaining DoF parameters. -func (wl *World) NewJointRevolute(parent, child int32, ppos, cpos, axis math32.Vector3) int32 { - idx := wl.newJoint(Revolute, parent, child, ppos, cpos) +func (ml *Model) NewJointRevolute(parent, child int32, ppos, cpos, axis math32.Vector3) int32 { + idx := ml.newJoint(Revolute, parent, child, ppos, cpos) SetJointAngularDoFN(idx, 1) - wl.newJointDoF(idx, 0, axis) + ml.newJointDoF(idx, 0, axis) return idx } @@ -423,13 +427,13 @@ func (wl *World) NewJointRevolute(parent, child int32, ppos, cpos, axis math32.V // ppos, cpos are the relative positions from the parent, child. // Sets relative rotation matricies to identity by default. // Use [SetJointDoF] to set the remaining DoF parameters. -func (wl *World) NewJointBall(parent, child int32, ppos, cpos math32.Vector3) int32 { - idx := wl.newJoint(Ball, parent, child, ppos, cpos) +func (ml *Model) NewJointBall(parent, child int32, ppos, cpos math32.Vector3) int32 { + idx := ml.newJoint(Ball, parent, child, ppos, cpos) SetJointAngularDoFN(idx, 3) for d := range math32.W { axis := math32.Vector3{} axis.SetDim(d, 1) - wl.newJointDoF(idx, int32(d), axis) + ml.newJointDoF(idx, int32(d), axis) } return idx } @@ -441,19 +445,19 @@ func (wl *World) NewJointBall(parent, child int32, ppos, cpos math32.Vector3) in // ppos, cpos are the relative positions from the parent, child. // Sets relative rotation matricies to identity by default. // Use [SetJointDoF] to set the remaining DoF parameters. -func (wl *World) NewJointDistance(parent, child int32, ppos, cpos math32.Vector3, minDist, maxDist float32) int32 { - idx := wl.newJoint(Distance, parent, child, ppos, cpos) +func (ml *Model) NewJointDistance(parent, child int32, ppos, cpos math32.Vector3, minDist, maxDist float32) int32 { + idx := ml.newJoint(Distance, parent, child, ppos, cpos) SetJointLinearDoFN(idx, 3) SetJointAngularDoFN(idx, 3) for d := range math32.W { axis := math32.Vector3{} axis.SetDim(d, 1) - wl.newJointDoF(idx, int32(d), axis) + ml.newJointDoF(idx, int32(d), axis) } for d := range math32.W { axis := math32.Vector3{} axis.SetDim(d, 1) - wl.newJointDoF(idx, int32(d), axis) + ml.newJointDoF(idx, int32(d), axis) } // only on the X linear axis SetJointDoF(idx, 0, JointLimitLower, minDist) @@ -468,19 +472,19 @@ func (wl *World) NewJointDistance(parent, child int32, ppos, cpos math32.Vector3 // ppos, cpos are the relative positions from the parent, child. // Sets relative rotation matricies to identity by default. // Use [SetJointDoF] to set the remaining DoF parameters. -func (wl *World) NewJointFree(parent, child int32, ppos, cpos math32.Vector3) int32 { - idx := wl.newJoint(Distance, parent, child, ppos, cpos) +func (ml *Model) NewJointFree(parent, child int32, ppos, cpos math32.Vector3) int32 { + idx := ml.newJoint(Distance, parent, child, ppos, cpos) SetJointLinearDoFN(idx, 0) SetJointAngularDoFN(idx, 0) for d := range math32.W { axis := math32.Vector3{} axis.SetDim(d, 1) - wl.newJointDoF(idx, int32(d), axis) + ml.newJointDoF(idx, int32(d), axis) } for d := range math32.W { axis := math32.Vector3{} axis.SetDim(d, 1) - wl.newJointDoF(idx, int32(d), axis) + ml.newJointDoF(idx, int32(d), axis) } return idx } @@ -490,12 +494,12 @@ func (wl *World) NewJointFree(parent, child int32, ppos, cpos math32.Vector3) in // Use -1 for parent to add a world-anchored joint. // ppos, cpos are the relative positions from the parent, child. // Sets relative rotation matricies to identity by default. -func (wl *World) newJoint(joint JointTypes, parent, child int32, ppos, cpos math32.Vector3) int32 { - sizes := wl.Joints.ShapeSizes() +func (ml *Model) newJoint(joint JointTypes, parent, child int32, ppos, cpos math32.Vector3) int32 { + sizes := ml.Joints.ShapeSizes() idx := int32(sizes[0]) - wl.Params[0].JointsN = idx + 1 - wl.Joints.SetShapeSizes(int(idx+1), int(JointVarsN)) - wl.JointDefaults(idx) + ml.Params[0].JointsN = idx + 1 + ml.Joints.SetShapeSizes(int(idx+1), int(JointVarsN)) + ml.JointDefaults(idx) SetJointType(idx, joint) SetJointEnabled(idx, true) SetJointParent(idx, parent) @@ -507,13 +511,13 @@ func (wl *World) newJoint(joint JointTypes, parent, child int32, ppos, cpos math // newJointDoF adds new JointDoFs and JointControls entries // initialized to detfaults. Returns index. -func (wl *World) newJointDoF(jidx, dof int32, axis math32.Vector3) int32 { - sizes := wl.JointDoFs.ShapeSizes() +func (ml *Model) newJointDoF(jidx, dof int32, axis math32.Vector3) int32 { + sizes := ml.JointDoFs.ShapeSizes() didx := int32(sizes[0]) - wl.JointDoFs.SetShapeSizes(int(didx+1), int(JointDoFVarsN)) - wl.JointControls.SetShapeSizes(int(didx+1), int(JointControlVarsN)) - wl.Params[0].JointDoFsN = didx + 1 - wl.JointDoFDefaults(didx) + ml.JointDoFs.SetShapeSizes(int(didx+1), int(JointDoFVarsN)) + ml.JointControls.SetShapeSizes(int(didx+1), int(JointControlVarsN)) + ml.Params[0].JointDoFsN = didx + 1 + ml.JointDoFDefaults(didx) SetJointDoFIndex(jidx, dof, didx) SetJointAxis(jidx, dof, axis) return didx diff --git a/physics/world.go b/physics/model.go similarity index 68% rename from physics/world.go rename to physics/model.go index e1f8cf48..8614b5a5 100644 --- a/physics/world.go +++ b/physics/model.go @@ -1,5 +1,5 @@ // Code generated by "goal build"; DO NOT EDIT. -//line world.goal:1 +//line model.goal:1 // Copyright (c) 2025, Cogent Core. All rights reserved. // Use of this source code is governed by a BSD-style // license that can be found in the LICENSE file. @@ -13,8 +13,8 @@ import ( //go:generate core generate -add-types -gosl -// World contains and manages all of the physics elements. -type World struct { +// Model contains and manages all of the physics elements. +type Model struct { // GPU determines whether to use GPU (else CPU). GPU bool @@ -26,15 +26,15 @@ type World struct { // while 0 and positive numbers only interact amongst themselves. CurrentWorld int - // WorldReplicasStart is the starting body index for replicated world bodies, + // ReplicasStart is the starting body index for replicated world bodies, // which is needed for viewers to efficiently select a specific world to view. // This is the start of the World=0 first instance. - WorldReplicasStart int32 + ReplicasStart int32 - // WorldReplicasN is the number of body elements within each set of + // ReplicasN is the number of body elements within each set of // replicated world bodies, which is needed for viewers to efficiently select // a specific world to view. - WorldReplicasN int32 + ReplicasN int32 // CurrentObject is the [BodyObject] value to use when creating new bodies. // Generally just increment when starting a new object. When using world replicas, @@ -95,71 +95,71 @@ type World struct { JointControls *tensor.Float32 `display:"no-inline"` } -func NewWorld() *World { - ph := &World{} - ph.Init() - return ph +func NewModel() *Model { + ml := &Model{} + ml.Init() + return ml } -// Init makes initial vars. Called in NewWorld. +// Init makes initial vars. Called in NewModel. // Must call Config once configured. -func (ph *World) Init() { - ph.GPU = true - ph.Params = make([]PhysParams, 1) - ph.Params[0].Defaults() - ph.Reset() +func (ml *Model) Init() { + ml.GPU = true + ml.Params = make([]PhysParams, 1) + ml.Params[0].Defaults() + ml.Reset() } // Reset resets all data to empty: starting over. -func (ph *World) Reset() { - ph.Bodies = tensor.NewFloat32(0, int(BodyVarsN)) - ph.Joints = tensor.NewFloat32(0, int(JointVarsN)) - ph.JointDoFs = tensor.NewFloat32(0, int(JointDoFVarsN)) - ph.BodyJoints = tensor.NewInt32(0, 2, 2) - ph.BodyCollidePairs = tensor.NewInt32(0, 2) - ph.Dynamics = tensor.NewFloat32(0, 2, int(DynamicVarsN)) - ph.BroadContactsN = tensor.NewInt32(1) - ph.BroadContacts = tensor.NewFloat32(0, int(ContactVarsN)) - ph.ContactsN = tensor.NewInt32(1) - ph.Contacts = tensor.NewFloat32(0, int(ContactVarsN)) - ph.JointControls = tensor.NewFloat32(0, int(JointControlVarsN)) - ph.SetAsCurrentVars() +func (ml *Model) Reset() { + ml.Bodies = tensor.NewFloat32(0, int(BodyVarsN)) + ml.Joints = tensor.NewFloat32(0, int(JointVarsN)) + ml.JointDoFs = tensor.NewFloat32(0, int(JointDoFVarsN)) + ml.BodyJoints = tensor.NewInt32(0, 2, 2) + ml.BodyCollidePairs = tensor.NewInt32(0, 2) + ml.Dynamics = tensor.NewFloat32(0, 2, int(DynamicVarsN)) + ml.BroadContactsN = tensor.NewInt32(1) + ml.BroadContacts = tensor.NewFloat32(0, int(ContactVarsN)) + ml.ContactsN = tensor.NewInt32(1) + ml.Contacts = tensor.NewFloat32(0, int(ContactVarsN)) + ml.JointControls = tensor.NewFloat32(0, int(JointControlVarsN)) + ml.SetAsCurrentVars() } // NewBody adds a new body with given parameters. Returns the index. // Use this for Static elements; NewDynamic for dynamic elements. -func (ph *World) NewBody(shape Shapes, size, pos math32.Vector3, rot math32.Quat) int32 { - sizes := ph.Bodies.ShapeSizes() +func (ml *Model) NewBody(shape Shapes, size, pos math32.Vector3, rot math32.Quat) int32 { + sizes := ml.Bodies.ShapeSizes() idx := int32(sizes[0]) - ph.Bodies.SetShapeSizes(int(idx+1), int(BodyVarsN)) - ph.Params[0].BodiesN = idx + 1 + ml.Bodies.SetShapeSizes(int(idx+1), int(BodyVarsN)) + ml.Params[0].BodiesN = idx + 1 SetBodyShape(idx, shape) SetBodyDynamic(idx, -1) SetBodyHSize(idx, size) SetBodyPos(idx, pos) SetBodyQuat(idx, rot) SetBodyGroup(idx, -1) // assume static - SetBodyWorld(idx, int32(ph.CurrentWorld)) - SetBodyObject(idx, int32(ph.CurrentObject)) - ph.SetMass(idx, shape, size, 0) // assume static + SetBodyWorld(idx, int32(ml.CurrentWorld)) + SetBodyObject(idx, int32(ml.CurrentObject)) + ml.SetMass(idx, shape, size, 0) // assume static return idx } // NewDynamic adds a new dynamic body with given parameters. Returns the index. // Shape cannot be [Plane]. -func (ph *World) NewDynamic(shape Shapes, mass float32, size, pos math32.Vector3, rot math32.Quat) (bodyIdx, dynIdx int32) { +func (ml *Model) NewDynamic(shape Shapes, mass float32, size, pos math32.Vector3, rot math32.Quat) (bodyIdx, dynIdx int32) { if shape == Plane { panic("physics.NewDynamic: shape cannot be Plane") } - bodyIdx = ph.NewBody(shape, size, pos, rot) - sizes := ph.Dynamics.ShapeSizes() + bodyIdx = ml.NewBody(shape, size, pos, rot) + sizes := ml.Dynamics.ShapeSizes() dynIdx = int32(sizes[0]) - ph.Dynamics.SetShapeSizes(int(dynIdx+1), 2, int(DynamicVarsN)) - ph.Params[0].DynamicsN = dynIdx + 1 + ml.Dynamics.SetShapeSizes(int(dynIdx+1), 2, int(DynamicVarsN)) + ml.Params[0].DynamicsN = dynIdx + 1 SetDynamicBody(dynIdx, bodyIdx) SetBodyDynamic(bodyIdx, dynIdx) SetBodyGroup(bodyIdx, 1) // dynamic - ph.SetMass(bodyIdx, shape, size, mass) + ml.SetMass(bodyIdx, shape, size, mass) return } @@ -167,42 +167,42 @@ func (ph *World) NewDynamic(shape Shapes, mass float32, size, pos math32.Vector3 // processed in the code (on the GPU). If this was not the setter of // the current variables, then the parameter variables are copied up // to the GPU. -func (ph *World) SetAsCurrent() { - isCur := (Bodies == ph.Bodies) - ph.SetAsCurrentVars() +func (ml *Model) SetAsCurrent() { + isCur := (Bodies == ml.Bodies) + ml.SetAsCurrentVars() if GPUInitialized && !isCur { - ph.ToGPUInfra() + ml.ToGPUInfra() } } // SetAsCurrentVars sets these as the current global values that are // processed in the code (on the GPU). -func (ph *World) SetAsCurrentVars() { - Params = ph.Params - Bodies = ph.Bodies - Joints = ph.Joints - JointDoFs = ph.JointDoFs - BodyJoints = ph.BodyJoints - BodyCollidePairs = ph.BodyCollidePairs - Dynamics = ph.Dynamics - BroadContactsN = ph.BroadContactsN - BroadContacts = ph.BroadContacts - ContactsN = ph.ContactsN - Contacts = ph.Contacts - JointControls = ph.JointControls +func (ml *Model) SetAsCurrentVars() { + Params = ml.Params + Bodies = ml.Bodies + Joints = ml.Joints + JointDoFs = ml.JointDoFs + BodyJoints = ml.BodyJoints + BodyCollidePairs = ml.BodyCollidePairs + Dynamics = ml.Dynamics + BroadContactsN = ml.BroadContactsN + BroadContacts = ml.BroadContacts + ContactsN = ml.ContactsN + Contacts = ml.Contacts + JointControls = ml.JointControls } // GPUInit initializes the GPU and transfers Infra. // Should have already called SetAsCurrent (needed for CPU and GPU). -func (ph *World) GPUInit() { +func (ml *Model) GPUInit() { GPUInit() - UseGPU = ph.GPU - ph.ToGPUInfra() + UseGPU = ml.GPU + ml.ToGPUInfra() } // ToGPUInfra copies all the infrastructure for these filters up to // the GPU. This is done in GPUInit, and if current switched. -func (ph *World) ToGPUInfra() { +func (ml *Model) ToGPUInfra() { ToGPUTensorStrides() ToGPU(ParamsVar, BodiesVar, JointsVar, JointDoFsVar, BodyJointsVar, BodyCollidePairsVar, DynamicsVar, BroadContactsNVar, BroadContactsVar, ContactsNVar, ContactsVar, JointControlsVar) } diff --git a/physics/world.goal b/physics/model.goal similarity index 68% rename from physics/world.goal rename to physics/model.goal index 9d73107c..9e45fbf6 100644 --- a/physics/world.goal +++ b/physics/model.goal @@ -11,8 +11,8 @@ import ( //go:generate core generate -add-types -gosl -// World contains and manages all of the physics elements. -type World struct { +// Model contains and manages all of the physics elements. +type Model struct { // GPU determines whether to use GPU (else CPU). GPU bool @@ -24,15 +24,15 @@ type World struct { // while 0 and positive numbers only interact amongst themselves. CurrentWorld int - // WorldReplicasStart is the starting body index for replicated world bodies, + // ReplicasStart is the starting body index for replicated world bodies, // which is needed for viewers to efficiently select a specific world to view. // This is the start of the World=0 first instance. - WorldReplicasStart int32 + ReplicasStart int32 - // WorldReplicasN is the number of body elements within each set of + // ReplicasN is the number of body elements within each set of // replicated world bodies, which is needed for viewers to efficiently select // a specific world to view. - WorldReplicasN int32 + ReplicasN int32 // CurrentObject is the [BodyObject] value to use when creating new bodies. // Generally just increment when starting a new object. When using world replicas, @@ -93,71 +93,71 @@ type World struct { JointControls *tensor.Float32 `display:"no-inline"` } -func NewWorld() *World { - ph := &World{} - ph.Init() - return ph +func NewModel() *Model { + ml := &Model{} + ml.Init() + return ml } -// Init makes initial vars. Called in NewWorld. +// Init makes initial vars. Called in NewModel. // Must call Config once configured. -func (ph *World) Init() { - ph.GPU = true - ph.Params = make([]PhysParams, 1) - ph.Params[0].Defaults() - ph.Reset() +func (ml *Model) Init() { + ml.GPU = true + ml.Params = make([]PhysParams, 1) + ml.Params[0].Defaults() + ml.Reset() } // Reset resets all data to empty: starting over. -func (ph *World) Reset() { - ph.Bodies = tensor.NewFloat32(0, int(BodyVarsN)) - ph.Joints = tensor.NewFloat32(0, int(JointVarsN)) - ph.JointDoFs = tensor.NewFloat32(0, int(JointDoFVarsN)) - ph.BodyJoints = tensor.NewInt32(0, 2, 2) - ph.BodyCollidePairs = tensor.NewInt32(0, 2) - ph.Dynamics = tensor.NewFloat32(0, 2, int(DynamicVarsN)) - ph.BroadContactsN = tensor.NewInt32(1) - ph.BroadContacts = tensor.NewFloat32(0, int(ContactVarsN)) - ph.ContactsN = tensor.NewInt32(1) - ph.Contacts = tensor.NewFloat32(0, int(ContactVarsN)) - ph.JointControls = tensor.NewFloat32(0, int(JointControlVarsN)) - ph.SetAsCurrentVars() +func (ml *Model) Reset() { + ml.Bodies = tensor.NewFloat32(0, int(BodyVarsN)) + ml.Joints = tensor.NewFloat32(0, int(JointVarsN)) + ml.JointDoFs = tensor.NewFloat32(0, int(JointDoFVarsN)) + ml.BodyJoints = tensor.NewInt32(0, 2, 2) + ml.BodyCollidePairs = tensor.NewInt32(0, 2) + ml.Dynamics = tensor.NewFloat32(0, 2, int(DynamicVarsN)) + ml.BroadContactsN = tensor.NewInt32(1) + ml.BroadContacts = tensor.NewFloat32(0, int(ContactVarsN)) + ml.ContactsN = tensor.NewInt32(1) + ml.Contacts = tensor.NewFloat32(0, int(ContactVarsN)) + ml.JointControls = tensor.NewFloat32(0, int(JointControlVarsN)) + ml.SetAsCurrentVars() } // NewBody adds a new body with given parameters. Returns the index. // Use this for Static elements; NewDynamic for dynamic elements. -func (ph *World) NewBody(shape Shapes, size, pos math32.Vector3, rot math32.Quat) int32 { - sizes := ph.Bodies.ShapeSizes() +func (ml *Model) NewBody(shape Shapes, size, pos math32.Vector3, rot math32.Quat) int32 { + sizes := ml.Bodies.ShapeSizes() idx := int32(sizes[0]) - ph.Bodies.SetShapeSizes(int(idx+1), int(BodyVarsN)) - ph.Params[0].BodiesN = idx + 1 + ml.Bodies.SetShapeSizes(int(idx+1), int(BodyVarsN)) + ml.Params[0].BodiesN = idx + 1 SetBodyShape(idx, shape) SetBodyDynamic(idx, -1) SetBodyHSize(idx, size) SetBodyPos(idx, pos) SetBodyQuat(idx, rot) SetBodyGroup(idx, -1) // assume static - SetBodyWorld(idx, int32(ph.CurrentWorld)) - SetBodyObject(idx, int32(ph.CurrentObject)) - ph.SetMass(idx, shape, size, 0) // assume static + SetBodyWorld(idx, int32(ml.CurrentWorld)) + SetBodyObject(idx, int32(ml.CurrentObject)) + ml.SetMass(idx, shape, size, 0) // assume static return idx } // NewDynamic adds a new dynamic body with given parameters. Returns the index. // Shape cannot be [Plane]. -func (ph *World) NewDynamic(shape Shapes, mass float32, size, pos math32.Vector3, rot math32.Quat) (bodyIdx, dynIdx int32) { +func (ml *Model) NewDynamic(shape Shapes, mass float32, size, pos math32.Vector3, rot math32.Quat) (bodyIdx, dynIdx int32) { if shape == Plane { panic("physics.NewDynamic: shape cannot be Plane") } - bodyIdx = ph.NewBody(shape, size, pos, rot) - sizes := ph.Dynamics.ShapeSizes() + bodyIdx = ml.NewBody(shape, size, pos, rot) + sizes := ml.Dynamics.ShapeSizes() dynIdx = int32(sizes[0]) - ph.Dynamics.SetShapeSizes(int(dynIdx+1), 2, int(DynamicVarsN)) - ph.Params[0].DynamicsN = dynIdx + 1 + ml.Dynamics.SetShapeSizes(int(dynIdx+1), 2, int(DynamicVarsN)) + ml.Params[0].DynamicsN = dynIdx + 1 SetDynamicBody(dynIdx, bodyIdx) SetBodyDynamic(bodyIdx, dynIdx) SetBodyGroup(bodyIdx, 1) // dynamic - ph.SetMass(bodyIdx, shape, size, mass) + ml.SetMass(bodyIdx, shape, size, mass) return } @@ -165,42 +165,42 @@ func (ph *World) NewDynamic(shape Shapes, mass float32, size, pos math32.Vector3 // processed in the code (on the GPU). If this was not the setter of // the current variables, then the parameter variables are copied up // to the GPU. -func (ph *World) SetAsCurrent() { - isCur := (Bodies == ph.Bodies) - ph.SetAsCurrentVars() +func (ml *Model) SetAsCurrent() { + isCur := (Bodies == ml.Bodies) + ml.SetAsCurrentVars() if GPUInitialized && !isCur { - ph.ToGPUInfra() + ml.ToGPUInfra() } } // SetAsCurrentVars sets these as the current global values that are // processed in the code (on the GPU). -func (ph *World) SetAsCurrentVars() { - Params = ph.Params - Bodies = ph.Bodies - Joints = ph.Joints - JointDoFs = ph.JointDoFs - BodyJoints = ph.BodyJoints - BodyCollidePairs = ph.BodyCollidePairs - Dynamics = ph.Dynamics - BroadContactsN = ph.BroadContactsN - BroadContacts = ph.BroadContacts - ContactsN = ph.ContactsN - Contacts = ph.Contacts - JointControls = ph.JointControls +func (ml *Model) SetAsCurrentVars() { + Params = ml.Params + Bodies = ml.Bodies + Joints = ml.Joints + JointDoFs = ml.JointDoFs + BodyJoints = ml.BodyJoints + BodyCollidePairs = ml.BodyCollidePairs + Dynamics = ml.Dynamics + BroadContactsN = ml.BroadContactsN + BroadContacts = ml.BroadContacts + ContactsN = ml.ContactsN + Contacts = ml.Contacts + JointControls = ml.JointControls } // GPUInit initializes the GPU and transfers Infra. // Should have already called SetAsCurrent (needed for CPU and GPU). -func (ph *World) GPUInit() { +func (ml *Model) GPUInit() { GPUInit() - UseGPU = ph.GPU - ph.ToGPUInfra() + UseGPU = ml.GPU + ml.ToGPUInfra() } // ToGPUInfra copies all the infrastructure for these filters up to // the GPU. This is done in GPUInit, and if current switched. -func (ph *World) ToGPUInfra() { +func (ml *Model) ToGPUInfra() { ToGPUTensorStrides() ToGPU(ParamsVar, BodiesVar, JointsVar, JointDoFsVar, BodyJointsVar, BodyCollidePairsVar, DynamicsVar, BroadContactsNVar, BroadContactsVar, ContactsNVar, ContactsVar, JointControlsVar) } diff --git a/physics/phyxyz/editor.go b/physics/phyxyz/editor.go index 43283a90..c2fbb6f9 100644 --- a/physics/phyxyz/editor.go +++ b/physics/phyxyz/editor.go @@ -26,17 +26,17 @@ import ( type Editor struct { //types:add core.Frame - // Physics has the physics simulation. - Physics *physics.World + // Model has the physics simulation. + Model *physics.Model - // World has the 3D GUI visualization. - World *World + // Scene has the 3D GUI visualization. + Scene *Scene // UserParams is a struct with parameters for configuring the physics sim. // These are displayed in the editor. UserParams any - // ConfigFunc is the function that configures the world. + // ConfigFunc is the function that configures the [physics.Model]. ConfigFunc func() // ControlFunc is the function that sets control parameters, @@ -57,8 +57,8 @@ type Editor struct { //types:add // TimeStep is current time step in physics update cycles. TimeStep int - // Scene is the xyz GUI visualization widget. - scene *xyzcore.SceneEditor + // editor is the xyz GUI visualization widget. + editor *xyzcore.SceneEditor // Toolbar is the top toolbar. toolbar *core.Toolbar @@ -108,15 +108,15 @@ func (pe *Editor) Init() { if pe.UserParams != nil { pe.userParamsForm.SetStruct(pe.UserParams) } - params := &pe.Physics.Params[0] + params := &pe.Model.Params[0] pe.paramsForm.SetStruct(params) }) }) tree.AddChildAt(w, "scene", func(w *xyzcore.SceneEditor) { - pe.scene = w + pe.editor = w w.UpdateWidget() - sc := pe.scene.SceneXYZ() + sc := pe.editor.SceneXYZ() sc.Background = colors.Scheme.Select.Container xyz.NewAmbient(sc, "ambient", 0.3, xyz.DirectSun) @@ -124,8 +124,8 @@ func (pe *Editor) Init() { dir := xyz.NewDirectional(sc, "dir", 1, xyz.DirectSun) dir.Pos.Set(0, 2, 1) - pe.World = NewWorld(sc) - pe.Physics = physics.NewWorld() + pe.Scene = NewScene(sc) + pe.Model = physics.NewModel() sc.Camera.Pose.Pos = math32.Vec3(0, 40, 3.5) sc.Camera.LookAt(math32.Vec3(0, 5, 0), math32.Vec3(0, 1, 0)) @@ -140,24 +140,24 @@ func (pe *Editor) Init() { sc.SaveCamera("1") sc.SaveCamera("default") - pe.ConfigWorld() + pe.ConfigModel() }) }) } -// ConfigWorld configures the physics world. -func (pe *Editor) ConfigWorld() { +// ConfigModel configures the physics world. +func (pe *Editor) ConfigModel() { if pe.isRunning { core.MessageSnackbar(pe, "Simulation is still running...") return } - pe.World.Reset() - pe.Physics.Reset() + pe.Scene.Reset() + pe.Model.Reset() if pe.ConfigFunc != nil { pe.ConfigFunc() } - pe.World.Init(pe.Physics) - pe.World.Update() + pe.Scene.Init(pe.Model) + pe.Scene.Update() pe.stop = false pe.TimeStep = 0 } @@ -170,8 +170,8 @@ func (pe *Editor) Restart() bool { } pe.stop = false pe.TimeStep = 0 - pe.World.Init(pe.Physics) - pe.World.Update() + pe.Scene.Init(pe.Model) + pe.Scene.Update() pe.Update() return true } @@ -197,12 +197,12 @@ func (pe *Editor) MakeToolbar(p *tree.Plan) { if pe.ControlFunc != nil { pe.ControlFunc(physics.StepsToMsec(pe.TimeStep)) } - pe.Physics.Step() + pe.Model.Step() pe.TimeStep++ - pe.World.Update() - pe.Scene.AsyncLock() - pe.Scene.NeedsRender() - pe.Scene.AsyncUnlock() + pe.Scene.Update() + pe.editor.AsyncLock() + pe.editor.NeedsRender() + pe.editor.AsyncUnlock() if pe.stop { pe.stop = false break @@ -253,7 +253,7 @@ func (pe *Editor) MakeToolbar(p *tree.Plan) { if !pe.Restart() { return } - pe.ConfigWorld() + pe.ConfigModel() }) w.FirstStyler(func(s *styles.Style) { s.SetEnabled(!pe.isRunning) }) }) diff --git a/physics/phyxyz/typegen.go b/physics/phyxyz/typegen.go index 1ccdf432..89610ca4 100644 --- a/physics/phyxyz/typegen.go +++ b/physics/phyxyz/typegen.go @@ -3,6 +3,7 @@ package phyxyz import ( + "cogentcore.org/core/math32" "cogentcore.org/core/tree" "cogentcore.org/core/types" "cogentcore.org/lab/physics" @@ -10,20 +11,20 @@ import ( var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics/phyxyz.Camera", IDName: "camera", Doc: "Camera defines the properties of a camera needed for rendering from a node.", Fields: []types.Field{{Name: "Size", Doc: "size of image to record"}, {Name: "FOV", Doc: "field of view in degrees"}, {Name: "Near", Doc: "near plane z coordinate"}, {Name: "Far", Doc: "far plane z coordinate"}, {Name: "MaxD", Doc: "maximum distance for depth maps. Anything above is 1.\nThis is independent of Near / Far rendering (though must be < Far)\nand is for normalized depth maps."}, {Name: "LogD", Doc: "use the natural log of 1 + depth for normalized depth values in display etc."}, {Name: "MSample", Doc: "number of multi-samples to use for antialising -- 4 is best and default."}, {Name: "UpDir", Doc: "up direction for camera. Defaults to positive Y axis,\nand is reset by call to LookAt method."}}}) -var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics/phyxyz.Editor", IDName: "editor", Doc: "Editor provides a basic viewer and parameter controller widget\nfor exploring physics models.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Embeds: []types.Field{{Name: "Frame"}}, Fields: []types.Field{{Name: "Physics", Doc: "Physics has the physics simulation."}, {Name: "World", Doc: "World has the 3D GUI visualization."}, {Name: "UserParams", Doc: "UserParams is a struct with parameters for configuring the physics sim.\nThese are displayed in the editor."}, {Name: "ConfigFunc", Doc: "ConfigFunc is the function that configures the world."}, {Name: "ControlFunc", Doc: "ControlFunc is the function that sets control parameters,\nbased on the current timestep (in milliseconds, converted from physics time)."}, {Name: "isRunning", Doc: "IsRunning is true if currently running sim."}, {Name: "stop", Doc: "Stop triggers topping of running."}, {Name: "TimeStep", Doc: "TimeStep is current time step in physics update cycles."}, {Name: "scene", Doc: "Scene is the xyz GUI visualization widget."}, {Name: "toolbar", Doc: "Toolbar is the top toolbar."}, {Name: "splits", Doc: "Splits is the container for elements."}, {Name: "userParamsForm", Doc: "UserParamsForm has the user's config parameters."}, {Name: "paramsForm", Doc: "ParamsForm has the Physics parameters."}}}) +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics/phyxyz.Editor", IDName: "editor", Doc: "Editor provides a basic viewer and parameter controller widget\nfor exploring physics models.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Embeds: []types.Field{{Name: "Frame"}}, Fields: []types.Field{{Name: "Model", Doc: "Model has the physics simulation."}, {Name: "Scene", Doc: "Scene has the 3D GUI visualization."}, {Name: "UserParams", Doc: "UserParams is a struct with parameters for configuring the physics sim.\nThese are displayed in the editor."}, {Name: "ConfigFunc", Doc: "ConfigFunc is the function that configures the [physics.Model]."}, {Name: "ControlFunc", Doc: "ControlFunc is the function that sets control parameters,\nbased on the current timestep (in milliseconds, converted from physics time)."}, {Name: "CameraPos", Doc: "CameraPos provides the default initial camera position, looking at the origin.\nSet this to larger numbers to zoom out, and smaller numbers to zoom in.\nDefaults to math32.Vec3(0, 25, 20)."}, {Name: "isRunning", Doc: "IsRunning is true if currently running sim."}, {Name: "stop", Doc: "Stop triggers topping of running."}, {Name: "TimeStep", Doc: "TimeStep is current time step in physics update cycles."}, {Name: "editor", Doc: "editor is the xyz GUI visualization widget."}, {Name: "toolbar", Doc: "Toolbar is the top toolbar."}, {Name: "splits", Doc: "Splits is the container for elements."}, {Name: "userParamsForm", Doc: "UserParamsForm has the user's config parameters."}, {Name: "paramsForm", Doc: "ParamsForm has the Physics parameters."}}}) // NewEditor returns a new [Editor] with the given optional parent: // Editor provides a basic viewer and parameter controller widget // for exploring physics models. func NewEditor(parent ...tree.Node) *Editor { return tree.New[Editor](parent...) } -// SetPhysics sets the [Editor.Physics]: -// Physics has the physics simulation. -func (t *Editor) SetPhysics(v *physics.World) *Editor { t.Physics = v; return t } +// SetModel sets the [Editor.Model]: +// Model has the physics simulation. +func (t *Editor) SetModel(v *physics.Model) *Editor { t.Model = v; return t } -// SetWorld sets the [Editor.World]: -// World has the 3D GUI visualization. -func (t *Editor) SetWorld(v *World) *Editor { t.World = v; return t } +// SetScene sets the [Editor.Scene]: +// Scene has the 3D GUI visualization. +func (t *Editor) SetScene(v *Scene) *Editor { t.Scene = v; return t } // SetUserParams sets the [Editor.UserParams]: // UserParams is a struct with parameters for configuring the physics sim. @@ -31,7 +32,7 @@ func (t *Editor) SetWorld(v *World) *Editor { t.World = v; return t } func (t *Editor) SetUserParams(v any) *Editor { t.UserParams = v; return t } // SetConfigFunc sets the [Editor.ConfigFunc]: -// ConfigFunc is the function that configures the world. +// ConfigFunc is the function that configures the [physics.Model]. func (t *Editor) SetConfigFunc(v func()) *Editor { t.ConfigFunc = v; return t } // SetControlFunc sets the [Editor.ControlFunc]: @@ -39,10 +40,16 @@ func (t *Editor) SetConfigFunc(v func()) *Editor { t.ConfigFunc = v; return t } // based on the current timestep (in milliseconds, converted from physics time). func (t *Editor) SetControlFunc(v func(timeStep int)) *Editor { t.ControlFunc = v; return t } +// SetCameraPos sets the [Editor.CameraPos]: +// CameraPos provides the default initial camera position, looking at the origin. +// Set this to larger numbers to zoom out, and smaller numbers to zoom in. +// Defaults to math32.Vec3(0, 25, 20). +func (t *Editor) SetCameraPos(v math32.Vector3) *Editor { t.CameraPos = v; return t } + // SetTimeStep sets the [Editor.TimeStep]: // TimeStep is current time step in physics update cycles. func (t *Editor) SetTimeStep(v int) *Editor { t.TimeStep = v; return t } var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics/phyxyz.View", IDName: "view", Doc: "View has visualization functions for physics elements.", Fields: []types.Field{{Name: "Name", Doc: "Name is a name for element (index always appended)."}, {Name: "Shape", Doc: "Shape is the physical shape of the element."}, {Name: "Color", Doc: "Color is the color of the element."}, {Name: "Size", Doc: "Size is the size (per shape)."}, {Name: "Pos", Doc: "Pos is the position."}, {Name: "Quat", Doc: "Quat is the rotation as a quaternion."}, {Name: "NewView", Doc: "NewView is a function that returns a new [xyz.Node]\nto represent this element. If nil, uses appropriate defaults."}, {Name: "InitView", Doc: "InitView is a function that initializes a new [xyz.Node]\nthat represents this element. If nil, uses appropriate defaults."}, {Name: "Index", Doc: "Index is the index of the element in a list."}, {Name: "DynamicIndex", Doc: "DynamicIndex is the index of a dynamic element (-1 if not dynamic)."}}}) -var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics/phyxyz.World", IDName: "world", Doc: "World displays a [physics.World] using a [xyz.Scene].\nOne World can be used for multiple different [physics.World]s which\nis more efficient when running multiple in parallel.\nInitial construction of the physics and visualization happens here.", Fields: []types.Field{{Name: "Scene", Doc: "Scene is the [xyz.Scene] object for visualizing."}, {Name: "Root", Doc: "Root is the root Group node in the Scene under which the world is rendered."}, {Name: "Views", Doc: "Views are the view elements for each body in [physics.World]."}}}) +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics/phyxyz.Scene", IDName: "scene", Doc: "Scene displays a [physics.Model] using a [xyz.Scene].\nOne Scene can be used for multiple different [physics.Model]s which\nis more efficient when running multiple in parallel.\nInitial construction of the physics and visualization happens here.", Fields: []types.Field{{Name: "Scene", Doc: "Scene is the [xyz.Scene] object for visualizing."}, {Name: "Root", Doc: "Root is the root Group node in the Scene under which the world is rendered."}, {Name: "Views", Doc: "Views are the view elements for each body in [physics.Model]."}}}) diff --git a/physics/phyxyz/view.go b/physics/phyxyz/view.go index 8a40cf74..41953c42 100644 --- a/physics/phyxyz/view.go +++ b/physics/phyxyz/view.go @@ -53,7 +53,7 @@ type View struct { // NewBody adds a new body with given parameters. // Returns the View which can then be further customized. // Use this for Static elements; NewDynamic for dynamic elements. -func (wr *World) NewBody(ph *physics.World, name string, shape physics.Shapes, clr string, size, pos math32.Vector3, rot math32.Quat) *View { +func (wr *Scene) NewBody(ph *physics.Model, name string, shape physics.Shapes, clr string, size, pos math32.Vector3, rot math32.Quat) *View { idx := ph.NewBody(shape, size, pos, rot) vw := &View{Name: name, Index: idx, DynamicIndex: -1, Shape: shape, Color: clr, Size: size, Pos: pos, Quat: rot} wr.Views = append(wr.Views, vw) @@ -62,7 +62,7 @@ func (wr *World) NewBody(ph *physics.World, name string, shape physics.Shapes, c // NewDynamic adds a new dynamic body with given parameters. // Returns the View which can then be further customized. -func (wr *World) NewDynamic(ph *physics.World, name string, shape physics.Shapes, clr string, mass float32, size, pos math32.Vector3, rot math32.Quat) *View { +func (wr *Scene) NewDynamic(ph *physics.Model, name string, shape physics.Shapes, clr string, mass float32, size, pos math32.Vector3, rot math32.Quat) *View { idx, dyIdx := ph.NewDynamic(shape, mass, size, pos, rot) vw := &View{Name: name, Index: idx, DynamicIndex: dyIdx, Shape: shape, Color: clr, Size: size, Pos: pos, Quat: rot} wr.Views = append(wr.Views, vw) @@ -269,7 +269,7 @@ func (vw *View) SetBodyFrictionRolling(val float32) { // These are for the non-rotated body (i.e., body rotation is applied // to these positions as well). // Sets relative rotation matricies to identity by default. -func (vw *View) NewJointFixed(ph *physics.World, parent *View, ppos, cpos math32.Vector3) int32 { +func (vw *View) NewJointFixed(ph *physics.Model, parent *View, ppos, cpos math32.Vector3) int32 { pidx := int32(-1) if parent != nil { pidx = parent.DynamicIndex @@ -285,7 +285,7 @@ func (vw *View) NewJointFixed(ph *physics.World, parent *View, ppos, cpos math32 // Sets relative rotation matricies to identity by default. // axis is the axis of articulation for the joint. // Use [SetJointDoF] to set the remaining DoF parameters. -func (vw *View) NewJointPrismatic(ph *physics.World, parent *View, ppos, cpos, axis math32.Vector3) int32 { +func (vw *View) NewJointPrismatic(ph *physics.Model, parent *View, ppos, cpos, axis math32.Vector3) int32 { pidx := int32(-1) if parent != nil { pidx = parent.DynamicIndex @@ -301,7 +301,7 @@ func (vw *View) NewJointPrismatic(ph *physics.World, parent *View, ppos, cpos, a // Sets relative rotation matricies to identity by default. // axis is the axis of articulation for the joint. // Use [SetJointDoF] to set the remaining DoF parameters. -func (vw *View) NewJointRevolute(ph *physics.World, parent *View, ppos, cpos, axis math32.Vector3) int32 { +func (vw *View) NewJointRevolute(ph *physics.Model, parent *View, ppos, cpos, axis math32.Vector3) int32 { pidx := int32(-1) if parent != nil { pidx = parent.DynamicIndex @@ -316,7 +316,7 @@ func (vw *View) NewJointRevolute(ph *physics.World, parent *View, ppos, cpos, ax // to these positions as well). // Sets relative rotation matricies to identity by default. // Use [SetJointDoF] to set the remaining DoF parameters. -func (vw *View) NewJointBall(ph *physics.World, parent *View, ppos, cpos math32.Vector3) int32 { +func (vw *View) NewJointBall(ph *physics.Model, parent *View, ppos, cpos math32.Vector3) int32 { pidx := int32(-1) if parent != nil { pidx = parent.DynamicIndex @@ -332,7 +332,7 @@ func (vw *View) NewJointBall(ph *physics.World, parent *View, ppos, cpos math32. // to these positions as well). // Sets relative rotation matricies to identity by default. // Use [SetJointDoF] to set the remaining DoF parameters. -func (vw *View) NewJointDistance(ph *physics.World, parent *View, ppos, cpos math32.Vector3, minDist, maxDist float32) int32 { +func (vw *View) NewJointDistance(ph *physics.Model, parent *View, ppos, cpos math32.Vector3, minDist, maxDist float32) int32 { pidx := int32(-1) if parent != nil { pidx = parent.DynamicIndex @@ -347,7 +347,7 @@ func (vw *View) NewJointDistance(ph *physics.World, parent *View, ppos, cpos mat // to these positions as well). // Sets relative rotation matricies to identity by default. // Use [SetJointDoF] to set the remaining DoF parameters. -func (vw *View) NewJointFree(ph *physics.World, parent *View, ppos, cpos math32.Vector3) int32 { +func (vw *View) NewJointFree(ph *physics.Model, parent *View, ppos, cpos math32.Vector3) int32 { pidx := int32(-1) if parent != nil { pidx = parent.DynamicIndex diff --git a/physics/phyxyz/world.go b/physics/phyxyz/world.go index c83314c6..2995bae9 100644 --- a/physics/phyxyz/world.go +++ b/physics/phyxyz/world.go @@ -11,74 +11,72 @@ package phyxyz import ( "image" - "cogentcore.org/core/core" - "cogentcore.org/core/icons" "cogentcore.org/core/tree" "cogentcore.org/core/xyz" "cogentcore.org/lab/physics" ) -// World displays a [physics.World] using a [xyz.Scene]. -// One World can be used for multiple different [physics.World]s which +// Scene displays a [physics.Model] using a [xyz.Scene]. +// One Scene can be used for multiple different [physics.Model]s which // is more efficient when running multiple in parallel. // Initial construction of the physics and visualization happens here. -type World struct { +type Scene struct { // Scene is the [xyz.Scene] object for visualizing. Scene *xyz.Scene // Root is the root Group node in the Scene under which the world is rendered. Root *xyz.Group - // Views are the view elements for each body in [physics.World]. + // Views are the view elements for each body in [physics.Model]. Views []*View } -// NewWorld returns a new World for visualizing a [physics.World]. +// NewScene returns a new Scene for visualizing a [physics.Model]. // with given [xyz.Scene], making a top-level Root group in the scene. -func NewWorld(sc *xyz.Scene) *World { +func NewScene(sc *xyz.Scene) *Scene { rgp := xyz.NewGroup(sc) rgp.SetName("world") - wr := &World{Scene: sc, Root: rgp} - return wr + xysc := &Scene{Scene: sc, Root: rgp} + return xysc } // Init configures the visual world based on Views, -// and calls Config on [physics.World]. +// and calls Config on [physics.Model]. // Call this _once_ after making all the new Views and Bodies. // (will return if already called). -func (wr *World) Init(ph *physics.World) { +func (sc *Scene) Init(ph *physics.Model) { ph.Config() - if len(wr.Root.Makers.Normal) > 0 { + if len(sc.Root.Makers.Normal) > 0 { return } - wr.Root.Maker(func(p *tree.Plan) { - for _, vw := range wr.Views { + sc.Root.Maker(func(p *tree.Plan) { + for _, vw := range sc.Views { vw.Add(p) } }) } // Reset resets any existing views, starting fresh for a new configuration. -func (wr *World) Reset() { - wr.Views = nil - if wr.Scene != nil { - wr.Scene.Update() +func (sc *Scene) Reset() { + sc.Views = nil + if sc.Scene != nil { + sc.Scene.Update() } } // Update updates the xyz scene from current physics node state. -// (use physics.World.SetAsCurrent()). -func (wr *World) Update() { - wr.UpdateFromPhysics() - if wr.Scene != nil { - wr.Scene.Update() +// (use physics.Model.SetAsCurrent()). +func (sc *Scene) Update() { + sc.UpdateFromPhysics() + if sc.Scene != nil { + sc.Scene.Update() } } -// UpdateFromPhysics updates the World from currently active -// physics state (use physics.World.SetAsCurrent()). -func (wr *World) UpdateFromPhysics() { - for _, vw := range wr.Views { +// UpdateFromPhysics updates the Scene from currently active +// physics state (use physics.Model.SetAsCurrent()). +func (sc *Scene) UpdateFromPhysics() { + for _, vw := range sc.Views { vw.UpdateFromPhysics() } } @@ -86,57 +84,27 @@ func (wr *World) UpdateFromPhysics() { // RenderFromView does an offscreen render using given [View] // for the camera position and orientation, returning the render image. // Current scene camera is saved and restored. -func (wr *World) RenderFromNode(vw *View, cam *Camera) image.Image { - sc := wr.Scene +func (sc *Scene) RenderFromNode(vw *View, cam *Camera) image.Image { + xysc := sc.Scene camnm := "physics-view-rendernode-save" - sc.SaveCamera(camnm) + xysc.SaveCamera(camnm) defer func() { - sc.SetCamera(camnm) - sc.UseMainFrame() + xysc.SetCamera(camnm) + xysc.UseMainFrame() }() - sc.Camera.FOV = cam.FOV - sc.Camera.Near = cam.Near - sc.Camera.Far = cam.Far - sc.Camera.Pose.Pos = vw.Pos - sc.Camera.Pose.Quat = vw.Quat - sc.Camera.Pose.Scale.Set(1, 1, 1) + xysc.Camera.FOV = cam.FOV + xysc.Camera.Near = cam.Near + xysc.Camera.Far = cam.Far + xysc.Camera.Pose.Pos = vw.Pos + xysc.Camera.Pose.Quat = vw.Quat + xysc.Camera.Pose.Scale.Set(1, 1, 1) - sc.UseAltFrame(cam.Size) - return sc.RenderGrabImage() + xysc.UseAltFrame(cam.Size) + return xysc.RenderGrabImage() } // DepthImage returns the current rendered depth image -// func (vw *World) DepthImage() ([]float32, error) { +// func (vw *Scene) DepthImage() ([]float32, error) { // return vw.Scene.DepthImage() // } - -// MakeStateToolbar returns a toolbar function for physics state updates, -// calling the given updt function after making the change. -func MakeStateToolbar(ps *physics.State, updt func()) func(p *tree.Plan) { - return func(p *tree.Plan) { - tree.Add(p, func(w *core.FuncButton) { - w.SetFunc(ps.SetEulerRotation).SetAfterFunc(updt).SetIcon(icons.Rotate90DegreesCcw) - }) - tree.Add(p, func(w *core.FuncButton) { - w.SetFunc(ps.SetAxisRotation).SetAfterFunc(updt).SetIcon(icons.Rotate90DegreesCcw) - }) - tree.Add(p, func(w *core.FuncButton) { - w.SetFunc(ps.RotateEuler).SetAfterFunc(updt).SetIcon(icons.Rotate90DegreesCcw) - }) - tree.Add(p, func(w *core.FuncButton) { - w.SetFunc(ps.RotateOnAxis).SetAfterFunc(updt).SetIcon(icons.Rotate90DegreesCcw) - }) - tree.Add(p, func(w *core.FuncButton) { - w.SetFunc(ps.EulerRotation).SetAfterFunc(updt).SetShowReturn(true).SetIcon(icons.Rotate90DegreesCcw) - }) - tree.Add(p, func(w *core.Separator) {}) - tree.Add(p, func(w *core.FuncButton) { - w.SetFunc(ps.MoveOnAxis).SetAfterFunc(updt).SetIcon(icons.MoveItem) - }) - tree.Add(p, func(w *core.FuncButton) { - w.SetFunc(ps.MoveOnAxisAbs).SetAfterFunc(updt).SetIcon(icons.MoveItem) - }) - - } -} diff --git a/physics/shaders/CollisionBroad.wgsl b/physics/shaders/CollisionBroad.wgsl index d8a47ac4..3c96fbf6 100644 --- a/physics/shaders/CollisionBroad.wgsl +++ b/physics/shaders/CollisionBroad.wgsl @@ -273,7 +273,7 @@ fn DynamicQuat(idx: i32,cni: i32) -> vec4 { } //////// import: "enumgen.go" -const BodyVarsN: BodyVars = 43; +const BodyVarsN: BodyVars = 44; const ContactVarsN: ContactVars = 33; const JointControlVarsN: JointControlVars = 5; const DynamicVarsN: DynamicVars = 33; diff --git a/physics/shaders/CollisionNarrow.wgsl b/physics/shaders/CollisionNarrow.wgsl index 302d4d74..e31db079 100644 --- a/physics/shaders/CollisionNarrow.wgsl +++ b/physics/shaders/CollisionNarrow.wgsl @@ -362,7 +362,7 @@ fn DynamicQuat(idx: i32,cni: i32) -> vec4 { } //////// import: "enumgen.go" -const BodyVarsN: BodyVars = 43; +const BodyVarsN: BodyVars = 44; const ContactVarsN: ContactVars = 33; const JointControlVarsN: JointControlVars = 5; const DynamicVarsN: DynamicVars = 33; diff --git a/physics/shaders/DynamicsCurToNext.wgsl b/physics/shaders/DynamicsCurToNext.wgsl index 9df7f5be..584a9bbd 100644 --- a/physics/shaders/DynamicsCurToNext.wgsl +++ b/physics/shaders/DynamicsCurToNext.wgsl @@ -156,7 +156,7 @@ const DynAngDeltaZ: DynamicVars = 31; const DynContactWeight: DynamicVars = 32; //////// import: "enumgen.go" -const BodyVarsN: BodyVars = 43; +const BodyVarsN: BodyVars = 44; const ContactVarsN: ContactVars = 33; const JointControlVarsN: JointControlVars = 5; const DynamicVarsN: DynamicVars = 33; diff --git a/physics/shaders/ForcesFromJoints.wgsl b/physics/shaders/ForcesFromJoints.wgsl index 1ecf6b68..70b13c56 100644 --- a/physics/shaders/ForcesFromJoints.wgsl +++ b/physics/shaders/ForcesFromJoints.wgsl @@ -174,7 +174,7 @@ fn SetDynamicTorque(idx: i32,cni: i32, torque: vec3) { } //////// import: "enumgen.go" -const BodyVarsN: BodyVars = 43; +const BodyVarsN: BodyVars = 44; const ContactVarsN: ContactVars = 33; const JointControlVarsN: JointControlVars = 5; const DynamicVarsN: DynamicVars = 33; diff --git a/physics/shaders/InitDynamics.wgsl b/physics/shaders/InitDynamics.wgsl index 39ff9664..a3284ee4 100644 --- a/physics/shaders/InitDynamics.wgsl +++ b/physics/shaders/InitDynamics.wgsl @@ -163,7 +163,7 @@ const DynContactWeight: DynamicVars = 32; fn DynamicBody(idx: i32) -> i32 { return i32(bitcast(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(0), u32(DynBody))])); } //////// import: "enumgen.go" -const BodyVarsN: BodyVars = 43; +const BodyVarsN: BodyVars = 44; const ContactVarsN: ContactVars = 33; const JointControlVarsN: JointControlVars = 5; const DynamicVarsN: DynamicVars = 33; diff --git a/physics/shaders/StepBodyContactDeltas.wgsl b/physics/shaders/StepBodyContactDeltas.wgsl index 0eede33a..ea738b80 100644 --- a/physics/shaders/StepBodyContactDeltas.wgsl +++ b/physics/shaders/StepBodyContactDeltas.wgsl @@ -259,7 +259,7 @@ fn SetDynamicAngDelta(idx: i32,cni: i32, angDelta: vec3) { } //////// import: "enumgen.go" -const BodyVarsN: BodyVars = 43; +const BodyVarsN: BodyVars = 44; const ContactVarsN: ContactVars = 33; const JointControlVarsN: JointControlVars = 5; const DynamicVarsN: DynamicVars = 33; diff --git a/physics/shaders/StepBodyContacts.wgsl b/physics/shaders/StepBodyContacts.wgsl index c9aa9a76..9877a32d 100644 --- a/physics/shaders/StepBodyContacts.wgsl +++ b/physics/shaders/StepBodyContacts.wgsl @@ -388,7 +388,7 @@ fn DynamicAngDelta(idx: i32,cni: i32) -> vec3 { } //////// import: "enumgen.go" -const BodyVarsN: BodyVars = 43; +const BodyVarsN: BodyVars = 44; const ContactVarsN: ContactVars = 33; const JointControlVarsN: JointControlVars = 5; const DynamicVarsN: DynamicVars = 33; diff --git a/physics/shaders/StepBodyJointDeltas.wgsl b/physics/shaders/StepBodyJointDeltas.wgsl index ee127e1d..aba91ee9 100644 --- a/physics/shaders/StepBodyJointDeltas.wgsl +++ b/physics/shaders/StepBodyJointDeltas.wgsl @@ -200,7 +200,7 @@ fn SetDynamicAngDelta(idx: i32,cni: i32, angDelta: vec3) { } //////// import: "enumgen.go" -const BodyVarsN: BodyVars = 43; +const BodyVarsN: BodyVars = 44; const ContactVarsN: ContactVars = 33; const JointControlVarsN: JointControlVars = 5; const DynamicVarsN: DynamicVars = 33; diff --git a/physics/shaders/StepIntegrateBodies.wgsl b/physics/shaders/StepIntegrateBodies.wgsl index 73c91053..b46be086 100644 --- a/physics/shaders/StepIntegrateBodies.wgsl +++ b/physics/shaders/StepIntegrateBodies.wgsl @@ -186,7 +186,7 @@ fn DynamicAngDelta(idx: i32,cni: i32) -> vec3 { return vec3(Dynamics[I fn SetDynamicAngDelta(idx: i32,cni: i32, angDelta: vec3) { Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaX))] = angDelta.x;; Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaY))] = angDelta.y;; Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaZ))] = angDelta.z; } //////// import: "enumgen.go" -const BodyVarsN: BodyVars = 43; +const BodyVarsN: BodyVars = 44; const ContactVarsN: ContactVars = 33; const JointControlVarsN: JointControlVars = 5; const DynamicVarsN: DynamicVars = 33; diff --git a/physics/shaders/StepJointForces.wgsl b/physics/shaders/StepJointForces.wgsl index 455a404c..8c80837f 100644 --- a/physics/shaders/StepJointForces.wgsl +++ b/physics/shaders/StepJointForces.wgsl @@ -177,7 +177,7 @@ fn DynamicPos(idx: i32,cni: i32) -> vec3 { return vec3(Dynamics[Index3 fn DynamicQuat(idx: i32,cni: i32) -> vec4 { return vec4(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatZ))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatW))]); } //////// import: "enumgen.go" -const BodyVarsN: BodyVars = 43; +const BodyVarsN: BodyVars = 44; const ContactVarsN: ContactVars = 33; const JointControlVarsN: JointControlVars = 5; const DynamicVarsN: DynamicVars = 33; diff --git a/physics/shaders/StepSolveJoints.wgsl b/physics/shaders/StepSolveJoints.wgsl index f84de152..09d65e4e 100644 --- a/physics/shaders/StepSolveJoints.wgsl +++ b/physics/shaders/StepSolveJoints.wgsl @@ -188,7 +188,7 @@ fn DynamicAngDelta(idx: i32,cni: i32) -> vec3 { } //////// import: "enumgen.go" -const BodyVarsN: BodyVars = 43; +const BodyVarsN: BodyVars = 44; const ContactVarsN: ContactVars = 33; const JointControlVarsN: JointControlVars = 5; const DynamicVarsN: DynamicVars = 33; diff --git a/physics/step.go b/physics/step.go index 00914836..d8685392 100644 --- a/physics/step.go +++ b/physics/step.go @@ -68,36 +68,36 @@ func StepInit(i uint32) { //gosl:kernel read-write:Params // Step runs one physics step, sending Params and JointControls // to the GPU, and getting the Dynamics state vars back. // Each step has SubSteps integration sub-steps. -func (wl *World) Step() { +func (ml *Model) Step() { params := GetParams(0) ToGPU(ParamsVar, JointControlsVar) if params.SubSteps > 1 { for range params.SubSteps - 1 { - wl.StepGet() + ml.StepGet() } } - wl.StepGet(ParamsVar, DynamicsVar) + ml.StepGet(ParamsVar, DynamicsVar) // wl.StepGet(ParamsVar, DynamicsVar, ContactsNVar) // fmt.Println("contacts:", ContactsN.Value(0), "max:", params.ContactsMax) } // StepGet runs one physics step and gets the given vars back // from the GPU. -func (wl *World) StepGet(vars ...GPUVars) { +func (ml *Model) StepGet(vars ...GPUVars) { params := GetParams(0) RunStepInit(1) - wl.StepCollision() - wl.StepJointForces() - wl.StepIntegrateBodies() + ml.StepCollision() + ml.StepJointForces() + ml.StepIntegrateBodies() for range params.Iterations { - wl.StepSolveJoints() - wl.StepBodyContacts() + ml.StepSolveJoints() + ml.StepBodyContacts() } RunDone(vars...) } -func (wl *World) StepCollision() { +func (ml *Model) StepCollision() { params := GetParams(0) RunCollisionBroad(int(params.BodyCollidePairsN)) // note: time getting BroadContactsN back down and using that vs. running full @@ -107,26 +107,26 @@ func (wl *World) StepCollision() { // fmt.Println("contacts:", ContactsN.Value(0), "max:", params.ContactsMax) } -func (wl *World) StepJointForces() { +func (ml *Model) StepJointForces() { params := GetParams(0) RunStepJointForces(int(params.JointsN)) RunForcesFromJoints(int(params.DynamicsN)) } -func (wl *World) StepIntegrateBodies() { +func (ml *Model) StepIntegrateBodies() { params := GetParams(0) RunStepIntegrateBodies(int(params.DynamicsN)) } -func (wl *World) StepSolveJoints() { +func (ml *Model) StepSolveJoints() { params := GetParams(0) RunStepSolveJoints(int(params.JointsN)) RunStepBodyJointDeltas(int(params.DynamicsN)) } -func (wl *World) StepBodyContacts() { +func (ml *Model) StepBodyContacts() { params := GetParams(0) - if !wl.GPU { + if !ml.GPU { cmax := int(ContactsN.Values[0]) if cmax > 0 { RunStepBodyContacts(cmax) diff --git a/physics/step.goal b/physics/step.goal index 8b7db9a4..4e15d429 100644 --- a/physics/step.goal +++ b/physics/step.goal @@ -66,36 +66,36 @@ func StepInit(i uint32) { //gosl:kernel read-write:Params // Step runs one physics step, sending Params and JointControls // to the GPU, and getting the Dynamics state vars back. // Each step has SubSteps integration sub-steps. -func (wl *World) Step() { +func (ml *Model) Step() { params := GetParams(0) ToGPU(ParamsVar, JointControlsVar) if params.SubSteps > 1 { for range params.SubSteps - 1 { - wl.StepGet() + ml.StepGet() } } - wl.StepGet(ParamsVar, DynamicsVar) + ml.StepGet(ParamsVar, DynamicsVar) // wl.StepGet(ParamsVar, DynamicsVar, ContactsNVar) // fmt.Println("contacts:", ContactsN.Value(0), "max:", params.ContactsMax) } // StepGet runs one physics step and gets the given vars back // from the GPU. -func (wl *World) StepGet(vars ...GPUVars) { +func (ml *Model) StepGet(vars ...GPUVars) { params := GetParams(0) RunStepInit(1) - wl.StepCollision() - wl.StepJointForces() - wl.StepIntegrateBodies() + ml.StepCollision() + ml.StepJointForces() + ml.StepIntegrateBodies() for range params.Iterations { - wl.StepSolveJoints() - wl.StepBodyContacts() + ml.StepSolveJoints() + ml.StepBodyContacts() } RunDone(vars...) } -func (wl *World) StepCollision() { +func (ml *Model) StepCollision() { params := GetParams(0) RunCollisionBroad(int(params.BodyCollidePairsN)) // note: time getting BroadContactsN back down and using that vs. running full @@ -105,26 +105,26 @@ func (wl *World) StepCollision() { // fmt.Println("contacts:", ContactsN.Value(0), "max:", params.ContactsMax) } -func (wl *World) StepJointForces() { +func (ml *Model) StepJointForces() { params := GetParams(0) RunStepJointForces(int(params.JointsN)) RunForcesFromJoints(int(params.DynamicsN)) } -func (wl *World) StepIntegrateBodies() { +func (ml *Model) StepIntegrateBodies() { params := GetParams(0) RunStepIntegrateBodies(int(params.DynamicsN)) } -func (wl *World) StepSolveJoints() { +func (ml *Model) StepSolveJoints() { params := GetParams(0) RunStepSolveJoints(int(params.JointsN)) RunStepBodyJointDeltas(int(params.DynamicsN)) } -func (wl *World) StepBodyContacts() { +func (ml *Model) StepBodyContacts() { params := GetParams(0) - if !wl.GPU { + if !ml.GPU { cmax := int(ContactsN.Values[0]) if cmax > 0 { RunStepBodyContacts(cmax) diff --git a/physics/typegen.go b/physics/typegen.go index 80452820..32123aaa 100644 --- a/physics/typegen.go +++ b/physics/typegen.go @@ -6,8 +6,6 @@ import ( "cogentcore.org/core/types" ) -var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.BBox", IDName: "b-box", Doc: "BBox contains bounding box and other gross object properties", Fields: []types.Field{{Name: "BBox", Doc: "bounding box in world coords (Axis-Aligned Bounding Box = AABB)"}, {Name: "VelBBox", Doc: "velocity-projected bounding box in world coords: extend BBox to include future position of moving bodies -- collision must be made on this basis"}, {Name: "BSphere", Doc: "bounding sphere in local coords"}, {Name: "Area", Doc: "area"}, {Name: "Volume", Doc: "volume"}}}) - var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.BodyVars", IDName: "body-vars", Doc: "BodyVars are body state variables stored in tensor.Float32"}) var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.ContactVars", IDName: "contact-vars", Doc: "Contact is one pairwise point of contact between two bodies.\nContacts are represented in spherical terms relative to the\nspherical BBox of A and B."}) @@ -24,12 +22,10 @@ var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.JointVars", var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.JointDoFVars", IDName: "joint-do-f-vars", Doc: "JointDoFVars are joint DoF state variables stored in tensor.Float32,\none for each DoF."}) +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.Model", IDName: "model", Doc: "Model contains and manages all of the physics elements.", Directives: []types.Directive{{Tool: "go", Directive: "generate", Args: []string{"core", "generate", "-add-types", "-gosl"}}}, Fields: []types.Field{{Name: "GPU", Doc: "GPU determines whether to use GPU (else CPU)."}, {Name: "Params", Doc: "Params are global parameters."}, {Name: "CurrentWorld", Doc: "CurrentWorld is the [BodyWorld] value to use when creating new bodies.\nSet to -1 to create global elements that interact with everything,\nwhile 0 and positive numbers only interact amongst themselves."}, {Name: "ReplicasStart", Doc: "ReplicasStart is the starting body index for replicated world bodies,\nwhich is needed for viewers to efficiently select a specific world to view.\nThis is the start of the World=0 first instance."}, {Name: "ReplicasN", Doc: "ReplicasN is the number of body elements within each set of\nreplicated world bodies, which is needed for viewers to efficiently select\na specific world to view."}, {Name: "CurrentObject", Doc: "CurrentObject is the [BodyObject] value to use when creating new bodies.\nGenerally just increment when starting a new object. When using world replicas,\nthe object indexes are copied, so objects are indexed by world and object id."}, {Name: "Bodies", Doc: "Bodies are the rigid body elements (dynamic and static),\nspecifying the constant, non-dynamic properties,\nwhich is initial state for dynamics.\n[body][BodyVarsN]"}, {Name: "Joints", Doc: "Joints is a list of permanent joints connecting bodies,\nwhich do not change (no dynamic variables).\n[joint][JointVarsN]"}, {Name: "JointDoFs", Doc: "JointDoFs is a list of joint DoF parameters, allocated per joint.\n[dof][JointDoFVars]"}, {Name: "BodyJoints", Doc: "BodyJoints is a list of joint indexes for each dynamic body, for aggregating.\n[dyn body][parent, child][Params.BodyJointsMax]"}, {Name: "BodyCollidePairs", Doc: "BodyCollidePairs are pairs of Body indexes that could potentially collide\nbased on precomputed collision logic, using World, Group, and Joint indexes.\n[BodyCollidePairsN][2]"}, {Name: "Dynamics", Doc: "Dynamics are the dynamic rigid body elements: these actually move.\nThe first set of variables are for initial values, and the second current.\n[body][cur/next][DynamicVarsN]"}, {Name: "BroadContactsN", Doc: "BroadContactsN has number of points of broad contact\nbetween bodies. [1]"}, {Name: "BroadContacts", Doc: "BroadContacts are the results of broad-phase contact processing,\nestablishing possible points of contact between bodies.\n[ContactsMax][BroadContactVarsN]"}, {Name: "ContactsN", Doc: "ContactsN has number of points of narrow (final) contact\nbetween bodies. [1]"}, {Name: "Contacts", Doc: "Contacts are the results of narrow-phase contact processing,\nwhere only actual contacts with fully-specified values are present.\n[ContactsMax][ContactVarsN]"}, {Name: "JointControls", Doc: "JointControls are dynamic joint control inputs, per joint DoF\n(in correspondence with [JointDoFs]). This can be uploaded to the\nGPU at every step.\n[dof][JointControlVarsN]"}}}) + var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.PhysParams", IDName: "phys-params", Doc: "PhysParams are the physics parameters", Directives: []types.Directive{{Tool: "gosl", Directive: "start"}}, Fields: []types.Field{{Name: "Iterations", Doc: "Iterations is the number of integration iterations to perform\nwithin each solver step. Muller et al (2020) report that 1 is best."}, {Name: "Dt", Doc: "Dt is the integration stepsize.\nFor highly kinetic situations (e.g., rapidly moving bouncing balls)\n0.0001 is needed to ensure contact registration. Use SubSteps to\naccomplish a target effective read-out step size."}, {Name: "SubSteps", Doc: "SubSteps is the number of integration steps to take per Step()\nfunction call. These sub steps are taken without any sync to/from\nthe GPU and are therefore much faster."}, {Name: "ContactMargin", Doc: "Contact margin is the extra distance for broadphase collision\naround rigid bodies. This can make some joints potentially unstable if > 0"}, {Name: "ContactRelax", Doc: "ContactRelax is rigid contact relaxation constant.\nHigher values cause errros"}, {Name: "ContactWeighting", Doc: "Contact weighting: balances contact forces?"}, {Name: "Restitution", Doc: "Restitution takes into account bounciness of objects."}, {Name: "JointLinearRelax", Doc: "JointLinearRelax is joint linear relaxation constant."}, {Name: "JointAngularRelax", Doc: "JointAngularRelax is joint angular relaxation constant."}, {Name: "JointLinearComply", Doc: "JointLinearComply is joint linear compliance constant."}, {Name: "JointAngularComply", Doc: "JointAngularComply is joint angular compliance constant."}, {Name: "AngularDamping", Doc: "AngularDamping is damping of angular motion."}, {Name: "SoftRelax", Doc: "SoftRelax is soft-body relaxation constant."}, {Name: "MaxGeomIter", Doc: "MaxGeomIter is number of iterations to perform in shape-based\ngeometry collision computations"}, {Name: "ContactsMax", Doc: "Maximum number of contacts to process at any given point."}, {Name: "Cur", Doc: "Index for the current state (0 or 1, alternates with Next)."}, {Name: "Next", Doc: "Index for the next state (1 or 0, alternates with Cur)."}, {Name: "BodiesN", Doc: "BodiesN is number of rigid bodies."}, {Name: "DynamicsN", Doc: "DynamicsN is number of dynamics bodies."}, {Name: "JointsN", Doc: "JointsN is number of joints."}, {Name: "JointDoFsN", Doc: "JointDoFsN is number of joint DoFs."}, {Name: "BodyJointsMax", Doc: "BodyJointsMax is max number of joints per body + 1 for actual n."}, {Name: "BodyCollidePairsN", Doc: "BodyCollidePairsN is the total number of pre-compiled collision pairs\nto examine."}, {Name: "pad"}, {Name: "Gravity", Doc: "Gravity is the gravity acceleration function"}}}) var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.GeomData", IDName: "geom-data", Doc: "GeomData contains all geometric data for narrow-phase collision.", Directives: []types.Directive{{Tool: "gosl", Directive: "start"}}, Fields: []types.Field{{Name: "BodyIdx"}, {Name: "Shape"}, {Name: "MinSize", Doc: "MinSize is the min of the Size dimensions."}, {Name: "Thick", Doc: "Thickness of shape."}, {Name: "Radius", Doc: "Radius is the effective radius for sphere-like elements (Sphere, Capsule, Cone)"}, {Name: "Size"}, {Name: "WbR", Doc: "World-to-Body transform\nPosition (R) (i.e., BodyPos)"}, {Name: "WbQ", Doc: "Quaternion (Q) (i.e., BodyQuat)"}, {Name: "BwR", Doc: "Body-to-World transform (inverse)\nPosition (R)"}, {Name: "BwQ", Doc: "Quaternion (Q)"}}}) var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.Shapes", IDName: "shapes", Doc: "Shapes are elemental shapes for rigid bodies.\nIn general, size dimensions are half values\n(e.g., radius, half-height, etc), which is natural for\ncenter-based body coordinates."}) - -var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.State", IDName: "state", Doc: "State contains the basic physical state including position, orientation, velocity.\nThese are only the values that can be either relative or absolute -- other physical\nstate values such as Mass should go in Rigid.", Methods: []types.Method{{Name: "MoveOnAxis", Doc: "MoveOnAxis moves (translates) the specified distance on the specified local axis,\nrelative to the current rotation orientation.\nThe axis is normalized prior to aplying the distance factor.\nSets the LinVel to motion vector.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z", "dist"}}, {Name: "MoveOnAxisAbs", Doc: "MoveOnAxisAbs moves (translates) the specified distance on the specified local axis,\nin absolute X,Y,Z coordinates (does not apply the Quat rotation factor.\nThe axis is normalized prior to aplying the distance factor.\nSets the LinVel to motion vector.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z", "dist"}}, {Name: "SetEulerRotation", Doc: "SetEulerRotation sets the rotation in Euler angles (degrees).", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z"}}, {Name: "EulerRotation", Doc: "EulerRotation returns the current rotation in Euler angles (degrees).", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Returns: []string{"Vector3"}}, {Name: "SetAxisRotation", Doc: "SetAxisRotation sets rotation from local axis and angle in degrees.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z", "angle"}}, {Name: "RotateOnAxis", Doc: "RotateOnAxis rotates around the specified local axis the specified angle in degrees.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z", "angle"}}, {Name: "RotateEuler", Doc: "RotateEuler rotates by given Euler angles (in degrees) relative to existing rotation.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z"}}}, Fields: []types.Field{{Name: "Pos", Doc: "Pos is the position of center of mass of object."}, {Name: "Quat", Doc: "Quat is the rotation specified as a quaternion."}, {Name: "LinVel", Doc: "LinVel is the linear velocity."}, {Name: "AngVel", Doc: "AngVel is the angular velocity."}}}) - -var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.World", IDName: "world", Doc: "World contains and manages all of the physics elements.", Directives: []types.Directive{{Tool: "go", Directive: "generate", Args: []string{"core", "generate", "-add-types", "-gosl"}}}, Fields: []types.Field{{Name: "GPU", Doc: "GPU determines whether to use GPU (else CPU)."}, {Name: "Params", Doc: "Params are global parameters."}, {Name: "CurrentWorld", Doc: "CurrentWorld is the [BodyWorld] value to use when creating new bodies.\nSet to -1 to create global elements that interact with everything,\nwhile 0 and positive numbers only interact amongst themselves."}, {Name: "WorldReplicasStart", Doc: "WorldReplicasStart is the starting body index for replicated world bodies,\nwhich is needed for viewers to efficiently select a specific world to view.\nThis is the start of the World=0 first instance."}, {Name: "WorldReplicasN", Doc: "WorldReplicasN is the number of body elements within each set of\nreplicated world bodies, which is needed for viewers to efficiently select\na specific world to view."}, {Name: "CurrentObject", Doc: "CurrentObject is the [BodyObject] value to use when creating new bodies.\nGenerally just increment when starting a new object. When using world replicas,\nthe object indexes are copied, so objects are indexed by world and object id."}, {Name: "Bodies", Doc: "Bodies are the rigid body elements (dynamic and static),\nspecifying the constant, non-dynamic properties,\nwhich is initial state for dynamics.\n[body][BodyVarsN]"}, {Name: "Joints", Doc: "Joints is a list of permanent joints connecting bodies,\nwhich do not change (no dynamic variables).\n[joint][JointVarsN]"}, {Name: "JointDoFs", Doc: "JointDoFs is a list of joint DoF parameters, allocated per joint.\n[dof][JointDoFVars]"}, {Name: "BodyJoints", Doc: "BodyJoints is a list of joint indexes for each dynamic body, for aggregating.\n[dyn body][parent, child][Params.BodyJointsMax]"}, {Name: "BodyCollidePairs", Doc: "BodyCollidePairs are pairs of Body indexes that could potentially collide\nbased on precomputed collision logic, using World, Group, and Joint indexes.\n[BodyCollidePairsN][2]"}, {Name: "Dynamics", Doc: "Dynamics are the dynamic rigid body elements: these actually move.\nThe first set of variables are for initial values, and the second current.\n[body][cur/next][DynamicVarsN]"}, {Name: "BroadContactsN", Doc: "BroadContactsN has number of points of broad contact\nbetween bodies. [1]"}, {Name: "BroadContacts", Doc: "BroadContacts are the results of broad-phase contact processing,\nestablishing possible points of contact between bodies.\n[ContactsMax][BroadContactVarsN]"}, {Name: "ContactsN", Doc: "ContactsN has number of points of narrow (final) contact\nbetween bodies. [1]"}, {Name: "Contacts", Doc: "Contacts are the results of narrow-phase contact processing,\nwhere only actual contacts with fully-specified values are present.\n[ContactsMax][ContactVarsN]"}, {Name: "JointControls", Doc: "JointControls are dynamic joint control inputs, per joint DoF\n(in correspondence with [JointDoFs]). This can be uploaded to the\nGPU at every step.\n[dof][JointControlVarsN]"}}}) diff --git a/yaegilab/labsymbols/cogentcore_org-lab-physics-phyxyz.go b/yaegilab/labsymbols/cogentcore_org-lab-physics-phyxyz.go index ae7df1e4..127a1718 100644 --- a/yaegilab/labsymbols/cogentcore_org-lab-physics-phyxyz.go +++ b/yaegilab/labsymbols/cogentcore_org-lab-physics-phyxyz.go @@ -10,17 +10,16 @@ import ( func init() { Symbols["cogentcore.org/lab/physics/phyxyz/phyxyz"] = map[string]reflect.Value{ // function, constant and variable definitions - "DepthImage": reflect.ValueOf(phyxyz.DepthImage), - "DepthNorm": reflect.ValueOf(phyxyz.DepthNorm), - "MakeStateToolbar": reflect.ValueOf(phyxyz.MakeStateToolbar), - "NewEditor": reflect.ValueOf(phyxyz.NewEditor), - "NewWorld": reflect.ValueOf(phyxyz.NewWorld), - "NoDisplayScene": reflect.ValueOf(phyxyz.NoDisplayScene), + "DepthImage": reflect.ValueOf(phyxyz.DepthImage), + "DepthNorm": reflect.ValueOf(phyxyz.DepthNorm), + "NewEditor": reflect.ValueOf(phyxyz.NewEditor), + "NewScene": reflect.ValueOf(phyxyz.NewScene), + "NoDisplayScene": reflect.ValueOf(phyxyz.NoDisplayScene), // type definitions "Camera": reflect.ValueOf((*phyxyz.Camera)(nil)), "Editor": reflect.ValueOf((*phyxyz.Editor)(nil)), + "Scene": reflect.ValueOf((*phyxyz.Scene)(nil)), "View": reflect.ValueOf((*phyxyz.View)(nil)), - "World": reflect.ValueOf((*phyxyz.World)(nil)), } } diff --git a/yaegilab/labsymbols/cogentcore_org-lab-physics.go b/yaegilab/labsymbols/cogentcore_org-lab-physics.go index 339782b4..b55cf7f4 100644 --- a/yaegilab/labsymbols/cogentcore_org-lab-physics.go +++ b/yaegilab/labsymbols/cogentcore_org-lab-physics.go @@ -13,7 +13,6 @@ func init() { Symbols["cogentcore.org/lab/physics/physics"] = map[string]reflect.Value{ // function, constant and variable definitions "AddBroadContacts": reflect.ValueOf(physics.AddBroadContacts), - "AngMotionMax": reflect.ValueOf(constant.MakeFromLiteral("0.78539816339744830961566084581987572104929234984377645524373614869749573092390898867610892833397416837141584743218870486667557894677841571570962890997663229058521681784366119671858541551046073436737060546875", token.FLOAT, 0)), "AngularCorrection": reflect.ValueOf(physics.AngularCorrection), "Ball": reflect.ValueOf(physics.Ball), "Bodies": reflect.ValueOf(&physics.Bodies).Elem(), @@ -60,6 +59,7 @@ func init() { "BodyJoints": reflect.ValueOf(&physics.BodyJoints).Elem(), "BodyJointsVar": reflect.ValueOf(physics.BodyJointsVar), "BodyMass": reflect.ValueOf(physics.BodyMass), + "BodyObject": reflect.ValueOf(physics.BodyObject), "BodyPos": reflect.ValueOf(physics.BodyPos), "BodyPosX": reflect.ValueOf(physics.BodyPosX), "BodyPosY": reflect.ValueOf(physics.BodyPosY), @@ -223,6 +223,7 @@ func init() { "GPUVarsValues": reflect.ValueOf(physics.GPUVarsValues), "GetBodyDynamic": reflect.ValueOf(physics.GetBodyDynamic), "GetBodyGroup": reflect.ValueOf(physics.GetBodyGroup), + "GetBodyObject": reflect.ValueOf(physics.GetBodyObject), "GetBodyShape": reflect.ValueOf(physics.GetBodyShape), "GetBodyWorld": reflect.ValueOf(physics.GetBodyWorld), "GetBroadContactA": reflect.ValueOf(physics.GetBroadContactA), @@ -336,7 +337,7 @@ func init() { "Joints": reflect.ValueOf(&physics.Joints).Elem(), "JointsVar": reflect.ValueOf(physics.JointsVar), "NewGeomData": reflect.ValueOf(physics.NewGeomData), - "NewWorld": reflect.ValueOf(physics.NewWorld), + "NewModel": reflect.ValueOf(physics.NewModel), "OneIfNonzero": reflect.ValueOf(physics.OneIfNonzero), "Params": reflect.ValueOf(&physics.Params).Elem(), "ParamsVar": reflect.ValueOf(physics.ParamsVar), @@ -407,6 +408,7 @@ func init() { "SetBodyHSize": reflect.ValueOf(physics.SetBodyHSize), "SetBodyInertia": reflect.ValueOf(physics.SetBodyInertia), "SetBodyInvInertia": reflect.ValueOf(physics.SetBodyInvInertia), + "SetBodyObject": reflect.ValueOf(physics.SetBodyObject), "SetBodyPos": reflect.ValueOf(physics.SetBodyPos), "SetBodyQuat": reflect.ValueOf(physics.SetBodyQuat), "SetBodyShape": reflect.ValueOf(physics.SetBodyShape), @@ -486,7 +488,6 @@ func init() { "WorldsCollide": reflect.ValueOf(physics.WorldsCollide), // type definitions - "BBox": reflect.ValueOf((*physics.BBox)(nil)), "BodyVars": reflect.ValueOf((*physics.BodyVars)(nil)), "ContactVars": reflect.ValueOf((*physics.ContactVars)(nil)), "DynamicVars": reflect.ValueOf((*physics.DynamicVars)(nil)), @@ -496,9 +497,8 @@ func init() { "JointDoFVars": reflect.ValueOf((*physics.JointDoFVars)(nil)), "JointTypes": reflect.ValueOf((*physics.JointTypes)(nil)), "JointVars": reflect.ValueOf((*physics.JointVars)(nil)), + "Model": reflect.ValueOf((*physics.Model)(nil)), "PhysParams": reflect.ValueOf((*physics.PhysParams)(nil)), "Shapes": reflect.ValueOf((*physics.Shapes)(nil)), - "State": reflect.ValueOf((*physics.State)(nil)), - "World": reflect.ValueOf((*physics.World)(nil)), } } From 92b60bebf49f5b0796b0fcda02fcb22308478c7b Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Sat, 27 Dec 2025 09:11:30 +0100 Subject: [PATCH 56/97] physics: rename View -> Skin; builder first-pass working --- physics/body.go | 25 +-- physics/body.goal | 25 +-- physics/builder/body.go | 98 +++++++-- physics/builder/builder.go | 59 ++++++ physics/builder/joint.go | 167 +++++++++++++++ physics/builder/object.go | 16 ++ physics/builder/typegen.go | 209 +++++++++++++++++++ physics/builder/world.go | 17 +- physics/contact.go | 22 +- physics/contact.goal | 22 +- physics/control.go | 18 +- physics/control.goal | 18 +- physics/enumgen.go | 14 +- physics/examples/balls/balls.go | 2 +- physics/examples/pendula/pendula.go | 36 ++-- physics/model.go | 18 +- physics/model.goal | 18 +- physics/phyxyz/editor.go | 2 +- physics/phyxyz/{world.go => scene.go} | 37 ++-- physics/phyxyz/{view.go => skin.go} | 228 +++++++++++---------- physics/phyxyz/typegen.go | 49 ++++- physics/shaders/CollisionBroad.wgsl | 89 ++++---- physics/shaders/CollisionNarrow.wgsl | 89 ++++---- physics/shaders/DynamicsCurToNext.wgsl | 89 ++++---- physics/shaders/ForcesFromJoints.wgsl | 89 ++++---- physics/shaders/InitDynamics.wgsl | 89 ++++---- physics/shaders/StepBodyContactDeltas.wgsl | 89 ++++---- physics/shaders/StepBodyContacts.wgsl | 89 ++++---- physics/shaders/StepBodyJointDeltas.wgsl | 89 ++++---- physics/shaders/StepIntegrateBodies.wgsl | 89 ++++---- physics/shaders/StepJointForces.wgsl | 89 ++++---- physics/shaders/StepSolveJoints.wgsl | 89 ++++---- physics/typegen.go | 2 +- 33 files changed, 1319 insertions(+), 762 deletions(-) create mode 100644 physics/builder/typegen.go rename physics/phyxyz/{world.go => scene.go} (73%) rename physics/phyxyz/{view.go => skin.go} (52%) diff --git a/physics/body.go b/physics/body.go index e154199f..94e55cf4 100644 --- a/physics/body.go +++ b/physics/body.go @@ -46,14 +46,6 @@ const ( // does not need to be handled here. BodyGroup - // BodyObject identifies bodies that all belong to the same overall - // physical object (e.g., a robot or simulated animal). It has no - // implications for collision or physics simulation. Instead, - // it is used for external manipulation on entire objects at a time, - // e.g. via PositionObject. When using world replicas, the object - // indexes are copied, so objects are indexed by world and object id. - BodyObject - // BodyHSize is the half-size (e.g., radius) of the body. // Values depend on shape type: X is generally radius, // Y is half-height. @@ -176,17 +168,6 @@ func GetBodyGroup(idx int32) int32 { return int32(math.Float32bits(Bodies.Value(int(idx), int(BodyGroup)))) } -// SetBodyObject sets the [BodyObject] index for given body. -// Object indexes are used for external manipulation of multiple -// bodies in a coherent manner. -func SetBodyObject(idx, w int32) { - Bodies.Set(math.Float32frombits(uint32(w)), int(idx), int(BodyObject)) -} - -func GetBodyObject(idx int32) int32 { - return int32(math.Float32bits(Bodies.Value(int(idx), int(BodyObject)))) -} - func BodyHSize(idx int32) math32.Vector3 { return math32.Vec3(Bodies.Value(int(idx), int(BodyHSizeX)), Bodies.Value(int(idx), int(BodyHSizeY)), Bodies.Value(int(idx), int(BodyHSizeZ))) } @@ -272,6 +253,12 @@ func SetBodyInvInertia(idx int32, invInertia math32.Matrix3) { } } +// SetBodyThick specifies the thickness of the body, as a hollow shape. +// if 0, then it is solid. +func SetBodyThick(idx int32, val float32) { + Bodies.Set(val, int(idx), int(BodyThick)) +} + // SetBodyBounce specifies the COR or coefficient of restitution (0..1), // which determines how elastic the collision is, // i.e., final velocity / initial velocity. diff --git a/physics/body.goal b/physics/body.goal index e6b23aa2..f6e86b03 100644 --- a/physics/body.goal +++ b/physics/body.goal @@ -44,14 +44,6 @@ const ( // does not need to be handled here. BodyGroup - // BodyObject identifies bodies that all belong to the same overall - // physical object (e.g., a robot or simulated animal). It has no - // implications for collision or physics simulation. Instead, - // it is used for external manipulation on entire objects at a time, - // e.g. via PositionObject. When using world replicas, the object - // indexes are copied, so objects are indexed by world and object id. - BodyObject - // BodyHSize is the half-size (e.g., radius) of the body. // Values depend on shape type: X is generally radius, // Y is half-height. @@ -174,17 +166,6 @@ func GetBodyGroup(idx int32) int32 { return int32(math.Float32bits(Bodies[idx, BodyGroup])) } -// SetBodyObject sets the [BodyObject] index for given body. -// Object indexes are used for external manipulation of multiple -// bodies in a coherent manner. -func SetBodyObject(idx, w int32) { - Bodies[idx, BodyObject] = math.Float32frombits(uint32(w)) -} - -func GetBodyObject(idx int32) int32 { - return int32(math.Float32bits(Bodies[idx, BodyObject])) -} - func BodyHSize(idx int32) math32.Vector3 { return math32.Vec3(Bodies[idx, BodyHSizeX], Bodies[idx, BodyHSizeY], Bodies[idx, BodyHSizeZ]) } @@ -270,6 +251,12 @@ func SetBodyInvInertia(idx int32, invInertia math32.Matrix3) { } } +// SetBodyThick specifies the thickness of the body, as a hollow shape. +// if 0, then it is solid. +func SetBodyThick(idx int32, val float32) { + Bodies[idx, BodyThick] = val +} + // SetBodyBounce specifies the COR or coefficient of restitution (0..1), // which determines how elastic the collision is, // i.e., final velocity / initial velocity. diff --git a/physics/builder/body.go b/physics/builder/body.go index 25e78219..4c7f8219 100644 --- a/physics/builder/body.go +++ b/physics/builder/body.go @@ -12,6 +12,10 @@ import ( // Body is a rigid body. type Body struct { + // ObjectIndex is the index of body within parent [Object], + // which is used for id in [Builder] context. + ObjectIndex int + // Shape of the body. Shape physics.Shapes @@ -44,6 +48,12 @@ type Body struct { // Mass of the object. Only relevant for Dynamic bodies. Mass float32 + // Pose has the position and rotation. + Pose Pose + + // Com is the center-of-mass offset from the Pose.Pos. + Com math32.Vector3 + // Bounce specifies the COR or coefficient of restitution (0..1), // which determines how elastic the collision is, // i.e., final velocity / initial velocity. @@ -58,20 +68,84 @@ type Body struct { // FrictionRolling is resistance to rolling motion at contact. FrictionRolling float32 - // Pose has the position and rotation. - Pose Pose - - // Com is the center-of-mass offset from the Pose.Pos. - Com math32.Vector3 + // Optional [phyxyz.Skin] for visualizing the body. + Skin *phyxyz.Skin - // View is the view element for this Body (optional). - View *phyxyz.View - - // Index is the index of this body in the physics.World, + // BodyIndex is the index of this body in the [physics.Model] Bodies list, // once built. - Index int32 + BodyIndex int32 - // DynamicIndex is the index of this dynamic body in - // the physics.World, once built. + // DynamicIndex is the index of this dynamic body in the + // [physics.Model] Dynamics list, once built. DynamicIndex int32 } + +// NewBody adds a new body with given parameters. +// Returns the [Body] which can then be further customized. +// Use this for Static elements; NewDynamic for dynamic elements. +func (ob *Object) NewBody(shape physics.Shapes, hsize, pos math32.Vector3, rot math32.Quat) *Body { + idx := len(ob.Bodies) + bd := Body{ObjectIndex: idx, Shape: shape, HSize: hsize} + bd.Pose.Pos = pos + bd.Pose.Quat = rot + ob.Bodies = append(ob.Bodies, bd) + return &(ob.Bodies[idx]) +} + +// NewDynamic adds a new dynamic body with given parameters. +// Returns the [Body] which can then be further customized. +func (ob *Object) NewDynamic(shape physics.Shapes, mass float32, hsize, pos math32.Vector3, rot math32.Quat) *Body { + bd := ob.NewBody(shape, hsize, pos, rot) + bd.Dynamic = true + bd.Mass = mass + return bd +} + +// NewBodySkin adds a new body with given parameters, including name and +// color parameters used for intializing a [phyxyz.Skin] in given [phyxyz.Scene]. +// Returns the [Body] which can then be further customized. +// Use this for Static elements; NewDynamicSkin for dynamic elements. +func (ob *Object) NewBodySkin(sc *phyxyz.Scene, name string, shape physics.Shapes, clr string, hsize, pos math32.Vector3, rot math32.Quat) *Body { + bd := ob.NewBody(shape, hsize, pos, rot) + sk := sc.NewSkin(shape, name, clr, hsize, pos, rot) + bd.Skin = sk + return bd +} + +// NewDynamicSkin adds a new dynamic body with given parameters, +// including name and color parameters used for intializing a [phyxyz.Skin] +// in given [phyxyz.Scene]. +// Returns the [Body] which can then be further customized. +func (ob *Object) NewDynamicSkin(sc *phyxyz.Scene, name string, shape physics.Shapes, clr string, mass float32, hsize, pos math32.Vector3, rot math32.Quat) *Body { + bd := ob.NewBodySkin(sc, name, shape, clr, hsize, pos, rot) + bd.Dynamic = true + bd.Mass = mass + return bd +} + +/////// Physics functions + +func (bd *Body) NewPhysicsBody(ml *physics.Model, world int) { + var bi, di int32 + if bd.Dynamic { + bi, di = ml.NewDynamic(bd.Shape, bd.Mass, bd.HSize, bd.Pose.Pos, bd.Pose.Quat) + bd.BodyIndex = bi + bd.DynamicIndex = di + } else { + bi = ml.NewBody(bd.Shape, bd.HSize, bd.Pose.Pos, bd.Pose.Quat) + bd.DynamicIndex = -1 + bd.BodyIndex = bi + } + physics.SetBodyWorld(bi, int32(world)) + physics.SetBodyGroup(bi, int32(bd.Group)) + if bd.Skin != nil { + bd.Skin.BodyIndex = bi + bd.Skin.DynamicIndex = di + } + physics.SetBodyThick(bi, bd.Thick) + physics.SetBodyCom(bi, bd.Com) + physics.SetBodyBounce(bi, bd.Bounce) + physics.SetBodyFriction(bi, bd.Friction) + physics.SetBodyFrictionTortion(bi, bd.FrictionTortion) + physics.SetBodyFrictionRolling(bi, bd.FrictionRolling) +} diff --git a/physics/builder/builder.go b/physics/builder/builder.go index db289e8c..efa69cb5 100644 --- a/physics/builder/builder.go +++ b/physics/builder/builder.go @@ -4,6 +4,13 @@ package builder +//go:generate core generate -add-types -setters + +import ( + "cogentcore.org/lab/physics" + "cogentcore.org/lab/physics/phyxyz" +) + // Builder is the global container of [physics.Model] elements, // organized into worlds that are independently updated. type Builder struct { @@ -11,3 +18,55 @@ type Builder struct { // Worlds are the independent world elements. Worlds []World } + +func NewBuilder() *Builder { + return &Builder{} +} + +// Reset starts over making a new model. +func (bl *Builder) Reset() { + bl.Worlds = nil +} + +func (bl *Builder) World(idx int) *World { + return &bl.Worlds[idx] +} + +// NewGlobalWorld creates a new world with World index = -1, +// which are globals that collide with all worlds. +func (bl *Builder) NewGlobalWorld() *World { + idx := len(bl.Worlds) + bl.Worlds = append(bl.Worlds, World{World: -1}) + return &bl.Worlds[idx] +} + +// NewWorld creates a new standard (non-global) world, with +// world index = index of last one + 1. +func (bl *Builder) NewWorld() *World { + wn := 0 + idx := len(bl.Worlds) + if idx > 0 { + wn = bl.Worlds[idx-1].World + } + bl.Worlds = append(bl.Worlds, World{World: wn}) + return &bl.Worlds[idx] +} + +// Build builds a physics model, with optional [phyxyz.Scene] for +// visualization (using Skin elements created for bodies). +func (bl *Builder) Build(ml *physics.Model, sc *phyxyz.Scene) { + for wi := range bl.Worlds { + wl := bl.World(wi) + for oi := range wl.Objects { + ob := wl.Object(oi) + for bbi := range ob.Bodies { + bd := ob.Body(bbi) + bd.NewPhysicsBody(ml, wl.World) + } + for bji := range ob.Joints { + jd := ob.Joint(bji) + jd.NewPhysicsJoint(ml, ob) + } + } + } +} diff --git a/physics/builder/joint.go b/physics/builder/joint.go index 4fb579c6..e9680f8e 100644 --- a/physics/builder/joint.go +++ b/physics/builder/joint.go @@ -37,6 +37,9 @@ type Joint struct { // DoFs are the degrees-of-freedom for this joint. DoFs []DoF + + // JointIndex is the index of this joint in [physics.Joints] when built. + JointIndex int32 } // DoF is a degree-of-freedom for a [Joint]. @@ -46,4 +49,168 @@ type DoF struct { // Limit has the limits for motion of this DoF. Limit minmax.F32 + + // TargetPos is the position target value, where 0 is the initial + // position. For angular joints, this is in radians. + TargetPos float32 + + // TargetStiff determines how strongly the target position + // is enforced: 0 = not at all; larger = stronger (e.g., 1000 or higher). + // Set to 0 to allow the joint to be fully flexible. + TargetStiff float32 + + // TargetVel is the velocity target value. For example, 0 + // effectively damps joint movement in proportion to Damp parameter. + TargetVel float32 + + // TargetDamp determines how strongly the target velocity is enforced: + // 0 = not at all; larger = stronger (e.g., 1 is reasonable). + // Set to 0 to allow the joint to be fully flexible. + TargetDamp float32 +} + +func (jd *Joint) DoF(idx int) *DoF { + return &jd.DoFs[idx] +} + +// newJoint adds a new joint of given type. +func (ob *Object) newJoint(typ physics.JointTypes, parent, child *Body, ppos, cpos math32.Vector3, linDoF, angDoF int) *Joint { + pidx := -1 + if parent != nil { + pidx = parent.ObjectIndex + } + idx := len(ob.Joints) + jt := Joint{Parent: pidx, Child: child.ObjectIndex, Type: typ, LinearDoFN: linDoF, AngularDoFN: angDoF} + jt.PPose.Pos = ppos + jt.CPose.Pos = cpos + ndof := linDoF + angDoF + if ndof > 0 { + jt.DoFs = make([]DoF, linDoF+angDoF) + for i := range ndof { + jt.DoFs[i].Limit.Min = -physics.JointLimitUnlimited + jt.DoFs[i].Limit.Max = physics.JointLimitUnlimited + } + } + ob.Joints = append(ob.Joints, jt) + return &ob.Joints[idx] +} + +// NewJointFixed adds a new Fixed joint as a child of given parent. +// Use nil for parent to add a world-anchored joint. +// ppos, cpos are the relative positions from the parent, child. +// These are for the non-rotated body (i.e., body rotation is applied +// to these positions as well). +// Sets relative rotation matricies to identity by default. +func (ob *Object) NewJointFixed(parent, child *Body, ppos, cpos math32.Vector3) *Joint { + return ob.newJoint(physics.Fixed, parent, child, ppos, cpos, 0, 0) +} + +// NewJointPrismatic adds a new Prismatic (slider) joint as a child +// of given parent. Use nil for parent to add a world-anchored joint. +// ppos, cpos are the relative positions from the parent, child. +// These are for the non-rotated body (i.e., body rotation is applied +// to these positions as well). +// Sets relative rotation matricies to identity by default. +// axis is the axis of articulation for the joint. +// Use [SetJointDoF] to set the remaining DoF parameters. +func (ob *Object) NewJointPrismatic(parent, child *Body, ppos, cpos, axis math32.Vector3) *Joint { + jt := ob.newJoint(physics.Prismatic, parent, child, ppos, cpos, 1, 0) + jt.DoFs[0].Axis = axis + return jt +} + +// NewJointRevolute adds a new Revolute (hinge, axel) joint as a child +// of given parent. Use nil for parent to add a world-anchored joint. +// ppos, cpos are the relative positions from the parent, child. +// These are for the non-rotated body (i.e., body rotation is applied +// to these positions as well). +// Sets relative rotation matricies to identity by default. +// axis is the axis of articulation for the joint. +// Use [SetJointDoF] to set the remaining DoF parameters. +func (ob *Object) NewJointRevolute(parent, child *Body, ppos, cpos, axis math32.Vector3) *Joint { + jt := ob.newJoint(physics.Revolute, parent, child, ppos, cpos, 0, 1) + jt.DoFs[0].Axis = axis + return jt +} + +// NewJointBall adds a new Ball joint (3 angular DoF) as a child +// of given parent. Use nil for parent to add a world-anchored joint. +// ppos, cpos are the relative positions from the parent, child. +// These are for the non-rotated body (i.e., body rotation is applied +// to these positions as well). +// Sets relative rotation matricies to identity by default. +// Use [SetJointDoF] to set the remaining DoF parameters. +func (ob *Object) NewJointBall(parent, child *Body, ppos, cpos math32.Vector3) *Joint { + jt := ob.newJoint(physics.Ball, parent, child, ppos, cpos, 0, 3) + return jt +} + +// NewJointDistance adds a new Distance joint (6 DoF), +// with distance constrained only on the first linear X axis, +// as a child of given parent. Use nil for parent to add a world-anchored joint. +// ppos, cpos are the relative positions from the parent, child. +// These are for the non-rotated body (i.e., body rotation is applied +// to these positions as well). +// Sets relative rotation matricies to identity by default. +// Use [SetJointDoF] to set the remaining DoF parameters. +func (ob *Object) NewJointDistance(parent, child *Body, ppos, cpos math32.Vector3, minDist, maxDist float32) *Joint { + jt := ob.newJoint(physics.Ball, parent, child, ppos, cpos, 3, 3) + jt.DoFs[0].Limit.Min = minDist + jt.DoFs[0].Limit.Max = maxDist + return jt +} + +// NewJointFree adds a new Free joint as a child +// of given parent. Use nil for parent to add a world-anchored joint. +// ppos, cpos are the relative positions from the parent, child. +// These are for the non-rotated body (i.e., body rotation is applied +// to these positions as well). +// Sets relative rotation matricies to identity by default. +// Use [SetJointDoF] to set the remaining DoF parameters. +func (ob *Object) NewJointFree(ml *physics.Model, parent, child *Body, ppos, cpos math32.Vector3) *Joint { + jt := ob.newJoint(physics.Free, parent, child, ppos, cpos, 0, 0) + return jt +} + +// NewPhysicsJoint makes the physics joint for joint +func (jd *Joint) NewPhysicsJoint(ml *physics.Model, ob *Object) int32 { + pi := jd.Parent + pdi := int32(-1) + if pi >= 0 { + pb := ob.Body(pi) + pdi = pb.DynamicIndex // todo: validate + } + cb := ob.Body(jd.Child) + cdi := cb.DynamicIndex + ji := int32(0) + switch jd.Type { + case physics.Prismatic: + ji = ml.NewJointPrismatic(pdi, cdi, jd.PPose.Pos, jd.CPose.Pos, jd.DoFs[0].Axis) + case physics.Revolute: + ji = ml.NewJointRevolute(pdi, cdi, jd.PPose.Pos, jd.CPose.Pos, jd.DoFs[0].Axis) + case physics.Ball: + ji = ml.NewJointBall(pdi, cdi, jd.PPose.Pos, jd.CPose.Pos) + case physics.Distance: + ji = ml.NewJointBall(pdi, cdi, jd.PPose.Pos, jd.CPose.Pos) + case physics.Free: + ji = ml.NewJointFree(pdi, cdi, jd.PPose.Pos, jd.CPose.Pos) + } + for i := range jd.LinearDoFN { + d := jd.DoF(i) + di := int32(i) + physics.SetJointDoF(ji, di, physics.JointLimitLower, d.Limit.Min) + physics.SetJointDoF(ji, di, physics.JointLimitUpper, d.Limit.Max) + physics.SetJointTargetPos(ji, di, d.TargetPos, d.TargetStiff) + physics.SetJointTargetVel(ji, di, d.TargetVel, d.TargetDamp) + } + for i := range jd.AngularDoFN { + d := jd.DoF(i) + di := int32(i + jd.LinearDoFN) + physics.SetJointDoF(ji, di, physics.JointLimitLower, d.Limit.Min) + physics.SetJointDoF(ji, di, physics.JointLimitUpper, d.Limit.Max) + physics.SetJointTargetPos(ji, di, d.TargetPos, d.TargetStiff) + physics.SetJointTargetVel(ji, di, d.TargetVel, d.TargetDamp) + } + jd.JointIndex = ji + return ji } diff --git a/physics/builder/object.go b/physics/builder/object.go index 034ace8c..a6441b72 100644 --- a/physics/builder/object.go +++ b/physics/builder/object.go @@ -4,7 +4,23 @@ package builder +// Object is an object within the [World]. +// Each object is a coherent collection of bodies, typically +// connected by joints. This is an organizational convenience +// for positioning elements; has no physical implications. type Object struct { + // Bodies are the bodies in the object. Bodies []Body + + // Joints are joints connecting object bodies. + // Joint indexes here refer strictly within bodies. Joints []Joint } + +func (ob *Object) Body(idx int) *Body { + return &ob.Bodies[idx] +} + +func (ob *Object) Joint(idx int) *Joint { + return &ob.Joints[idx] +} diff --git a/physics/builder/typegen.go b/physics/builder/typegen.go new file mode 100644 index 00000000..e74f20c5 --- /dev/null +++ b/physics/builder/typegen.go @@ -0,0 +1,209 @@ +// Code generated by "core generate -add-types -setters"; DO NOT EDIT. + +package builder + +import ( + "cogentcore.org/core/math32" + "cogentcore.org/core/math32/minmax" + "cogentcore.org/core/types" + "cogentcore.org/lab/physics" + "cogentcore.org/lab/physics/phyxyz" +) + +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics/builder.Body", IDName: "body", Doc: "Body is a rigid body.", Fields: []types.Field{{Name: "ObjectIndex", Doc: "ObjectIndex is the index of body within parent [Object],\nwhich is used for id in [Builder] context."}, {Name: "Shape", Doc: "Shape of the body."}, {Name: "Dynamic", Doc: "Dynamic makes this a dynamic body."}, {Name: "Group", Doc: "Group partitions bodies within worlds into different groups\nfor collision detection. 0 does not collide with anything.\nNegative numbers are global within a world, except they don't\ncollide amongst themselves (all non-dynamic bodies should go\nin -1 because they don't collide amongst each-other, but do\npotentially collide with dynamics).\nPositive numbers only collide amongst themselves, and with\nnegative groups, but not other positive groups. To avoid\nunwanted collisions, put bodies into separate groups.\nThere is an automatic constraint that the two objects\nwithin a single joint do not collide with each other, so this\ndoes not need to be handled here."}, {Name: "HSize", Doc: "HSize is the half-size (e.g., radius) of the body.\nValues depend on shape type: X is generally radius,\nY is half-height."}, {Name: "Thick", Doc: "Thick is the thickness of the body, as a hollow shape.\nIf 0, then it is a solid shape (default)."}, {Name: "Mass", Doc: "Mass of the object. Only relevant for Dynamic bodies."}, {Name: "Pose", Doc: "Pose has the position and rotation."}, {Name: "Com", Doc: "Com is the center-of-mass offset from the Pose.Pos."}, {Name: "Bounce", Doc: "Bounce specifies the COR or coefficient of restitution (0..1),\nwhich determines how elastic the collision is,\ni.e., final velocity / initial velocity."}, {Name: "Friction", Doc: "Friction is the standard coefficient for linear friction (mu)."}, {Name: "FrictionTortion", Doc: "FrictionTortion is resistance to spinning at the contact point."}, {Name: "FrictionRolling", Doc: "FrictionRolling is resistance to rolling motion at contact."}, {Name: "Skin", Doc: "Optional [phyxyz.Skin] for visualizing the body."}, {Name: "BodyIndex", Doc: "BodyIndex is the index of this body in the [physics.Model] Bodies list,\nonce built."}, {Name: "DynamicIndex", Doc: "DynamicIndex is the index of this dynamic body in the\n[physics.Model] Dynamics list, once built."}}}) + +// SetObjectIndex sets the [Body.ObjectIndex]: +// ObjectIndex is the index of body within parent [Object], +// which is used for id in [Builder] context. +func (t *Body) SetObjectIndex(v int) *Body { t.ObjectIndex = v; return t } + +// SetShape sets the [Body.Shape]: +// Shape of the body. +func (t *Body) SetShape(v physics.Shapes) *Body { t.Shape = v; return t } + +// SetDynamic sets the [Body.Dynamic]: +// Dynamic makes this a dynamic body. +func (t *Body) SetDynamic(v bool) *Body { t.Dynamic = v; return t } + +// SetGroup sets the [Body.Group]: +// Group partitions bodies within worlds into different groups +// for collision detection. 0 does not collide with anything. +// Negative numbers are global within a world, except they don't +// collide amongst themselves (all non-dynamic bodies should go +// in -1 because they don't collide amongst each-other, but do +// potentially collide with dynamics). +// Positive numbers only collide amongst themselves, and with +// negative groups, but not other positive groups. To avoid +// unwanted collisions, put bodies into separate groups. +// There is an automatic constraint that the two objects +// within a single joint do not collide with each other, so this +// does not need to be handled here. +func (t *Body) SetGroup(v int) *Body { t.Group = v; return t } + +// SetHSize sets the [Body.HSize]: +// HSize is the half-size (e.g., radius) of the body. +// Values depend on shape type: X is generally radius, +// Y is half-height. +func (t *Body) SetHSize(v math32.Vector3) *Body { t.HSize = v; return t } + +// SetThick sets the [Body.Thick]: +// Thick is the thickness of the body, as a hollow shape. +// If 0, then it is a solid shape (default). +func (t *Body) SetThick(v float32) *Body { t.Thick = v; return t } + +// SetMass sets the [Body.Mass]: +// Mass of the object. Only relevant for Dynamic bodies. +func (t *Body) SetMass(v float32) *Body { t.Mass = v; return t } + +// SetPose sets the [Body.Pose]: +// Pose has the position and rotation. +func (t *Body) SetPose(v Pose) *Body { t.Pose = v; return t } + +// SetCom sets the [Body.Com]: +// Com is the center-of-mass offset from the Pose.Pos. +func (t *Body) SetCom(v math32.Vector3) *Body { t.Com = v; return t } + +// SetBounce sets the [Body.Bounce]: +// Bounce specifies the COR or coefficient of restitution (0..1), +// which determines how elastic the collision is, +// i.e., final velocity / initial velocity. +func (t *Body) SetBounce(v float32) *Body { t.Bounce = v; return t } + +// SetFriction sets the [Body.Friction]: +// Friction is the standard coefficient for linear friction (mu). +func (t *Body) SetFriction(v float32) *Body { t.Friction = v; return t } + +// SetFrictionTortion sets the [Body.FrictionTortion]: +// FrictionTortion is resistance to spinning at the contact point. +func (t *Body) SetFrictionTortion(v float32) *Body { t.FrictionTortion = v; return t } + +// SetFrictionRolling sets the [Body.FrictionRolling]: +// FrictionRolling is resistance to rolling motion at contact. +func (t *Body) SetFrictionRolling(v float32) *Body { t.FrictionRolling = v; return t } + +// SetSkin sets the [Body.Skin]: +// Optional [phyxyz.Skin] for visualizing the body. +func (t *Body) SetSkin(v *phyxyz.Skin) *Body { t.Skin = v; return t } + +// SetBodyIndex sets the [Body.BodyIndex]: +// BodyIndex is the index of this body in the [physics.Model] Bodies list, +// once built. +func (t *Body) SetBodyIndex(v int32) *Body { t.BodyIndex = v; return t } + +// SetDynamicIndex sets the [Body.DynamicIndex]: +// DynamicIndex is the index of this dynamic body in the +// [physics.Model] Dynamics list, once built. +func (t *Body) SetDynamicIndex(v int32) *Body { t.DynamicIndex = v; return t } + +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics/builder.Builder", IDName: "builder", Doc: "Builder is the global container of [physics.Model] elements,\norganized into worlds that are independently updated.", Fields: []types.Field{{Name: "Worlds", Doc: "Worlds are the independent world elements."}}}) + +// SetWorlds sets the [Builder.Worlds]: +// Worlds are the independent world elements. +func (t *Builder) SetWorlds(v ...World) *Builder { t.Worlds = v; return t } + +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics/builder.Joint", IDName: "joint", Doc: "Joint describes a joint between two bodies.", Fields: []types.Field{{Name: "Parent", Doc: "Parent is index within an Object for parent body."}, {Name: "Child", Doc: "Parent is index within an Object for parent body."}, {Name: "Type", Doc: "Type is the type of the joint."}, {Name: "PPose", Doc: "PPose is the parent position and orientation of the joint\nin the parent's body-centered coordinates."}, {Name: "CPose", Doc: "CPose is the child position and orientation of the joint\nin the parent's body-centered coordinates."}, {Name: "LinearDoFN", Doc: "LinearDoFN is the number of linear degrees of freedom (3 max)."}, {Name: "AngularDoFN", Doc: "AngularDoFN is the number of linear degrees of freedom (3 max)."}, {Name: "DoFs", Doc: "DoFs are the degrees-of-freedom for this joint."}, {Name: "JointIndex", Doc: "JointIndex is the index of this joint in [physics.Joints] when built."}}}) + +// SetParent sets the [Joint.Parent]: +// Parent is index within an Object for parent body. +func (t *Joint) SetParent(v int) *Joint { t.Parent = v; return t } + +// SetChild sets the [Joint.Child]: +// Parent is index within an Object for parent body. +func (t *Joint) SetChild(v int) *Joint { t.Child = v; return t } + +// SetType sets the [Joint.Type]: +// Type is the type of the joint. +func (t *Joint) SetType(v physics.JointTypes) *Joint { t.Type = v; return t } + +// SetPPose sets the [Joint.PPose]: +// PPose is the parent position and orientation of the joint +// in the parent's body-centered coordinates. +func (t *Joint) SetPPose(v Pose) *Joint { t.PPose = v; return t } + +// SetCPose sets the [Joint.CPose]: +// CPose is the child position and orientation of the joint +// in the parent's body-centered coordinates. +func (t *Joint) SetCPose(v Pose) *Joint { t.CPose = v; return t } + +// SetLinearDoFN sets the [Joint.LinearDoFN]: +// LinearDoFN is the number of linear degrees of freedom (3 max). +func (t *Joint) SetLinearDoFN(v int) *Joint { t.LinearDoFN = v; return t } + +// SetAngularDoFN sets the [Joint.AngularDoFN]: +// AngularDoFN is the number of linear degrees of freedom (3 max). +func (t *Joint) SetAngularDoFN(v int) *Joint { t.AngularDoFN = v; return t } + +// SetDoFs sets the [Joint.DoFs]: +// DoFs are the degrees-of-freedom for this joint. +func (t *Joint) SetDoFs(v ...DoF) *Joint { t.DoFs = v; return t } + +// SetJointIndex sets the [Joint.JointIndex]: +// JointIndex is the index of this joint in [physics.Joints] when built. +func (t *Joint) SetJointIndex(v int32) *Joint { t.JointIndex = v; return t } + +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics/builder.DoF", IDName: "do-f", Doc: "DoF is a degree-of-freedom for a [Joint].", Fields: []types.Field{{Name: "Axis", Doc: "Axis is the axis of articulation."}, {Name: "Limit", Doc: "Limit has the limits for motion of this DoF."}, {Name: "TargetPos", Doc: "TargetPos is the position target value, where 0 is the initial\nposition. For angular joints, this is in radians."}, {Name: "TargetStiff", Doc: "TargetStiff determines how strongly the target position\nis enforced: 0 = not at all; larger = stronger (e.g., 1000 or higher).\nSet to 0 to allow the joint to be fully flexible."}, {Name: "TargetVel", Doc: "TargetVel is the velocity target value. For example, 0\neffectively damps joint movement in proportion to Damp parameter."}, {Name: "TargetDamp", Doc: "TargetDamp determines how strongly the target velocity is enforced:\n0 = not at all; larger = stronger (e.g., 1 is reasonable).\nSet to 0 to allow the joint to be fully flexible."}}}) + +// SetAxis sets the [DoF.Axis]: +// Axis is the axis of articulation. +func (t *DoF) SetAxis(v math32.Vector3) *DoF { t.Axis = v; return t } + +// SetLimit sets the [DoF.Limit]: +// Limit has the limits for motion of this DoF. +func (t *DoF) SetLimit(v minmax.F32) *DoF { t.Limit = v; return t } + +// SetTargetPos sets the [DoF.TargetPos]: +// TargetPos is the position target value, where 0 is the initial +// position. For angular joints, this is in radians. +func (t *DoF) SetTargetPos(v float32) *DoF { t.TargetPos = v; return t } + +// SetTargetStiff sets the [DoF.TargetStiff]: +// TargetStiff determines how strongly the target position +// is enforced: 0 = not at all; larger = stronger (e.g., 1000 or higher). +// Set to 0 to allow the joint to be fully flexible. +func (t *DoF) SetTargetStiff(v float32) *DoF { t.TargetStiff = v; return t } + +// SetTargetVel sets the [DoF.TargetVel]: +// TargetVel is the velocity target value. For example, 0 +// effectively damps joint movement in proportion to Damp parameter. +func (t *DoF) SetTargetVel(v float32) *DoF { t.TargetVel = v; return t } + +// SetTargetDamp sets the [DoF.TargetDamp]: +// TargetDamp determines how strongly the target velocity is enforced: +// 0 = not at all; larger = stronger (e.g., 1 is reasonable). +// Set to 0 to allow the joint to be fully flexible. +func (t *DoF) SetTargetDamp(v float32) *DoF { t.TargetDamp = v; return t } + +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics/builder.Object", IDName: "object", Doc: "Object is an object within the [World].\nEach object is a coherent collection of bodies, typically\nconnected by joints. This is an organizational convenience\nfor positioning elements; has no physical implications.", Fields: []types.Field{{Name: "Bodies", Doc: "Bodies are the bodies in the object."}, {Name: "Joints", Doc: "Joints are joints connecting object bodies.\nJoint indexes here refer strictly within bodies."}}}) + +// SetBodies sets the [Object.Bodies]: +// Bodies are the bodies in the object. +func (t *Object) SetBodies(v ...Body) *Object { t.Bodies = v; return t } + +// SetJoints sets the [Object.Joints]: +// Joints are joints connecting object bodies. +// Joint indexes here refer strictly within bodies. +func (t *Object) SetJoints(v ...Joint) *Object { t.Joints = v; return t } + +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics/builder.Pose", IDName: "pose", Doc: "Pose represents the 3D position and rotation.", Methods: []types.Method{{Name: "MoveOnAxis", Doc: "MoveOnAxis moves (translates) the specified distance on the specified local axis,\nrelative to the current rotation orientation.\nThe axis is normalized prior to aplying the distance factor.\nSets the LinVel to motion vector.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z", "dist"}}, {Name: "MoveOnAxisAbs", Doc: "MoveOnAxisAbs moves (translates) the specified distance on the specified local axis,\nin absolute X,Y,Z coordinates (does not apply the Quat rotation factor.\nThe axis is normalized prior to aplying the distance factor.\nSets the LinVel to motion vector.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z", "dist"}}, {Name: "SetEulerRotation", Doc: "SetEulerRotation sets the rotation in Euler angles (degrees).", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z"}}, {Name: "EulerRotation", Doc: "EulerRotation returns the current rotation in Euler angles (degrees).", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Returns: []string{"Vector3"}}, {Name: "SetAxisRotation", Doc: "SetAxisRotation sets rotation from local axis and angle in degrees.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z", "angle"}}, {Name: "RotateOnAxis", Doc: "RotateOnAxis rotates around the specified local axis the specified angle in degrees.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z", "angle"}}, {Name: "RotateEuler", Doc: "RotateEuler rotates by given Euler angles (in degrees) relative to existing rotation.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z"}}}, Fields: []types.Field{{Name: "Pos", Doc: "Pos is the position of center of mass of object."}, {Name: "Quat", Doc: "Quat is the rotation specified as a quaternion."}}}) + +// SetPos sets the [Pose.Pos]: +// Pos is the position of center of mass of object. +func (t *Pose) SetPos(v math32.Vector3) *Pose { t.Pos = v; return t } + +// SetQuat sets the [Pose.Quat]: +// Quat is the rotation specified as a quaternion. +func (t *Pose) SetQuat(v math32.Quat) *Pose { t.Quat = v; return t } + +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics/builder.World", IDName: "world", Doc: "World is one world within the Builder.", Fields: []types.Field{{Name: "World", Doc: "World is the world index. -1 = globals, else positive.. are distinct\nnon-interacting worlds."}, {Name: "Objects", Doc: "Objects are the objects within the [World].\nEach object is a coherent collection of bodies, typically\nconnected by joints. This is an organizational convenience\nfor positioning elements; has no physical implications."}}}) + +// SetWorld sets the [World.World]: +// World is the world index. -1 = globals, else positive.. are distinct +// non-interacting worlds. +func (t *World) SetWorld(v int) *World { t.World = v; return t } + +// SetObjects sets the [World.Objects]: +// Objects are the objects within the [World]. +// Each object is a coherent collection of bodies, typically +// connected by joints. This is an organizational convenience +// for positioning elements; has no physical implications. +func (t *World) SetObjects(v ...Object) *World { t.Objects = v; return t } diff --git a/physics/builder/world.go b/physics/builder/world.go index 4d9dc9c4..54ac69a5 100644 --- a/physics/builder/world.go +++ b/physics/builder/world.go @@ -6,12 +6,23 @@ package builder // World is one world within the Builder. type World struct { - // World is the world index. + // World is the world index. -1 = globals, else positive.. are distinct + // non-interacting worlds. World int - // Objects are the objects within the world. + // Objects are the objects within the [World]. // Each object is a coherent collection of bodies, typically // connected by joints. This is an organizational convenience - // for positioning elements as well. + // for positioning elements; has no physical implications. Objects []Object } + +func (wl *World) Object(idx int) *Object { + return &wl.Objects[idx] +} + +func (wl *World) NewObject() *Object { + idx := len(wl.Objects) + wl.Objects = append(wl.Objects, Object{}) + return &wl.Objects[idx] +} diff --git a/physics/contact.go b/physics/contact.go index 55fb8787..ac0a5555 100644 --- a/physics/contact.go +++ b/physics/contact.go @@ -707,13 +707,13 @@ func ContactConstraint(err float32, q0A, q0B math32.Quat, mInvA, mInvB float32, // IsChildDynamic returns true if dic is a direct child // on any joint where dip is the parent. -func (wl *Model) IsChildDynamic(dip, dic int32) bool { +func (ml *Model) IsChildDynamic(dip, dic int32) bool { if dip < 0 || dic < 0 { return false } - npja := wl.BodyJoints.Value(int(dip), int(0), int(0)) + npja := ml.BodyJoints.Value(int(dip), int(0), int(0)) for j := range npja { - ji := wl.BodyJoints.Value(int(dip), int(0), int(1+j)) + ji := ml.BodyJoints.Value(int(dip), int(0), int(1+j)) jci := JointChildIndex(ji) if jci == dic { return true @@ -728,8 +728,8 @@ func (wl *Model) IsChildDynamic(dip, dic int32) bool { // based on world and group settings and not being direct parent // child relationship within a joint. Result has A with lower shape type, // so that shapes are in a canonical order. -func (wl *Model) ConfigBodyCollidePairs() { - params := &wl.Params[0] +func (ml *Model) ConfigBodyCollidePairs() { + params := &ml.Params[0] nb := params.BodiesN nalc := int(nb) * 10 pt := tensor.NewInt32(nalc, 2) @@ -752,7 +752,7 @@ func (wl *Model) ConfigBodyCollidePairs() { } dib := GetBodyDynamic(b) // now check joints (ConfigJoints must have been called first) - if wl.IsChildDynamic(dia, dib) || wl.IsChildDynamic(dib, dia) { + if ml.IsChildDynamic(dia, dib) || ml.IsChildDynamic(dib, dia) { continue } if np >= nalc { @@ -778,7 +778,7 @@ func (wl *Model) ConfigBodyCollidePairs() { np = 1 } pt.SetShapeSizes(np, 2) - wl.BodyCollidePairs = pt + ml.BodyCollidePairs = pt BodyCollidePairs = pt // fmt.Println("body pairs over alloc", nalc - np, "total:", np) } @@ -787,8 +787,8 @@ func (wl *Model) ConfigBodyCollidePairs() { // SetMaxContacts computes [Params.MaxContacts] based on current list of // [BodyCollidePairs]. -func (wl *Model) SetMaxContacts() { - params := &wl.Params[0] +func (ml *Model) SetMaxContacts() { + params := &ml.Params[0] n := int32(0) for ci := range params.BodyCollidePairsN { @@ -816,6 +816,6 @@ func (wl *Model) SetMaxContacts() { } n = max(n, params.DynamicsN) params.ContactsMax = n - wl.BroadContacts.SetShapeSizes(int(n), int(BroadContactVarsN)) - wl.Contacts.SetShapeSizes(int(n), int(ContactVarsN)) + ml.BroadContacts.SetShapeSizes(int(n), int(BroadContactVarsN)) + ml.Contacts.SetShapeSizes(int(n), int(ContactVarsN)) } diff --git a/physics/contact.goal b/physics/contact.goal index 87f40ac9..24d0c366 100644 --- a/physics/contact.goal +++ b/physics/contact.goal @@ -706,13 +706,13 @@ func ContactConstraint(err float32, q0A, q0B math32.Quat, mInvA, mInvB float32, // IsChildDynamic returns true if dic is a direct child // on any joint where dip is the parent. -func (wl *Model) IsChildDynamic(dip, dic int32) bool { +func (ml *Model) IsChildDynamic(dip, dic int32) bool { if dip < 0 || dic < 0 { return false } - npja := wl.BodyJoints[dip, 0, 0] + npja := ml.BodyJoints[dip, 0, 0] for j := range npja { - ji := wl.BodyJoints[dip, 0, 1+j] + ji := ml.BodyJoints[dip, 0, 1+j] jci := JointChildIndex(ji) if jci == dic { return true @@ -727,8 +727,8 @@ func (wl *Model) IsChildDynamic(dip, dic int32) bool { // based on world and group settings and not being direct parent // child relationship within a joint. Result has A with lower shape type, // so that shapes are in a canonical order. -func (wl *Model) ConfigBodyCollidePairs() { - params := &wl.Params[0] +func (ml *Model) ConfigBodyCollidePairs() { + params := &ml.Params[0] nb := params.BodiesN nalc := int(nb) * 10 pt := tensor.NewInt32(nalc, 2) @@ -751,7 +751,7 @@ func (wl *Model) ConfigBodyCollidePairs() { } dib := GetBodyDynamic(b) // now check joints (ConfigJoints must have been called first) - if wl.IsChildDynamic(dia, dib) || wl.IsChildDynamic(dib, dia) { + if ml.IsChildDynamic(dia, dib) || ml.IsChildDynamic(dib, dia) { continue } if np >= nalc { @@ -777,7 +777,7 @@ func (wl *Model) ConfigBodyCollidePairs() { np = 1 } pt.SetShapeSizes(np, 2) - wl.BodyCollidePairs = pt + ml.BodyCollidePairs = pt BodyCollidePairs = pt // fmt.Println("body pairs over alloc", nalc - np, "total:", np) } @@ -786,8 +786,8 @@ func (wl *Model) ConfigBodyCollidePairs() { // SetMaxContacts computes [Params.MaxContacts] based on current list of // [BodyCollidePairs]. -func (wl *Model) SetMaxContacts() { - params := &wl.Params[0] +func (ml *Model) SetMaxContacts() { + params := &ml.Params[0] n := int32(0) for ci := range params.BodyCollidePairsN { @@ -815,7 +815,7 @@ func (wl *Model) SetMaxContacts() { } n = max(n, params.DynamicsN) params.ContactsMax = n - wl.BroadContacts.SetShapeSizes(int(n), int(BroadContactVarsN)) - wl.Contacts.SetShapeSizes(int(n), int(ContactVarsN)) + ml.BroadContacts.SetShapeSizes(int(n), int(BroadContactVarsN)) + ml.Contacts.SetShapeSizes(int(n), int(ContactVarsN)) } diff --git a/physics/control.go b/physics/control.go index 19426757..bf1823cf 100644 --- a/physics/control.go +++ b/physics/control.go @@ -16,19 +16,23 @@ const ( // Joint force and torque inputs JointControlForce JointControlVars = iota - // Joint target position settings: the stiffness parameter determines - // how strongly the target position target is enforced: - // 0 = not at all; larger = stronger (e.g., 1000 or higher). + // JointTargetPos is the position target value, where 0 is the initial + // position. For angular joints, this is in radians. + JointTargetPos + + // JointTargetStiff determines how strongly the target position + // is enforced: 0 = not at all; larger = stronger (e.g., 1000 or higher). // Set to 0 to allow the joint to be fully flexible. JointTargetStiff - JointTargetPos - // Joint target velocity settings: the damping parameter determines - // how strongly the target velocity target is enforced: + // JointTargetVel is the velocity target value. For example, 0 + // effectively damps joint movement in proportion to Damp parameter. + JointTargetVel + + // JointTargetDamp determines how strongly the target velocity is enforced: // 0 = not at all; larger = stronger (e.g., 1 is reasonable). // Set to 0 to allow the joint to be fully flexible. JointTargetDamp - JointTargetVel ) // SetJointControl sets the control for given joint, dof and parameter diff --git a/physics/control.goal b/physics/control.goal index 97920d11..cee48a46 100644 --- a/physics/control.goal +++ b/physics/control.goal @@ -14,19 +14,23 @@ const ( // Joint force and torque inputs JointControlForce JointControlVars = iota - // Joint target position settings: the stiffness parameter determines - // how strongly the target position target is enforced: - // 0 = not at all; larger = stronger (e.g., 1000 or higher). + // JointTargetPos is the position target value, where 0 is the initial + // position. For angular joints, this is in radians. + JointTargetPos + + // JointTargetStiff determines how strongly the target position + // is enforced: 0 = not at all; larger = stronger (e.g., 1000 or higher). // Set to 0 to allow the joint to be fully flexible. JointTargetStiff - JointTargetPos - // Joint target velocity settings: the damping parameter determines - // how strongly the target velocity target is enforced: + // JointTargetVel is the velocity target value. For example, 0 + // effectively damps joint movement in proportion to Damp parameter. + JointTargetVel + + // JointTargetDamp determines how strongly the target velocity is enforced: // 0 = not at all; larger = stronger (e.g., 1 is reasonable). // Set to 0 to allow the joint to be fully flexible. JointTargetDamp - JointTargetVel ) // SetJointControl sets the control for given joint, dof and parameter diff --git a/physics/enumgen.go b/physics/enumgen.go index bbfd2c9f..acc74776 100644 --- a/physics/enumgen.go +++ b/physics/enumgen.go @@ -6,20 +6,20 @@ import ( "cogentcore.org/core/enums" ) -var _BodyVarsValues = []BodyVars{0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43} +var _BodyVarsValues = []BodyVars{0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42} // BodyVarsN is the highest valid value for type BodyVars, plus one. // //gosl:start -const BodyVarsN BodyVars = 44 +const BodyVarsN BodyVars = 43 //gosl:end -var _BodyVarsValueMap = map[string]BodyVars{`BodyShape`: 0, `BodyDynamic`: 1, `BodyWorld`: 2, `BodyGroup`: 3, `BodyObject`: 4, `BodyHSizeX`: 5, `BodyHSizeY`: 6, `BodyHSizeZ`: 7, `BodyThick`: 8, `BodyMass`: 9, `BodyInvMass`: 10, `BodyBounce`: 11, `BodyFriction`: 12, `BodyFrictionTortion`: 13, `BodyFrictionRolling`: 14, `BodyPosX`: 15, `BodyPosY`: 16, `BodyPosZ`: 17, `BodyQuatX`: 18, `BodyQuatY`: 19, `BodyQuatZ`: 20, `BodyQuatW`: 21, `BodyComX`: 22, `BodyComY`: 23, `BodyComZ`: 24, `BodyInertiaXX`: 25, `BodyInertiaYX`: 26, `BodyInertiaZX`: 27, `BodyInertiaXY`: 28, `BodyInertiaYY`: 29, `BodyInertiaZY`: 30, `BodyInertiaXZ`: 31, `BodyInertiaYZ`: 32, `BodyInertiaZZ`: 33, `BodyInvInertiaXX`: 34, `BodyInvInertiaYX`: 35, `BodyInvInertiaZX`: 36, `BodyInvInertiaXY`: 37, `BodyInvInertiaYY`: 38, `BodyInvInertiaZY`: 39, `BodyInvInertiaXZ`: 40, `BodyInvInertiaYZ`: 41, `BodyInvInertiaZZ`: 42, `BodyRadius`: 43} +var _BodyVarsValueMap = map[string]BodyVars{`BodyShape`: 0, `BodyDynamic`: 1, `BodyWorld`: 2, `BodyGroup`: 3, `BodyHSizeX`: 4, `BodyHSizeY`: 5, `BodyHSizeZ`: 6, `BodyThick`: 7, `BodyMass`: 8, `BodyInvMass`: 9, `BodyBounce`: 10, `BodyFriction`: 11, `BodyFrictionTortion`: 12, `BodyFrictionRolling`: 13, `BodyPosX`: 14, `BodyPosY`: 15, `BodyPosZ`: 16, `BodyQuatX`: 17, `BodyQuatY`: 18, `BodyQuatZ`: 19, `BodyQuatW`: 20, `BodyComX`: 21, `BodyComY`: 22, `BodyComZ`: 23, `BodyInertiaXX`: 24, `BodyInertiaYX`: 25, `BodyInertiaZX`: 26, `BodyInertiaXY`: 27, `BodyInertiaYY`: 28, `BodyInertiaZY`: 29, `BodyInertiaXZ`: 30, `BodyInertiaYZ`: 31, `BodyInertiaZZ`: 32, `BodyInvInertiaXX`: 33, `BodyInvInertiaYX`: 34, `BodyInvInertiaZX`: 35, `BodyInvInertiaXY`: 36, `BodyInvInertiaYY`: 37, `BodyInvInertiaZY`: 38, `BodyInvInertiaXZ`: 39, `BodyInvInertiaYZ`: 40, `BodyInvInertiaZZ`: 41, `BodyRadius`: 42} -var _BodyVarsDescMap = map[BodyVars]string{0: `BodyShape is the shape type of the object, as a Shapes type.`, 1: `BodyDynamic is the index into Dynamics for this body, which is -1 for static bodies. Use this to get current Pos and Quat values for a dynamic body.`, 2: `BodyWorld partitions bodies into different worlds for collision detection: Global bodies = -1 can collide with everything; otherwise only items within the same world collide. NewBody uses [World.CurrentWorld] to initialize.`, 3: `BodyGroup partitions bodies within worlds into different groups for collision detection. 0 does not collide with anything. Negative numbers are global within a world, except they don't collide amongst themselves (all non-dynamic bodies should go in -1 because they don't collide amongst each-other, but do potentially collide with dynamics). Positive numbers only collide amongst themselves, and with negative groups, but not other positive groups. To avoid unwanted collisions, put bodies into separate groups. There is an automatic constraint that the two objects within a single joint do not collide with each other, so this does not need to be handled here.`, 4: `BodyObject identifies bodies that all belong to the same overall physical object (e.g., a robot or simulated animal). It has no implications for collision or physics simulation. Instead, it is used for external manipulation on entire objects at a time, e.g. via PositionObject. When using world replicas, the object indexes are copied, so objects are indexed by world and object id.`, 5: `BodyHSize is the half-size (e.g., radius) of the body. Values depend on shape type: X is generally radius, Y is half-height.`, 6: ``, 7: ``, 8: `BodyThick is the thickness of the body, as a hollow shape. If 0, then it is a solid shape (default).`, 9: `BodyMass is the mass of the object.`, 10: `BodyInvMass is 1/mass of the object or 0 if no mass.`, 11: `BodyBounce specifies the COR or coefficient of restitution (0..1), which determines how elastic the collision is, i.e., final velocity / initial velocity.`, 12: `BodyFriction is the standard coefficient for linear friction (mu).`, 13: `BodyFrictionTortion is resistance to spinning at the contact point.`, 14: `BodyFrictionRolling is resistance to rolling motion at contact.`, 15: `3D position of body (structural center).`, 16: ``, 17: ``, 18: `Quaternion rotation of body.`, 19: ``, 20: ``, 21: ``, 22: `Relative center-of-mass offset from 3D position of body.`, 23: ``, 24: ``, 25: `Inertia 3x3 matrix (column matrix organization, r,c labels).`, 26: ``, 27: ``, 28: ``, 29: ``, 30: ``, 31: ``, 32: ``, 33: ``, 34: `InvInertia inverse inertia 3x3 matrix (column matrix organization, r,c labels).`, 35: ``, 36: ``, 37: ``, 38: ``, 39: ``, 40: ``, 41: ``, 42: ``, 43: `radius for broadphase collision`} +var _BodyVarsDescMap = map[BodyVars]string{0: `BodyShape is the shape type of the object, as a Shapes type.`, 1: `BodyDynamic is the index into Dynamics for this body, which is -1 for static bodies. Use this to get current Pos and Quat values for a dynamic body.`, 2: `BodyWorld partitions bodies into different worlds for collision detection: Global bodies = -1 can collide with everything; otherwise only items within the same world collide. NewBody uses [World.CurrentWorld] to initialize.`, 3: `BodyGroup partitions bodies within worlds into different groups for collision detection. 0 does not collide with anything. Negative numbers are global within a world, except they don't collide amongst themselves (all non-dynamic bodies should go in -1 because they don't collide amongst each-other, but do potentially collide with dynamics). Positive numbers only collide amongst themselves, and with negative groups, but not other positive groups. To avoid unwanted collisions, put bodies into separate groups. There is an automatic constraint that the two objects within a single joint do not collide with each other, so this does not need to be handled here.`, 4: `BodyHSize is the half-size (e.g., radius) of the body. Values depend on shape type: X is generally radius, Y is half-height.`, 5: ``, 6: ``, 7: `BodyThick is the thickness of the body, as a hollow shape. If 0, then it is a solid shape (default).`, 8: `BodyMass is the mass of the object.`, 9: `BodyInvMass is 1/mass of the object or 0 if no mass.`, 10: `BodyBounce specifies the COR or coefficient of restitution (0..1), which determines how elastic the collision is, i.e., final velocity / initial velocity.`, 11: `BodyFriction is the standard coefficient for linear friction (mu).`, 12: `BodyFrictionTortion is resistance to spinning at the contact point.`, 13: `BodyFrictionRolling is resistance to rolling motion at contact.`, 14: `3D position of body (structural center).`, 15: ``, 16: ``, 17: `Quaternion rotation of body.`, 18: ``, 19: ``, 20: ``, 21: `Relative center-of-mass offset from 3D position of body.`, 22: ``, 23: ``, 24: `Inertia 3x3 matrix (column matrix organization, r,c labels).`, 25: ``, 26: ``, 27: ``, 28: ``, 29: ``, 30: ``, 31: ``, 32: ``, 33: `InvInertia inverse inertia 3x3 matrix (column matrix organization, r,c labels).`, 34: ``, 35: ``, 36: ``, 37: ``, 38: ``, 39: ``, 40: ``, 41: ``, 42: `radius for broadphase collision`} -var _BodyVarsMap = map[BodyVars]string{0: `BodyShape`, 1: `BodyDynamic`, 2: `BodyWorld`, 3: `BodyGroup`, 4: `BodyObject`, 5: `BodyHSizeX`, 6: `BodyHSizeY`, 7: `BodyHSizeZ`, 8: `BodyThick`, 9: `BodyMass`, 10: `BodyInvMass`, 11: `BodyBounce`, 12: `BodyFriction`, 13: `BodyFrictionTortion`, 14: `BodyFrictionRolling`, 15: `BodyPosX`, 16: `BodyPosY`, 17: `BodyPosZ`, 18: `BodyQuatX`, 19: `BodyQuatY`, 20: `BodyQuatZ`, 21: `BodyQuatW`, 22: `BodyComX`, 23: `BodyComY`, 24: `BodyComZ`, 25: `BodyInertiaXX`, 26: `BodyInertiaYX`, 27: `BodyInertiaZX`, 28: `BodyInertiaXY`, 29: `BodyInertiaYY`, 30: `BodyInertiaZY`, 31: `BodyInertiaXZ`, 32: `BodyInertiaYZ`, 33: `BodyInertiaZZ`, 34: `BodyInvInertiaXX`, 35: `BodyInvInertiaYX`, 36: `BodyInvInertiaZX`, 37: `BodyInvInertiaXY`, 38: `BodyInvInertiaYY`, 39: `BodyInvInertiaZY`, 40: `BodyInvInertiaXZ`, 41: `BodyInvInertiaYZ`, 42: `BodyInvInertiaZZ`, 43: `BodyRadius`} +var _BodyVarsMap = map[BodyVars]string{0: `BodyShape`, 1: `BodyDynamic`, 2: `BodyWorld`, 3: `BodyGroup`, 4: `BodyHSizeX`, 5: `BodyHSizeY`, 6: `BodyHSizeZ`, 7: `BodyThick`, 8: `BodyMass`, 9: `BodyInvMass`, 10: `BodyBounce`, 11: `BodyFriction`, 12: `BodyFrictionTortion`, 13: `BodyFrictionRolling`, 14: `BodyPosX`, 15: `BodyPosY`, 16: `BodyPosZ`, 17: `BodyQuatX`, 18: `BodyQuatY`, 19: `BodyQuatZ`, 20: `BodyQuatW`, 21: `BodyComX`, 22: `BodyComY`, 23: `BodyComZ`, 24: `BodyInertiaXX`, 25: `BodyInertiaYX`, 26: `BodyInertiaZX`, 27: `BodyInertiaXY`, 28: `BodyInertiaYY`, 29: `BodyInertiaZY`, 30: `BodyInertiaXZ`, 31: `BodyInertiaYZ`, 32: `BodyInertiaZZ`, 33: `BodyInvInertiaXX`, 34: `BodyInvInertiaYX`, 35: `BodyInvInertiaZX`, 36: `BodyInvInertiaXY`, 37: `BodyInvInertiaYY`, 38: `BodyInvInertiaZY`, 39: `BodyInvInertiaXZ`, 40: `BodyInvInertiaYZ`, 41: `BodyInvInertiaZZ`, 42: `BodyRadius`} // String returns the string representation of this BodyVars value. func (i BodyVars) String() string { return enums.String(i, _BodyVarsMap) } @@ -107,11 +107,11 @@ const JointControlVarsN JointControlVars = 5 //gosl:end -var _JointControlVarsValueMap = map[string]JointControlVars{`JointControlForce`: 0, `JointTargetStiff`: 1, `JointTargetPos`: 2, `JointTargetDamp`: 3, `JointTargetVel`: 4} +var _JointControlVarsValueMap = map[string]JointControlVars{`JointControlForce`: 0, `JointTargetPos`: 1, `JointTargetStiff`: 2, `JointTargetVel`: 3, `JointTargetDamp`: 4} var _JointControlVarsDescMap = map[JointControlVars]string{0: `Joint force and torque inputs`, 1: `Joint target position settings: the stiffness parameter determines how strongly the target position target is enforced: 0 = not at all; larger = stronger (e.g., 1000 or higher). Set to 0 to allow the joint to be fully flexible.`, 2: ``, 3: `Joint target velocity settings: the damping parameter determines how strongly the target velocity target is enforced: 0 = not at all; larger = stronger (e.g., 1 is reasonable). Set to 0 to allow the joint to be fully flexible.`, 4: ``} -var _JointControlVarsMap = map[JointControlVars]string{0: `JointControlForce`, 1: `JointTargetStiff`, 2: `JointTargetPos`, 3: `JointTargetDamp`, 4: `JointTargetVel`} +var _JointControlVarsMap = map[JointControlVars]string{0: `JointControlForce`, 1: `JointTargetPos`, 2: `JointTargetStiff`, 3: `JointTargetVel`, 4: `JointTargetDamp`} // String returns the string representation of this JointControlVars value. func (i JointControlVars) String() string { return enums.String(i, _JointControlVarsMap) } diff --git a/physics/examples/balls/balls.go b/physics/examples/balls/balls.go index 4dd50d3d..cd8ea1ba 100644 --- a/physics/examples/balls/balls.go +++ b/physics/examples/balls/balls.go @@ -100,7 +100,7 @@ func main() { bl := sc.NewDynamic(ml, "ball", physics.Sphere, clr, bs.Mass, math32.Vec3(size, size, size), math32.Vec3(x, size+ht, z), rot) if !bs.Collide { - physics.SetBodyGroup(bl.Index, int32(i+1)) // only collide within same group + physics.SetBodyGroup(bl.BodyIndex, int32(i+1)) // only collide within same group } bl.SetBodyBounce(bs.Bounce) bl.SetBodyFriction(bs.Friction) diff --git a/physics/examples/pendula/pendula.go b/physics/examples/pendula/pendula.go index 3fc6d96e..c3c404c6 100644 --- a/physics/examples/pendula/pendula.go +++ b/physics/examples/pendula/pendula.go @@ -14,6 +14,7 @@ import ( "cogentcore.org/core/math32" _ "cogentcore.org/lab/gosl/slbool/slboolcore" // include to get gui views "cogentcore.org/lab/physics" + "cogentcore.org/lab/physics/builder" "cogentcore.org/lab/physics/phyxyz" ) @@ -81,9 +82,15 @@ func main() { ed.SetUserParams(ps) - var botJoint int32 + bld := builder.NewBuilder() + + var botJoint *builder.Joint ed.SetConfigFunc(func() { + bld.Reset() + wld := bld.NewWorld() + obj := wld.NewObject() + ml := ed.Model sc := ed.Scene rot := math32.NewQuat(0, 0, 0, 1) @@ -101,16 +108,16 @@ func main() { x = 0 y -= ps.HSize.Y } - pb := sc.NewDynamic(ml, "top", physics.Capsule, clr, ps.Mass, ps.HSize, math32.Vec3(x, y, 0), rleft) + pb := obj.NewDynamicSkin(sc, "top", physics.Capsule, clr, ps.Mass, ps.HSize, math32.Vec3(x, y, 0), rleft) if !ps.Collide { - pb.SetBodyGroup(1) + pb.SetGroup(1) } targ := math32.DegToRad(float32(ps.TargetDegFromVert)) - ji := pb.NewJointRevolute(ml, nil, math32.Vec3(0, stY, 0), math32.Vec3(0, ps.HSize.Y, 0), math32.Vec3(0, 0, 1)) - physics.SetJointTargetPos(ji, 0, targ, ps.Stiff) - physics.SetJointTargetVel(ji, 0, 0, ps.Damp) + jd := obj.NewJointRevolute(nil, pb, math32.Vec3(0, stY, 0), math32.Vec3(0, ps.HSize.Y, 0), math32.Vec3(0, 0, 1)) + jd.DoF(0).SetTargetPos(targ).SetTargetStiff(ps.Stiff). + SetTargetVel(0).SetTargetDamp(ps.Damp) for i := 1; i < ps.NPendula; i++ { clr := colors.Names[12+i%len(colors.Names)] @@ -120,24 +127,25 @@ func main() { y = stY + x x = 0 } - cb := sc.NewDynamic(ml, "child", physics.Capsule, clr, ps.Mass, ps.HSize, math32.Vec3(x, y, 0), rleft) + cb := obj.NewDynamicSkin(sc, "child", physics.Capsule, clr, ps.Mass, ps.HSize, math32.Vec3(x, y, 0), rleft) if !ps.Collide { - cb.SetBodyGroup(1 + i) + cb.SetGroup(1 + i) } - ji = cb.NewJointRevolute(ml, pb, math32.Vec3(0, -ps.HSize.Y, 0), math32.Vec3(0, ps.HSize.Y, 0), math32.Vec3(0, 0, 1)) - physics.SetJointTargetPos(ji, 0, targ, ps.Stiff) - physics.SetJointTargetVel(ji, 0, 0, ps.Damp) + jd = obj.NewJointRevolute(pb, cb, math32.Vec3(0, -ps.HSize.Y, 0), math32.Vec3(0, ps.HSize.Y, 0), math32.Vec3(0, 0, 1)) + jd.DoF(0).SetTargetPos(targ).SetTargetStiff(ps.Stiff). + SetTargetVel(0).SetTargetDamp(ps.Damp) pb = cb - botJoint = ji + botJoint = jd } + bld.Build(ml, sc) }) ed.SetControlFunc(func(timeStep int) { if timeStep >= ps.ForceOn && timeStep < ps.ForceOff { fmt.Println(timeStep, "\tforce on:", ps.Force) - physics.SetJointControlForce(botJoint, 0, ps.Force) + physics.SetJointControlForce(botJoint.JointIndex, 0, ps.Force) } else { - physics.SetJointControlForce(botJoint, 0, 0) + physics.SetJointControlForce(botJoint.JointIndex, 0, 0) } }) diff --git a/physics/model.go b/physics/model.go index 8614b5a5..551be302 100644 --- a/physics/model.go +++ b/physics/model.go @@ -36,11 +36,6 @@ type Model struct { // a specific world to view. ReplicasN int32 - // CurrentObject is the [BodyObject] value to use when creating new bodies. - // Generally just increment when starting a new object. When using world replicas, - // the object indexes are copied, so objects are indexed by world and object id. - CurrentObject int - // Bodies are the rigid body elements (dynamic and static), // specifying the constant, non-dynamic properties, // which is initial state for dynamics. @@ -128,30 +123,29 @@ func (ml *Model) Reset() { // NewBody adds a new body with given parameters. Returns the index. // Use this for Static elements; NewDynamic for dynamic elements. -func (ml *Model) NewBody(shape Shapes, size, pos math32.Vector3, rot math32.Quat) int32 { +func (ml *Model) NewBody(shape Shapes, hsize, pos math32.Vector3, rot math32.Quat) int32 { sizes := ml.Bodies.ShapeSizes() idx := int32(sizes[0]) ml.Bodies.SetShapeSizes(int(idx+1), int(BodyVarsN)) ml.Params[0].BodiesN = idx + 1 SetBodyShape(idx, shape) SetBodyDynamic(idx, -1) - SetBodyHSize(idx, size) + SetBodyHSize(idx, hsize) SetBodyPos(idx, pos) SetBodyQuat(idx, rot) SetBodyGroup(idx, -1) // assume static SetBodyWorld(idx, int32(ml.CurrentWorld)) - SetBodyObject(idx, int32(ml.CurrentObject)) - ml.SetMass(idx, shape, size, 0) // assume static + ml.SetMass(idx, shape, hsize, 0) // assume static return idx } // NewDynamic adds a new dynamic body with given parameters. Returns the index. // Shape cannot be [Plane]. -func (ml *Model) NewDynamic(shape Shapes, mass float32, size, pos math32.Vector3, rot math32.Quat) (bodyIdx, dynIdx int32) { +func (ml *Model) NewDynamic(shape Shapes, mass float32, hsize, pos math32.Vector3, rot math32.Quat) (bodyIdx, dynIdx int32) { if shape == Plane { panic("physics.NewDynamic: shape cannot be Plane") } - bodyIdx = ml.NewBody(shape, size, pos, rot) + bodyIdx = ml.NewBody(shape, hsize, pos, rot) sizes := ml.Dynamics.ShapeSizes() dynIdx = int32(sizes[0]) ml.Dynamics.SetShapeSizes(int(dynIdx+1), 2, int(DynamicVarsN)) @@ -159,7 +153,7 @@ func (ml *Model) NewDynamic(shape Shapes, mass float32, size, pos math32.Vector3 SetDynamicBody(dynIdx, bodyIdx) SetBodyDynamic(bodyIdx, dynIdx) SetBodyGroup(bodyIdx, 1) // dynamic - ml.SetMass(bodyIdx, shape, size, mass) + ml.SetMass(bodyIdx, shape, hsize, mass) return } diff --git a/physics/model.goal b/physics/model.goal index 9e45fbf6..964ba352 100644 --- a/physics/model.goal +++ b/physics/model.goal @@ -34,11 +34,6 @@ type Model struct { // a specific world to view. ReplicasN int32 - // CurrentObject is the [BodyObject] value to use when creating new bodies. - // Generally just increment when starting a new object. When using world replicas, - // the object indexes are copied, so objects are indexed by world and object id. - CurrentObject int - // Bodies are the rigid body elements (dynamic and static), // specifying the constant, non-dynamic properties, // which is initial state for dynamics. @@ -126,30 +121,29 @@ func (ml *Model) Reset() { // NewBody adds a new body with given parameters. Returns the index. // Use this for Static elements; NewDynamic for dynamic elements. -func (ml *Model) NewBody(shape Shapes, size, pos math32.Vector3, rot math32.Quat) int32 { +func (ml *Model) NewBody(shape Shapes, hsize, pos math32.Vector3, rot math32.Quat) int32 { sizes := ml.Bodies.ShapeSizes() idx := int32(sizes[0]) ml.Bodies.SetShapeSizes(int(idx+1), int(BodyVarsN)) ml.Params[0].BodiesN = idx + 1 SetBodyShape(idx, shape) SetBodyDynamic(idx, -1) - SetBodyHSize(idx, size) + SetBodyHSize(idx, hsize) SetBodyPos(idx, pos) SetBodyQuat(idx, rot) SetBodyGroup(idx, -1) // assume static SetBodyWorld(idx, int32(ml.CurrentWorld)) - SetBodyObject(idx, int32(ml.CurrentObject)) - ml.SetMass(idx, shape, size, 0) // assume static + ml.SetMass(idx, shape, hsize, 0) // assume static return idx } // NewDynamic adds a new dynamic body with given parameters. Returns the index. // Shape cannot be [Plane]. -func (ml *Model) NewDynamic(shape Shapes, mass float32, size, pos math32.Vector3, rot math32.Quat) (bodyIdx, dynIdx int32) { +func (ml *Model) NewDynamic(shape Shapes, mass float32, hsize, pos math32.Vector3, rot math32.Quat) (bodyIdx, dynIdx int32) { if shape == Plane { panic("physics.NewDynamic: shape cannot be Plane") } - bodyIdx = ml.NewBody(shape, size, pos, rot) + bodyIdx = ml.NewBody(shape, hsize, pos, rot) sizes := ml.Dynamics.ShapeSizes() dynIdx = int32(sizes[0]) ml.Dynamics.SetShapeSizes(int(dynIdx+1), 2, int(DynamicVarsN)) @@ -157,7 +151,7 @@ func (ml *Model) NewDynamic(shape Shapes, mass float32, size, pos math32.Vector3 SetDynamicBody(dynIdx, bodyIdx) SetBodyDynamic(bodyIdx, dynIdx) SetBodyGroup(bodyIdx, 1) // dynamic - ml.SetMass(bodyIdx, shape, size, mass) + ml.SetMass(bodyIdx, shape, hsize, mass) return } diff --git a/physics/phyxyz/editor.go b/physics/phyxyz/editor.go index c2fbb6f9..afb85e92 100644 --- a/physics/phyxyz/editor.go +++ b/physics/phyxyz/editor.go @@ -145,7 +145,7 @@ func (pe *Editor) Init() { }) } -// ConfigModel configures the physics world. +// ConfigModel configures the physics model. func (pe *Editor) ConfigModel() { if pe.isRunning { core.MessageSnackbar(pe, "Simulation is still running...") diff --git a/physics/phyxyz/world.go b/physics/phyxyz/scene.go similarity index 73% rename from physics/phyxyz/world.go rename to physics/phyxyz/scene.go index 2995bae9..23909e3c 100644 --- a/physics/phyxyz/world.go +++ b/physics/phyxyz/scene.go @@ -11,6 +11,7 @@ package phyxyz import ( "image" + "cogentcore.org/core/math32" "cogentcore.org/core/tree" "cogentcore.org/core/xyz" "cogentcore.org/lab/physics" @@ -27,8 +28,8 @@ type Scene struct { // Root is the root Group node in the Scene under which the world is rendered. Root *xyz.Group - // Views are the view elements for each body in [physics.Model]. - Views []*View + // Skins are the view elements for each body in [physics.Model]. + Skins []*Skin } // NewScene returns a new Scene for visualizing a [physics.Model]. @@ -40,25 +41,25 @@ func NewScene(sc *xyz.Scene) *Scene { return xysc } -// Init configures the visual world based on Views, +// Init configures the visual world based on Skins, // and calls Config on [physics.Model]. -// Call this _once_ after making all the new Views and Bodies. +// Call this _once_ after making all the new Skins and Bodies. // (will return if already called). -func (sc *Scene) Init(ph *physics.Model) { - ph.Config() +func (sc *Scene) Init(ml *physics.Model) { + ml.Config() if len(sc.Root.Makers.Normal) > 0 { return } sc.Root.Maker(func(p *tree.Plan) { - for _, vw := range sc.Views { - vw.Add(p) + for _, sk := range sc.Skins { + sk.Add(p) } }) } // Reset resets any existing views, starting fresh for a new configuration. func (sc *Scene) Reset() { - sc.Views = nil + sc.Skins = nil if sc.Scene != nil { sc.Scene.Update() } @@ -76,15 +77,15 @@ func (sc *Scene) Update() { // UpdateFromPhysics updates the Scene from currently active // physics state (use physics.Model.SetAsCurrent()). func (sc *Scene) UpdateFromPhysics() { - for _, vw := range sc.Views { - vw.UpdateFromPhysics() + for _, sk := range sc.Skins { + sk.UpdateFromPhysics() } } -// RenderFromView does an offscreen render using given [View] +// RenderFromNode does an offscreen render using given [Skin] // for the camera position and orientation, returning the render image. // Current scene camera is saved and restored. -func (sc *Scene) RenderFromNode(vw *View, cam *Camera) image.Image { +func (sc *Scene) RenderFromNode(sk *Skin, cam *Camera) image.Image { xysc := sc.Scene camnm := "physics-view-rendernode-save" xysc.SaveCamera(camnm) @@ -96,8 +97,8 @@ func (sc *Scene) RenderFromNode(vw *View, cam *Camera) image.Image { xysc.Camera.FOV = cam.FOV xysc.Camera.Near = cam.Near xysc.Camera.Far = cam.Far - xysc.Camera.Pose.Pos = vw.Pos - xysc.Camera.Pose.Quat = vw.Quat + xysc.Camera.Pose.Pos = sk.Pos + xysc.Camera.Pose.Quat = sk.Quat xysc.Camera.Pose.Scale.Set(1, 1, 1) xysc.UseAltFrame(cam.Size) @@ -108,3 +109,9 @@ func (sc *Scene) RenderFromNode(vw *View, cam *Camera) image.Image { // func (vw *Scene) DepthImage() ([]float32, error) { // return vw.Scene.DepthImage() // } + +func (sc *Scene) NewSkin(shape physics.Shapes, name, clr string, hsize math32.Vector3, pos math32.Vector3, rot math32.Quat) *Skin { + sk := &Skin{Name: name, Shape: shape, Color: clr, HSize: hsize, DynamicIndex: -1, Pos: pos, Quat: rot} + sc.Skins = append(sc.Skins, sk) + return sk +} diff --git a/physics/phyxyz/view.go b/physics/phyxyz/skin.go similarity index 52% rename from physics/phyxyz/view.go rename to physics/phyxyz/skin.go index 41953c42..7d1602c9 100644 --- a/physics/phyxyz/view.go +++ b/physics/phyxyz/skin.go @@ -15,9 +15,9 @@ import ( "cogentcore.org/lab/physics" ) -// View has visualization functions for physics elements. -type View struct { - // Name is a name for element (index always appended). +// Skin has visualization functions for physics elements. +type Skin struct { //types:add -setters + // Name is a name for element (index always appended, so it is unique). Name string // Shape is the physical shape of the element. @@ -26,8 +26,10 @@ type View struct { // Color is the color of the element. Color string - // Size is the size (per shape). - Size math32.Vector3 + // HSize is the half-size (e.g., radius) of the body. + // Values depend on shape type: X is generally radius, + // Y is half-height. + HSize math32.Vector3 // Pos is the position. Pos math32.Vector3 @@ -35,194 +37,194 @@ type View struct { // Quat is the rotation as a quaternion. Quat math32.Quat - // NewView is a function that returns a new [xyz.Node] + // NewSkin is a function that returns a new [xyz.Node] // to represent this element. If nil, uses appropriate defaults. - NewView func() tree.Node + NewSkin func() tree.Node - // InitView is a function that initializes a new [xyz.Node] + // InitSkin is a function that initializes a new [xyz.Node] // that represents this element. If nil, uses appropriate defaults. - InitView func(sld *xyz.Solid) + InitSkin func(sld *xyz.Solid) - // Index is the index of the element in a list. - Index int32 + // BodyIndex is the index of the body in [physics.Bodies] + BodyIndex int32 - // DynamicIndex is the index of a dynamic element (-1 if not dynamic). + // DynamicIndex is the index in [physics.Dynamics] (-1 if not dynamic). DynamicIndex int32 } // NewBody adds a new body with given parameters. -// Returns the View which can then be further customized. +// Returns the Skin which can then be further customized. // Use this for Static elements; NewDynamic for dynamic elements. -func (wr *Scene) NewBody(ph *physics.Model, name string, shape physics.Shapes, clr string, size, pos math32.Vector3, rot math32.Quat) *View { - idx := ph.NewBody(shape, size, pos, rot) - vw := &View{Name: name, Index: idx, DynamicIndex: -1, Shape: shape, Color: clr, Size: size, Pos: pos, Quat: rot} - wr.Views = append(wr.Views, vw) - return vw +func (sc *Scene) NewBody(ml *physics.Model, name string, shape physics.Shapes, clr string, hsize, pos math32.Vector3, rot math32.Quat) *Skin { + idx := ml.NewBody(shape, hsize, pos, rot) + sk := sc.NewSkin(shape, name, clr, hsize, pos, rot) + sk.SetBodyIndex(idx) + return sk } // NewDynamic adds a new dynamic body with given parameters. -// Returns the View which can then be further customized. -func (wr *Scene) NewDynamic(ph *physics.Model, name string, shape physics.Shapes, clr string, mass float32, size, pos math32.Vector3, rot math32.Quat) *View { - idx, dyIdx := ph.NewDynamic(shape, mass, size, pos, rot) - vw := &View{Name: name, Index: idx, DynamicIndex: dyIdx, Shape: shape, Color: clr, Size: size, Pos: pos, Quat: rot} - wr.Views = append(wr.Views, vw) - return vw +// Returns the Skin which can then be further customized. +func (sc *Scene) NewDynamic(ml *physics.Model, name string, shape physics.Shapes, clr string, mass float32, hsize, pos math32.Vector3, rot math32.Quat) *Skin { + idx, dyIdx := ml.NewDynamic(shape, mass, hsize, pos, rot) + sk := sc.NewSkin(shape, name, clr, hsize, pos, rot) + sk.SetBodyIndex(idx).SetDynamicIndex(dyIdx) + return sk } -// UpdateFromPhysics updates the View from physics state. -func (vw *View) UpdateFromPhysics() { +// UpdateFromPhysics updates the Skin from physics state. +func (sk *Skin) UpdateFromPhysics() { params := physics.GetParams(0) - if vw.DynamicIndex >= 0 { - ix := int32(vw.DynamicIndex) - vw.Pos = physics.DynamicPos(ix, params.Cur) - vw.Quat = physics.DynamicQuat(ix, params.Cur) + if sk.DynamicIndex >= 0 { + ix := int32(sk.DynamicIndex) + sk.Pos = physics.DynamicPos(ix, params.Cur) + sk.Quat = physics.DynamicQuat(ix, params.Cur) } else { - ix := int32(vw.Index) - vw.Pos = physics.BodyPos(ix) - vw.Quat = physics.BodyQuat(ix) + ix := int32(sk.BodyIndex) + sk.Pos = physics.BodyPos(ix) + sk.Quat = physics.BodyQuat(ix) } } -// UpdatePose updates the xyz node pose from view. -func (vw *View) UpdatePose(sld *xyz.Solid) { - sld.Pose.Pos = vw.Pos - sld.Pose.Quat = vw.Quat +// UpdatePose updates the xyz node pose from skin. +func (sk *Skin) UpdatePose(sld *xyz.Solid) { + sld.Pose.Pos = sk.Pos + sld.Pose.Quat = sk.Quat } -// UpdateColor updates the xyz node color from view. -func (vw *View) UpdateColor(clr string, sld *xyz.Solid) { +// UpdateColor updates the xyz node color from skin. +func (sk *Skin) UpdateColor(clr string, sld *xyz.Solid) { if clr == "" { return } sld.Material.Color = errors.Log1(colors.FromString(clr)) } -// Add adds given physics node to the [tree.Plan], using NewView +// Add adds given physics node to the [tree.Plan], using NewSkin // function on the node, or default. -func (vw *View) Add(p *tree.Plan) { - nm := vw.Name + strconv.Itoa(int(vw.Index)) - newFunc := vw.NewView +func (sk *Skin) Add(p *tree.Plan) { + nm := sk.Name + strconv.Itoa(int(sk.BodyIndex)) + newFunc := sk.NewSkin if newFunc == nil { newFunc = func() tree.Node { return any(tree.New[xyz.Solid]()).(tree.Node) } } - p.Add(nm, newFunc, func(n tree.Node) { vw.Init(n.(*xyz.Solid)) }) + p.Add(nm, newFunc, func(n tree.Node) { sk.Init(n.(*xyz.Solid)) }) } -// Init initializes xyz node using InitView function or default. -func (vw *View) Init(sld *xyz.Solid) { - initFunc := vw.InitView +// Init initializes xyz node using InitSkin function or default. +func (sk *Skin) Init(sld *xyz.Solid) { + initFunc := sk.InitSkin if initFunc != nil { initFunc(sld) return } - switch vw.Shape { + switch sk.Shape { case physics.Plane: - vw.PlaneInit(sld) + sk.PlaneInit(sld) case physics.Sphere: - vw.SphereInit(sld) + sk.SphereInit(sld) case physics.Capsule: - vw.CapsuleInit(sld) + sk.CapsuleInit(sld) case physics.Cylinder: - vw.CylinderInit(sld) + sk.CylinderInit(sld) case physics.Box: - vw.BoxInit(sld) + sk.BoxInit(sld) } } -// BoxInit is the default InitView function for [physics.Box]. +// BoxInit is the default InitSkin function for [physics.Box]. // Only updates Pose in Updater: if node will change size or color, // add updaters for that. -func (vw *View) BoxInit(sld *xyz.Solid) { +func (sk *Skin) BoxInit(sld *xyz.Solid) { mnm := "physics.Box" if ms, _ := sld.Scene.MeshByName(mnm); ms == nil { xyz.NewBox(sld.Scene, mnm, 1, 1, 1) } sld.SetMeshName(mnm) - sld.Pose.Scale = vw.Size.MulScalar(2) - vw.UpdateColor(vw.Color, sld) + sld.Pose.Scale = sk.HSize.MulScalar(2) + sk.UpdateColor(sk.Color, sld) sld.Updater(func() { - vw.UpdatePose(sld) + sk.UpdatePose(sld) }) } -// PlaneInit is the default InitView function for [physics.Plane]. +// PlaneInit is the default InitSkin function for [physics.Plane]. // Only updates Pose in Updater: if node will change size or color, // add updaters for that. -func (vw *View) PlaneInit(sld *xyz.Solid) { +func (sk *Skin) PlaneInit(sld *xyz.Solid) { mnm := "physics.Plane" if ms, _ := sld.Scene.MeshByName(mnm); ms == nil { pl := xyz.NewPlane(sld.Scene, mnm, 1, 1) pl.Segs.Set(4, 4) } sld.SetMeshName(mnm) - if vw.Size.X == 0 { + if sk.HSize.X == 0 { inf := float32(1e3) sld.Pose.Scale = math32.Vec3(inf, 1, inf) } else { - sld.Pose.Scale = vw.Size.MulScalar(2) + sld.Pose.Scale = sk.HSize.MulScalar(2) } - vw.UpdateColor(vw.Color, sld) + sk.UpdateColor(sk.Color, sld) sld.Updater(func() { - vw.UpdatePose(sld) + sk.UpdatePose(sld) }) } -// CylinderInit is the default InitView function for [physics.Cylinder]. +// CylinderInit is the default InitSkin function for [physics.Cylinder]. // Only updates Pose in Updater: if node will change size or color, // add updaters for that. -func (vw *View) CylinderInit(sld *xyz.Solid) { +func (sk *Skin) CylinderInit(sld *xyz.Solid) { mnm := "physics.Cylinder" if ms, _ := sld.Scene.MeshByName(mnm); ms == nil { xyz.NewCylinder(sld.Scene, mnm, 1, 1, 32, 1, true, true) } sld.SetMeshName(mnm) - sld.Pose.Scale = vw.Size + sld.Pose.Scale = sk.HSize sld.Pose.Scale.Y *= 2 - vw.UpdateColor(vw.Color, sld) + sk.UpdateColor(sk.Color, sld) sld.Updater(func() { - vw.UpdatePose(sld) + sk.UpdatePose(sld) }) } -// CapsuleInit is the default InitView function for [physics.Capsule]. +// CapsuleInit is the default InitSkin function for [physics.Capsule]. // Only updates Pose in Updater: if node will change size or color, // add updaters for that. -func (vw *View) CapsuleInit(sld *xyz.Solid) { +func (sk *Skin) CapsuleInit(sld *xyz.Solid) { mnm := "physics.Capsule" if ms, _ := sld.Scene.MeshByName(mnm); ms == nil { ms = xyz.NewCapsule(sld.Scene, mnm, 1, .2, 32, 1) } sld.SetMeshName(mnm) - sld.Pose.Scale.Set(vw.Size.X/.2, 2*(vw.Size.Y/1.4), vw.Size.Z/.2) - vw.UpdateColor(vw.Color, sld) + sld.Pose.Scale.Set(sk.HSize.X/.2, 2*(sk.HSize.Y/1.4), sk.HSize.Z/.2) + sk.UpdateColor(sk.Color, sld) sld.Updater(func() { - vw.UpdatePose(sld) + sk.UpdatePose(sld) }) } -// SphereInit is the default InitView function for [physics.Sphere]. +// SphereInit is the default InitSkin function for [physics.Sphere]. // Only updates Pose in Updater: if node will change size or color, // add updaters for that. -func (vw *View) SphereInit(sld *xyz.Solid) { +func (sk *Skin) SphereInit(sld *xyz.Solid) { mnm := "physics.Sphere" if ms, _ := sld.Scene.MeshByName(mnm); ms == nil { ms = xyz.NewSphere(sld.Scene, mnm, 1, 32) } sld.SetMeshName(mnm) - sld.Pose.Scale.SetScalar(vw.Size.X) - vw.UpdateColor(vw.Color, sld) + sld.Pose.Scale.SetScalar(sk.HSize.X) + sk.UpdateColor(sk.Color, sld) sld.Updater(func() { - vw.UpdatePose(sld) + sk.UpdatePose(sld) }) } // SetBodyWorld partitions bodies into different worlds for // collision detection: Global bodies = -1 can collide with // everything; otherwise only items within the same world collide. -func (vw *View) SetBodyWorld(world int) { - physics.SetBodyWorld(vw.Index, int32(world)) +func (sk *Skin) SetBodyWorld(world int) { + physics.SetBodyWorld(sk.BodyIndex, int32(world)) } // SetBodyGroup partitions bodies within worlds into different groups @@ -237,120 +239,120 @@ func (vw *View) SetBodyWorld(world int) { // bodies. There is an automatic constraint that the two objects // within a single joint do not collide with each other, so this // does not need to be handled here. -func (vw *View) SetBodyGroup(group int) { - physics.SetBodyGroup(vw.Index, int32(group)) +func (sk *Skin) SetBodyGroup(group int) { + physics.SetBodyGroup(sk.BodyIndex, int32(group)) } // SetBodyBounce specifies the COR or coefficient of restitution (0..1), // which determines how elastic the collision is, // i.e., final velocity / initial velocity. -func (vw *View) SetBodyBounce(val float32) { - physics.Bodies.Set(val, int(vw.Index), int(physics.BodyBounce)) +func (sk *Skin) SetBodyBounce(val float32) { + physics.Bodies.Set(val, int(sk.BodyIndex), int(physics.BodyBounce)) } // SetBodyFriction is the standard coefficient for linear friction (mu). -func (vw *View) SetBodyFriction(val float32) { - physics.Bodies.Set(val, int(vw.Index), int(physics.BodyFriction)) +func (sk *Skin) SetBodyFriction(val float32) { + physics.Bodies.Set(val, int(sk.BodyIndex), int(physics.BodyFriction)) } // SetBodyFrictionTortion is resistance to spinning at the contact point. -func (vw *View) SetBodyFrictionTortion(val float32) { - physics.Bodies.Set(val, int(vw.Index), int(physics.BodyFrictionTortion)) +func (sk *Skin) SetBodyFrictionTortion(val float32) { + physics.Bodies.Set(val, int(sk.BodyIndex), int(physics.BodyFrictionTortion)) } // SetBodyFrictionRolling is resistance to rolling motion at contact. -func (vw *View) SetBodyFrictionRolling(val float32) { - physics.Bodies.Set(val, int(vw.Index), int(physics.BodyFrictionRolling)) +func (sk *Skin) SetBodyFrictionRolling(val float32) { + physics.Bodies.Set(val, int(sk.BodyIndex), int(physics.BodyFrictionRolling)) } -// NewJointFixed adds a new Fixed joint as a child of given parent. +// NewJointFixed adds a new Fixed joint between given parent and child. // Use nil for parent to add a world-anchored joint. // ppos, cpos are the relative positions from the parent, child. // These are for the non-rotated body (i.e., body rotation is applied // to these positions as well). // Sets relative rotation matricies to identity by default. -func (vw *View) NewJointFixed(ph *physics.Model, parent *View, ppos, cpos math32.Vector3) int32 { +func (sc *Scene) NewJointFixed(ml *physics.Model, parent, child *Skin, ppos, cpos math32.Vector3) int32 { pidx := int32(-1) if parent != nil { pidx = parent.DynamicIndex } - return ph.NewJointFixed(pidx, vw.DynamicIndex, ppos, cpos) + return ml.NewJointFixed(pidx, child.DynamicIndex, ppos, cpos) } -// NewJointPrismatic adds a new Prismatic (slider) joint as a child -// of given parent. Use nil for parent to add a world-anchored joint. +// NewJointPrismatic adds a new Prismatic (slider) joint between given +// parent and child. Use nil for parent to add a world-anchored joint. // ppos, cpos are the relative positions from the parent, child. // These are for the non-rotated body (i.e., body rotation is applied // to these positions as well). // Sets relative rotation matricies to identity by default. // axis is the axis of articulation for the joint. // Use [SetJointDoF] to set the remaining DoF parameters. -func (vw *View) NewJointPrismatic(ph *physics.Model, parent *View, ppos, cpos, axis math32.Vector3) int32 { +func (sc *Scene) NewJointPrismatic(ml *physics.Model, parent, child *Skin, ppos, cpos, axis math32.Vector3) int32 { pidx := int32(-1) if parent != nil { pidx = parent.DynamicIndex } - return ph.NewJointPrismatic(pidx, vw.DynamicIndex, ppos, cpos, axis) + return ml.NewJointPrismatic(pidx, child.DynamicIndex, ppos, cpos, axis) } -// NewJointRevolute adds a new Revolute (hinge, axel) joint as a child -// of given parent. Use nil for parent to add a world-anchored joint. +// NewJointRevolute adds a new Revolute (hinge, axel) joint between given +// parent and child. Use nil for parent to add a world-anchored joint. // ppos, cpos are the relative positions from the parent, child. // These are for the non-rotated body (i.e., body rotation is applied // to these positions as well). // Sets relative rotation matricies to identity by default. // axis is the axis of articulation for the joint. // Use [SetJointDoF] to set the remaining DoF parameters. -func (vw *View) NewJointRevolute(ph *physics.Model, parent *View, ppos, cpos, axis math32.Vector3) int32 { +func (sc *Scene) NewJointRevolute(ml *physics.Model, parent, child *Skin, ppos, cpos, axis math32.Vector3) int32 { pidx := int32(-1) if parent != nil { pidx = parent.DynamicIndex } - return ph.NewJointRevolute(pidx, vw.DynamicIndex, ppos, cpos, axis) + return ml.NewJointRevolute(pidx, child.DynamicIndex, ppos, cpos, axis) } -// NewJointBall adds a new Ball joint (3 angular DoF) as a child -// of given parent. Use nil for parent to add a world-anchored joint. +// NewJointBall adds a new Ball joint (3 angular DoF) between given parent +// and child. Use nil for parent to add a world-anchored joint. // ppos, cpos are the relative positions from the parent, child. // These are for the non-rotated body (i.e., body rotation is applied // to these positions as well). // Sets relative rotation matricies to identity by default. // Use [SetJointDoF] to set the remaining DoF parameters. -func (vw *View) NewJointBall(ph *physics.Model, parent *View, ppos, cpos math32.Vector3) int32 { +func (sc *Scene) NewJointBall(ml *physics.Model, parent, child *Skin, ppos, cpos math32.Vector3) int32 { pidx := int32(-1) if parent != nil { pidx = parent.DynamicIndex } - return ph.NewJointBall(pidx, vw.DynamicIndex, ppos, cpos) + return ml.NewJointBall(pidx, child.DynamicIndex, ppos, cpos) } // NewJointDistance adds a new Distance joint (6 DoF), // with distance constrained only on the first linear X axis, -// as a child of given parent. Use nil for parent to add a world-anchored joint. +// between given parent and child. Use nil for parent to add a world-anchored joint. // ppos, cpos are the relative positions from the parent, child. // These are for the non-rotated body (i.e., body rotation is applied // to these positions as well). // Sets relative rotation matricies to identity by default. // Use [SetJointDoF] to set the remaining DoF parameters. -func (vw *View) NewJointDistance(ph *physics.Model, parent *View, ppos, cpos math32.Vector3, minDist, maxDist float32) int32 { +func (sc *Scene) NewJointDistance(ml *physics.Model, parent, child *Skin, ppos, cpos math32.Vector3, minDist, maxDist float32) int32 { pidx := int32(-1) if parent != nil { pidx = parent.DynamicIndex } - return ph.NewJointDistance(pidx, vw.DynamicIndex, ppos, cpos, minDist, maxDist) + return ml.NewJointDistance(pidx, child.DynamicIndex, ppos, cpos, minDist, maxDist) } -// NewJointFree adds a new Free joint as a child -// of given parent. Use nil for parent to add a world-anchored joint. +// NewJointFree adds a new Free joint between given parent and child. +// Use nil for parent to add a world-anchored joint. // ppos, cpos are the relative positions from the parent, child. // These are for the non-rotated body (i.e., body rotation is applied // to these positions as well). // Sets relative rotation matricies to identity by default. // Use [SetJointDoF] to set the remaining DoF parameters. -func (vw *View) NewJointFree(ph *physics.Model, parent *View, ppos, cpos math32.Vector3) int32 { +func (sc *Scene) NewJointFree(ml *physics.Model, parent, child *Skin, ppos, cpos math32.Vector3) int32 { pidx := int32(-1) if parent != nil { pidx = parent.DynamicIndex } - return ph.NewJointFree(pidx, vw.DynamicIndex, ppos, cpos) + return ml.NewJointFree(pidx, child.DynamicIndex, ppos, cpos) } diff --git a/physics/phyxyz/typegen.go b/physics/phyxyz/typegen.go index 89610ca4..8e8ceb66 100644 --- a/physics/phyxyz/typegen.go +++ b/physics/phyxyz/typegen.go @@ -6,6 +6,7 @@ import ( "cogentcore.org/core/math32" "cogentcore.org/core/tree" "cogentcore.org/core/types" + "cogentcore.org/core/xyz" "cogentcore.org/lab/physics" ) @@ -50,6 +51,50 @@ func (t *Editor) SetCameraPos(v math32.Vector3) *Editor { t.CameraPos = v; retur // TimeStep is current time step in physics update cycles. func (t *Editor) SetTimeStep(v int) *Editor { t.TimeStep = v; return t } -var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics/phyxyz.View", IDName: "view", Doc: "View has visualization functions for physics elements.", Fields: []types.Field{{Name: "Name", Doc: "Name is a name for element (index always appended)."}, {Name: "Shape", Doc: "Shape is the physical shape of the element."}, {Name: "Color", Doc: "Color is the color of the element."}, {Name: "Size", Doc: "Size is the size (per shape)."}, {Name: "Pos", Doc: "Pos is the position."}, {Name: "Quat", Doc: "Quat is the rotation as a quaternion."}, {Name: "NewView", Doc: "NewView is a function that returns a new [xyz.Node]\nto represent this element. If nil, uses appropriate defaults."}, {Name: "InitView", Doc: "InitView is a function that initializes a new [xyz.Node]\nthat represents this element. If nil, uses appropriate defaults."}, {Name: "Index", Doc: "Index is the index of the element in a list."}, {Name: "DynamicIndex", Doc: "DynamicIndex is the index of a dynamic element (-1 if not dynamic)."}}}) +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics/phyxyz.Scene", IDName: "scene", Doc: "Scene displays a [physics.Model] using a [xyz.Scene].\nOne Scene can be used for multiple different [physics.Model]s which\nis more efficient when running multiple in parallel.\nInitial construction of the physics and visualization happens here.", Fields: []types.Field{{Name: "Scene", Doc: "Scene is the [xyz.Scene] object for visualizing."}, {Name: "Root", Doc: "Root is the root Group node in the Scene under which the world is rendered."}, {Name: "Skins", Doc: "Skins are the view elements for each body in [physics.Model]."}}}) -var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics/phyxyz.Scene", IDName: "scene", Doc: "Scene displays a [physics.Model] using a [xyz.Scene].\nOne Scene can be used for multiple different [physics.Model]s which\nis more efficient when running multiple in parallel.\nInitial construction of the physics and visualization happens here.", Fields: []types.Field{{Name: "Scene", Doc: "Scene is the [xyz.Scene] object for visualizing."}, {Name: "Root", Doc: "Root is the root Group node in the Scene under which the world is rendered."}, {Name: "Views", Doc: "Views are the view elements for each body in [physics.Model]."}}}) +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics/phyxyz.Skin", IDName: "skin", Doc: "Skin has visualization functions for physics elements.", Directives: []types.Directive{{Tool: "types", Directive: "add", Args: []string{"-setters"}}}, Fields: []types.Field{{Name: "Name", Doc: "Name is a name for element (index always appended, so it is unique)."}, {Name: "Shape", Doc: "Shape is the physical shape of the element."}, {Name: "Color", Doc: "Color is the color of the element."}, {Name: "HSize", Doc: "HSize is the half-size (e.g., radius) of the body.\nValues depend on shape type: X is generally radius,\nY is half-height."}, {Name: "Pos", Doc: "Pos is the position."}, {Name: "Quat", Doc: "Quat is the rotation as a quaternion."}, {Name: "NewSkin", Doc: "NewSkin is a function that returns a new [xyz.Node]\nto represent this element. If nil, uses appropriate defaults."}, {Name: "InitSkin", Doc: "InitSkin is a function that initializes a new [xyz.Node]\nthat represents this element. If nil, uses appropriate defaults."}, {Name: "BodyIndex", Doc: "BodyIndex is the index of the body in [physics.Bodies]"}, {Name: "DynamicIndex", Doc: "DynamicIndex is the index in [physics.Dynamics] (-1 if not dynamic)."}}}) + +// SetName sets the [Skin.Name]: +// Name is a name for element (index always appended, so it is unique). +func (t *Skin) SetName(v string) *Skin { t.Name = v; return t } + +// SetShape sets the [Skin.Shape]: +// Shape is the physical shape of the element. +func (t *Skin) SetShape(v physics.Shapes) *Skin { t.Shape = v; return t } + +// SetColor sets the [Skin.Color]: +// Color is the color of the element. +func (t *Skin) SetColor(v string) *Skin { t.Color = v; return t } + +// SetHSize sets the [Skin.HSize]: +// HSize is the half-size (e.g., radius) of the body. +// Values depend on shape type: X is generally radius, +// Y is half-height. +func (t *Skin) SetHSize(v math32.Vector3) *Skin { t.HSize = v; return t } + +// SetPos sets the [Skin.Pos]: +// Pos is the position. +func (t *Skin) SetPos(v math32.Vector3) *Skin { t.Pos = v; return t } + +// SetQuat sets the [Skin.Quat]: +// Quat is the rotation as a quaternion. +func (t *Skin) SetQuat(v math32.Quat) *Skin { t.Quat = v; return t } + +// SetNewSkin sets the [Skin.NewSkin]: +// NewSkin is a function that returns a new [xyz.Node] +// to represent this element. If nil, uses appropriate defaults. +func (t *Skin) SetNewSkin(v func() tree.Node) *Skin { t.NewSkin = v; return t } + +// SetInitSkin sets the [Skin.InitSkin]: +// InitSkin is a function that initializes a new [xyz.Node] +// that represents this element. If nil, uses appropriate defaults. +func (t *Skin) SetInitSkin(v func(sld *xyz.Solid)) *Skin { t.InitSkin = v; return t } + +// SetBodyIndex sets the [Skin.BodyIndex]: +// BodyIndex is the index of the body in [physics.Bodies] +func (t *Skin) SetBodyIndex(v int32) *Skin { t.BodyIndex = v; return t } + +// SetDynamicIndex sets the [Skin.DynamicIndex]: +// DynamicIndex is the index in [physics.Dynamics] (-1 if not dynamic). +func (t *Skin) SetDynamicIndex(v int32) *Skin { t.DynamicIndex = v; return t } diff --git a/physics/shaders/CollisionBroad.wgsl b/physics/shaders/CollisionBroad.wgsl index 3c96fbf6..041e3353 100644 --- a/physics/shaders/CollisionBroad.wgsl +++ b/physics/shaders/CollisionBroad.wgsl @@ -49,46 +49,45 @@ const BodyShape: BodyVars = 0; const BodyDynamic: BodyVars = 1; const BodyWorld: BodyVars = 2; const BodyGroup: BodyVars = 3; -const BodyObject: BodyVars = 4; -const BodyHSizeX: BodyVars = 5; -const BodyHSizeY: BodyVars = 6; -const BodyHSizeZ: BodyVars = 7; -const BodyThick: BodyVars = 8; -const BodyMass: BodyVars = 9; -const BodyInvMass: BodyVars = 10; -const BodyBounce: BodyVars = 11; -const BodyFriction: BodyVars = 12; -const BodyFrictionTortion: BodyVars = 13; -const BodyFrictionRolling: BodyVars = 14; -const BodyPosX: BodyVars = 15; -const BodyPosY: BodyVars = 16; -const BodyPosZ: BodyVars = 17; -const BodyQuatX: BodyVars = 18; -const BodyQuatY: BodyVars = 19; -const BodyQuatZ: BodyVars = 20; -const BodyQuatW: BodyVars = 21; -const BodyComX: BodyVars = 22; -const BodyComY: BodyVars = 23; -const BodyComZ: BodyVars = 24; -const BodyInertiaXX: BodyVars = 25; -const BodyInertiaYX: BodyVars = 26; -const BodyInertiaZX: BodyVars = 27; -const BodyInertiaXY: BodyVars = 28; -const BodyInertiaYY: BodyVars = 29; -const BodyInertiaZY: BodyVars = 30; -const BodyInertiaXZ: BodyVars = 31; -const BodyInertiaYZ: BodyVars = 32; -const BodyInertiaZZ: BodyVars = 33; -const BodyInvInertiaXX: BodyVars = 34; -const BodyInvInertiaYX: BodyVars = 35; -const BodyInvInertiaZX: BodyVars = 36; -const BodyInvInertiaXY: BodyVars = 37; -const BodyInvInertiaYY: BodyVars = 38; -const BodyInvInertiaZY: BodyVars = 39; -const BodyInvInertiaXZ: BodyVars = 40; -const BodyInvInertiaYZ: BodyVars = 41; -const BodyInvInertiaZZ: BodyVars = 42; -const BodyRadius: BodyVars = 43; +const BodyHSizeX: BodyVars = 4; +const BodyHSizeY: BodyVars = 5; +const BodyHSizeZ: BodyVars = 6; +const BodyThick: BodyVars = 7; +const BodyMass: BodyVars = 8; +const BodyInvMass: BodyVars = 9; +const BodyBounce: BodyVars = 10; +const BodyFriction: BodyVars = 11; +const BodyFrictionTortion: BodyVars = 12; +const BodyFrictionRolling: BodyVars = 13; +const BodyPosX: BodyVars = 14; +const BodyPosY: BodyVars = 15; +const BodyPosZ: BodyVars = 16; +const BodyQuatX: BodyVars = 17; +const BodyQuatY: BodyVars = 18; +const BodyQuatZ: BodyVars = 19; +const BodyQuatW: BodyVars = 20; +const BodyComX: BodyVars = 21; +const BodyComY: BodyVars = 22; +const BodyComZ: BodyVars = 23; +const BodyInertiaXX: BodyVars = 24; +const BodyInertiaYX: BodyVars = 25; +const BodyInertiaZX: BodyVars = 26; +const BodyInertiaXY: BodyVars = 27; +const BodyInertiaYY: BodyVars = 28; +const BodyInertiaZY: BodyVars = 29; +const BodyInertiaXZ: BodyVars = 30; +const BodyInertiaYZ: BodyVars = 31; +const BodyInertiaZZ: BodyVars = 32; +const BodyInvInertiaXX: BodyVars = 33; +const BodyInvInertiaYX: BodyVars = 34; +const BodyInvInertiaZX: BodyVars = 35; +const BodyInvInertiaXY: BodyVars = 36; +const BodyInvInertiaYY: BodyVars = 37; +const BodyInvInertiaZY: BodyVars = 38; +const BodyInvInertiaXZ: BodyVars = 39; +const BodyInvInertiaYZ: BodyVars = 40; +const BodyInvInertiaZZ: BodyVars = 41; +const BodyRadius: BodyVars = 42; fn GetBodyShape(idx: i32) -> Shapes { return Shapes(bitcast(Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyShape))])); } @@ -225,10 +224,10 @@ fn AddBroadContacts(biA: i32,biB: i32,nci: i32,ncA: i32,ncB: i32) { //////// import: "control.go" alias JointControlVars = i32; //enums:enum const JointControlForce: JointControlVars = 0; -const JointTargetStiff: JointControlVars = 1; -const JointTargetPos: JointControlVars = 2; -const JointTargetDamp: JointControlVars = 3; -const JointTargetVel: JointControlVars = 4; +const JointTargetPos: JointControlVars = 1; +const JointTargetStiff: JointControlVars = 2; +const JointTargetVel: JointControlVars = 3; +const JointTargetDamp: JointControlVars = 4; //////// import: "dynamics.go" alias DynamicVars = i32; //enums:enum @@ -273,7 +272,7 @@ fn DynamicQuat(idx: i32,cni: i32) -> vec4 { } //////// import: "enumgen.go" -const BodyVarsN: BodyVars = 44; +const BodyVarsN: BodyVars = 43; const ContactVarsN: ContactVars = 33; const JointControlVarsN: JointControlVars = 5; const DynamicVarsN: DynamicVars = 33; diff --git a/physics/shaders/CollisionNarrow.wgsl b/physics/shaders/CollisionNarrow.wgsl index e31db079..08b9c3d2 100644 --- a/physics/shaders/CollisionNarrow.wgsl +++ b/physics/shaders/CollisionNarrow.wgsl @@ -51,46 +51,45 @@ const BodyShape: BodyVars = 0; const BodyDynamic: BodyVars = 1; const BodyWorld: BodyVars = 2; const BodyGroup: BodyVars = 3; -const BodyObject: BodyVars = 4; -const BodyHSizeX: BodyVars = 5; -const BodyHSizeY: BodyVars = 6; -const BodyHSizeZ: BodyVars = 7; -const BodyThick: BodyVars = 8; -const BodyMass: BodyVars = 9; -const BodyInvMass: BodyVars = 10; -const BodyBounce: BodyVars = 11; -const BodyFriction: BodyVars = 12; -const BodyFrictionTortion: BodyVars = 13; -const BodyFrictionRolling: BodyVars = 14; -const BodyPosX: BodyVars = 15; -const BodyPosY: BodyVars = 16; -const BodyPosZ: BodyVars = 17; -const BodyQuatX: BodyVars = 18; -const BodyQuatY: BodyVars = 19; -const BodyQuatZ: BodyVars = 20; -const BodyQuatW: BodyVars = 21; -const BodyComX: BodyVars = 22; -const BodyComY: BodyVars = 23; -const BodyComZ: BodyVars = 24; -const BodyInertiaXX: BodyVars = 25; -const BodyInertiaYX: BodyVars = 26; -const BodyInertiaZX: BodyVars = 27; -const BodyInertiaXY: BodyVars = 28; -const BodyInertiaYY: BodyVars = 29; -const BodyInertiaZY: BodyVars = 30; -const BodyInertiaXZ: BodyVars = 31; -const BodyInertiaYZ: BodyVars = 32; -const BodyInertiaZZ: BodyVars = 33; -const BodyInvInertiaXX: BodyVars = 34; -const BodyInvInertiaYX: BodyVars = 35; -const BodyInvInertiaZX: BodyVars = 36; -const BodyInvInertiaXY: BodyVars = 37; -const BodyInvInertiaYY: BodyVars = 38; -const BodyInvInertiaZY: BodyVars = 39; -const BodyInvInertiaXZ: BodyVars = 40; -const BodyInvInertiaYZ: BodyVars = 41; -const BodyInvInertiaZZ: BodyVars = 42; -const BodyRadius: BodyVars = 43; +const BodyHSizeX: BodyVars = 4; +const BodyHSizeY: BodyVars = 5; +const BodyHSizeZ: BodyVars = 6; +const BodyThick: BodyVars = 7; +const BodyMass: BodyVars = 8; +const BodyInvMass: BodyVars = 9; +const BodyBounce: BodyVars = 10; +const BodyFriction: BodyVars = 11; +const BodyFrictionTortion: BodyVars = 12; +const BodyFrictionRolling: BodyVars = 13; +const BodyPosX: BodyVars = 14; +const BodyPosY: BodyVars = 15; +const BodyPosZ: BodyVars = 16; +const BodyQuatX: BodyVars = 17; +const BodyQuatY: BodyVars = 18; +const BodyQuatZ: BodyVars = 19; +const BodyQuatW: BodyVars = 20; +const BodyComX: BodyVars = 21; +const BodyComY: BodyVars = 22; +const BodyComZ: BodyVars = 23; +const BodyInertiaXX: BodyVars = 24; +const BodyInertiaYX: BodyVars = 25; +const BodyInertiaZX: BodyVars = 26; +const BodyInertiaXY: BodyVars = 27; +const BodyInertiaYY: BodyVars = 28; +const BodyInertiaZY: BodyVars = 29; +const BodyInertiaXZ: BodyVars = 30; +const BodyInertiaYZ: BodyVars = 31; +const BodyInertiaZZ: BodyVars = 32; +const BodyInvInertiaXX: BodyVars = 33; +const BodyInvInertiaYX: BodyVars = 34; +const BodyInvInertiaZX: BodyVars = 35; +const BodyInvInertiaXY: BodyVars = 36; +const BodyInvInertiaYY: BodyVars = 37; +const BodyInvInertiaZY: BodyVars = 38; +const BodyInvInertiaXZ: BodyVars = 39; +const BodyInvInertiaYZ: BodyVars = 40; +const BodyInvInertiaZZ: BodyVars = 41; +const BodyRadius: BodyVars = 42; fn GetBodyShape(idx: i32) -> Shapes { return Shapes(bitcast(Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyShape))])); } @@ -314,10 +313,10 @@ fn CollisionNarrow(i: u32) { //gosl:kernel //////// import: "control.go" alias JointControlVars = i32; //enums:enum const JointControlForce: JointControlVars = 0; -const JointTargetStiff: JointControlVars = 1; -const JointTargetPos: JointControlVars = 2; -const JointTargetDamp: JointControlVars = 3; -const JointTargetVel: JointControlVars = 4; +const JointTargetPos: JointControlVars = 1; +const JointTargetStiff: JointControlVars = 2; +const JointTargetVel: JointControlVars = 3; +const JointTargetDamp: JointControlVars = 4; //////// import: "dynamics.go" alias DynamicVars = i32; //enums:enum @@ -362,7 +361,7 @@ fn DynamicQuat(idx: i32,cni: i32) -> vec4 { } //////// import: "enumgen.go" -const BodyVarsN: BodyVars = 44; +const BodyVarsN: BodyVars = 43; const ContactVarsN: ContactVars = 33; const JointControlVarsN: JointControlVars = 5; const DynamicVarsN: DynamicVars = 33; diff --git a/physics/shaders/DynamicsCurToNext.wgsl b/physics/shaders/DynamicsCurToNext.wgsl index 584a9bbd..497ccecb 100644 --- a/physics/shaders/DynamicsCurToNext.wgsl +++ b/physics/shaders/DynamicsCurToNext.wgsl @@ -33,46 +33,45 @@ const BodyShape: BodyVars = 0; const BodyDynamic: BodyVars = 1; const BodyWorld: BodyVars = 2; const BodyGroup: BodyVars = 3; -const BodyObject: BodyVars = 4; -const BodyHSizeX: BodyVars = 5; -const BodyHSizeY: BodyVars = 6; -const BodyHSizeZ: BodyVars = 7; -const BodyThick: BodyVars = 8; -const BodyMass: BodyVars = 9; -const BodyInvMass: BodyVars = 10; -const BodyBounce: BodyVars = 11; -const BodyFriction: BodyVars = 12; -const BodyFrictionTortion: BodyVars = 13; -const BodyFrictionRolling: BodyVars = 14; -const BodyPosX: BodyVars = 15; -const BodyPosY: BodyVars = 16; -const BodyPosZ: BodyVars = 17; -const BodyQuatX: BodyVars = 18; -const BodyQuatY: BodyVars = 19; -const BodyQuatZ: BodyVars = 20; -const BodyQuatW: BodyVars = 21; -const BodyComX: BodyVars = 22; -const BodyComY: BodyVars = 23; -const BodyComZ: BodyVars = 24; -const BodyInertiaXX: BodyVars = 25; -const BodyInertiaYX: BodyVars = 26; -const BodyInertiaZX: BodyVars = 27; -const BodyInertiaXY: BodyVars = 28; -const BodyInertiaYY: BodyVars = 29; -const BodyInertiaZY: BodyVars = 30; -const BodyInertiaXZ: BodyVars = 31; -const BodyInertiaYZ: BodyVars = 32; -const BodyInertiaZZ: BodyVars = 33; -const BodyInvInertiaXX: BodyVars = 34; -const BodyInvInertiaYX: BodyVars = 35; -const BodyInvInertiaZX: BodyVars = 36; -const BodyInvInertiaXY: BodyVars = 37; -const BodyInvInertiaYY: BodyVars = 38; -const BodyInvInertiaZY: BodyVars = 39; -const BodyInvInertiaXZ: BodyVars = 40; -const BodyInvInertiaYZ: BodyVars = 41; -const BodyInvInertiaZZ: BodyVars = 42; -const BodyRadius: BodyVars = 43; +const BodyHSizeX: BodyVars = 4; +const BodyHSizeY: BodyVars = 5; +const BodyHSizeZ: BodyVars = 6; +const BodyThick: BodyVars = 7; +const BodyMass: BodyVars = 8; +const BodyInvMass: BodyVars = 9; +const BodyBounce: BodyVars = 10; +const BodyFriction: BodyVars = 11; +const BodyFrictionTortion: BodyVars = 12; +const BodyFrictionRolling: BodyVars = 13; +const BodyPosX: BodyVars = 14; +const BodyPosY: BodyVars = 15; +const BodyPosZ: BodyVars = 16; +const BodyQuatX: BodyVars = 17; +const BodyQuatY: BodyVars = 18; +const BodyQuatZ: BodyVars = 19; +const BodyQuatW: BodyVars = 20; +const BodyComX: BodyVars = 21; +const BodyComY: BodyVars = 22; +const BodyComZ: BodyVars = 23; +const BodyInertiaXX: BodyVars = 24; +const BodyInertiaYX: BodyVars = 25; +const BodyInertiaZX: BodyVars = 26; +const BodyInertiaXY: BodyVars = 27; +const BodyInertiaYY: BodyVars = 28; +const BodyInertiaZY: BodyVars = 29; +const BodyInertiaXZ: BodyVars = 30; +const BodyInertiaYZ: BodyVars = 31; +const BodyInertiaZZ: BodyVars = 32; +const BodyInvInertiaXX: BodyVars = 33; +const BodyInvInertiaYX: BodyVars = 34; +const BodyInvInertiaZX: BodyVars = 35; +const BodyInvInertiaXY: BodyVars = 36; +const BodyInvInertiaYY: BodyVars = 37; +const BodyInvInertiaZY: BodyVars = 38; +const BodyInvInertiaXZ: BodyVars = 39; +const BodyInvInertiaYZ: BodyVars = 40; +const BodyInvInertiaZZ: BodyVars = 41; +const BodyRadius: BodyVars = 42; //////// import: "contact.go" alias ContactVars = i32; //enums:enum @@ -114,10 +113,10 @@ const BroadContactVarsN = ContactAPointX; //////// import: "control.go" alias JointControlVars = i32; //enums:enum const JointControlForce: JointControlVars = 0; -const JointTargetStiff: JointControlVars = 1; -const JointTargetPos: JointControlVars = 2; -const JointTargetDamp: JointControlVars = 3; -const JointTargetVel: JointControlVars = 4; +const JointTargetPos: JointControlVars = 1; +const JointTargetStiff: JointControlVars = 2; +const JointTargetVel: JointControlVars = 3; +const JointTargetDamp: JointControlVars = 4; //////// import: "dynamics.go" alias DynamicVars = i32; //enums:enum @@ -156,7 +155,7 @@ const DynAngDeltaZ: DynamicVars = 31; const DynContactWeight: DynamicVars = 32; //////// import: "enumgen.go" -const BodyVarsN: BodyVars = 44; +const BodyVarsN: BodyVars = 43; const ContactVarsN: ContactVars = 33; const JointControlVarsN: JointControlVars = 5; const DynamicVarsN: DynamicVars = 33; diff --git a/physics/shaders/ForcesFromJoints.wgsl b/physics/shaders/ForcesFromJoints.wgsl index 70b13c56..23a0e341 100644 --- a/physics/shaders/ForcesFromJoints.wgsl +++ b/physics/shaders/ForcesFromJoints.wgsl @@ -41,46 +41,45 @@ const BodyShape: BodyVars = 0; const BodyDynamic: BodyVars = 1; const BodyWorld: BodyVars = 2; const BodyGroup: BodyVars = 3; -const BodyObject: BodyVars = 4; -const BodyHSizeX: BodyVars = 5; -const BodyHSizeY: BodyVars = 6; -const BodyHSizeZ: BodyVars = 7; -const BodyThick: BodyVars = 8; -const BodyMass: BodyVars = 9; -const BodyInvMass: BodyVars = 10; -const BodyBounce: BodyVars = 11; -const BodyFriction: BodyVars = 12; -const BodyFrictionTortion: BodyVars = 13; -const BodyFrictionRolling: BodyVars = 14; -const BodyPosX: BodyVars = 15; -const BodyPosY: BodyVars = 16; -const BodyPosZ: BodyVars = 17; -const BodyQuatX: BodyVars = 18; -const BodyQuatY: BodyVars = 19; -const BodyQuatZ: BodyVars = 20; -const BodyQuatW: BodyVars = 21; -const BodyComX: BodyVars = 22; -const BodyComY: BodyVars = 23; -const BodyComZ: BodyVars = 24; -const BodyInertiaXX: BodyVars = 25; -const BodyInertiaYX: BodyVars = 26; -const BodyInertiaZX: BodyVars = 27; -const BodyInertiaXY: BodyVars = 28; -const BodyInertiaYY: BodyVars = 29; -const BodyInertiaZY: BodyVars = 30; -const BodyInertiaXZ: BodyVars = 31; -const BodyInertiaYZ: BodyVars = 32; -const BodyInertiaZZ: BodyVars = 33; -const BodyInvInertiaXX: BodyVars = 34; -const BodyInvInertiaYX: BodyVars = 35; -const BodyInvInertiaZX: BodyVars = 36; -const BodyInvInertiaXY: BodyVars = 37; -const BodyInvInertiaYY: BodyVars = 38; -const BodyInvInertiaZY: BodyVars = 39; -const BodyInvInertiaXZ: BodyVars = 40; -const BodyInvInertiaYZ: BodyVars = 41; -const BodyInvInertiaZZ: BodyVars = 42; -const BodyRadius: BodyVars = 43; +const BodyHSizeX: BodyVars = 4; +const BodyHSizeY: BodyVars = 5; +const BodyHSizeZ: BodyVars = 6; +const BodyThick: BodyVars = 7; +const BodyMass: BodyVars = 8; +const BodyInvMass: BodyVars = 9; +const BodyBounce: BodyVars = 10; +const BodyFriction: BodyVars = 11; +const BodyFrictionTortion: BodyVars = 12; +const BodyFrictionRolling: BodyVars = 13; +const BodyPosX: BodyVars = 14; +const BodyPosY: BodyVars = 15; +const BodyPosZ: BodyVars = 16; +const BodyQuatX: BodyVars = 17; +const BodyQuatY: BodyVars = 18; +const BodyQuatZ: BodyVars = 19; +const BodyQuatW: BodyVars = 20; +const BodyComX: BodyVars = 21; +const BodyComY: BodyVars = 22; +const BodyComZ: BodyVars = 23; +const BodyInertiaXX: BodyVars = 24; +const BodyInertiaYX: BodyVars = 25; +const BodyInertiaZX: BodyVars = 26; +const BodyInertiaXY: BodyVars = 27; +const BodyInertiaYY: BodyVars = 28; +const BodyInertiaZY: BodyVars = 29; +const BodyInertiaXZ: BodyVars = 30; +const BodyInertiaYZ: BodyVars = 31; +const BodyInertiaZZ: BodyVars = 32; +const BodyInvInertiaXX: BodyVars = 33; +const BodyInvInertiaYX: BodyVars = 34; +const BodyInvInertiaZX: BodyVars = 35; +const BodyInvInertiaXY: BodyVars = 36; +const BodyInvInertiaYY: BodyVars = 37; +const BodyInvInertiaZY: BodyVars = 38; +const BodyInvInertiaXZ: BodyVars = 39; +const BodyInvInertiaYZ: BodyVars = 40; +const BodyInvInertiaZZ: BodyVars = 41; +const BodyRadius: BodyVars = 42; //////// import: "contact.go" alias ContactVars = i32; //enums:enum @@ -122,10 +121,10 @@ const BroadContactVarsN = ContactAPointX; //////// import: "control.go" alias JointControlVars = i32; //enums:enum const JointControlForce: JointControlVars = 0; -const JointTargetStiff: JointControlVars = 1; -const JointTargetPos: JointControlVars = 2; -const JointTargetDamp: JointControlVars = 3; -const JointTargetVel: JointControlVars = 4; +const JointTargetPos: JointControlVars = 1; +const JointTargetStiff: JointControlVars = 2; +const JointTargetVel: JointControlVars = 3; +const JointTargetDamp: JointControlVars = 4; //////// import: "dynamics.go" alias DynamicVars = i32; //enums:enum @@ -174,7 +173,7 @@ fn SetDynamicTorque(idx: i32,cni: i32, torque: vec3) { } //////// import: "enumgen.go" -const BodyVarsN: BodyVars = 44; +const BodyVarsN: BodyVars = 43; const ContactVarsN: ContactVars = 33; const JointControlVarsN: JointControlVars = 5; const DynamicVarsN: DynamicVars = 33; diff --git a/physics/shaders/InitDynamics.wgsl b/physics/shaders/InitDynamics.wgsl index a3284ee4..0accff03 100644 --- a/physics/shaders/InitDynamics.wgsl +++ b/physics/shaders/InitDynamics.wgsl @@ -39,46 +39,45 @@ const BodyShape: BodyVars = 0; const BodyDynamic: BodyVars = 1; const BodyWorld: BodyVars = 2; const BodyGroup: BodyVars = 3; -const BodyObject: BodyVars = 4; -const BodyHSizeX: BodyVars = 5; -const BodyHSizeY: BodyVars = 6; -const BodyHSizeZ: BodyVars = 7; -const BodyThick: BodyVars = 8; -const BodyMass: BodyVars = 9; -const BodyInvMass: BodyVars = 10; -const BodyBounce: BodyVars = 11; -const BodyFriction: BodyVars = 12; -const BodyFrictionTortion: BodyVars = 13; -const BodyFrictionRolling: BodyVars = 14; -const BodyPosX: BodyVars = 15; -const BodyPosY: BodyVars = 16; -const BodyPosZ: BodyVars = 17; -const BodyQuatX: BodyVars = 18; -const BodyQuatY: BodyVars = 19; -const BodyQuatZ: BodyVars = 20; -const BodyQuatW: BodyVars = 21; -const BodyComX: BodyVars = 22; -const BodyComY: BodyVars = 23; -const BodyComZ: BodyVars = 24; -const BodyInertiaXX: BodyVars = 25; -const BodyInertiaYX: BodyVars = 26; -const BodyInertiaZX: BodyVars = 27; -const BodyInertiaXY: BodyVars = 28; -const BodyInertiaYY: BodyVars = 29; -const BodyInertiaZY: BodyVars = 30; -const BodyInertiaXZ: BodyVars = 31; -const BodyInertiaYZ: BodyVars = 32; -const BodyInertiaZZ: BodyVars = 33; -const BodyInvInertiaXX: BodyVars = 34; -const BodyInvInertiaYX: BodyVars = 35; -const BodyInvInertiaZX: BodyVars = 36; -const BodyInvInertiaXY: BodyVars = 37; -const BodyInvInertiaYY: BodyVars = 38; -const BodyInvInertiaZY: BodyVars = 39; -const BodyInvInertiaXZ: BodyVars = 40; -const BodyInvInertiaYZ: BodyVars = 41; -const BodyInvInertiaZZ: BodyVars = 42; -const BodyRadius: BodyVars = 43; +const BodyHSizeX: BodyVars = 4; +const BodyHSizeY: BodyVars = 5; +const BodyHSizeZ: BodyVars = 6; +const BodyThick: BodyVars = 7; +const BodyMass: BodyVars = 8; +const BodyInvMass: BodyVars = 9; +const BodyBounce: BodyVars = 10; +const BodyFriction: BodyVars = 11; +const BodyFrictionTortion: BodyVars = 12; +const BodyFrictionRolling: BodyVars = 13; +const BodyPosX: BodyVars = 14; +const BodyPosY: BodyVars = 15; +const BodyPosZ: BodyVars = 16; +const BodyQuatX: BodyVars = 17; +const BodyQuatY: BodyVars = 18; +const BodyQuatZ: BodyVars = 19; +const BodyQuatW: BodyVars = 20; +const BodyComX: BodyVars = 21; +const BodyComY: BodyVars = 22; +const BodyComZ: BodyVars = 23; +const BodyInertiaXX: BodyVars = 24; +const BodyInertiaYX: BodyVars = 25; +const BodyInertiaZX: BodyVars = 26; +const BodyInertiaXY: BodyVars = 27; +const BodyInertiaYY: BodyVars = 28; +const BodyInertiaZY: BodyVars = 29; +const BodyInertiaXZ: BodyVars = 30; +const BodyInertiaYZ: BodyVars = 31; +const BodyInertiaZZ: BodyVars = 32; +const BodyInvInertiaXX: BodyVars = 33; +const BodyInvInertiaYX: BodyVars = 34; +const BodyInvInertiaZX: BodyVars = 35; +const BodyInvInertiaXY: BodyVars = 36; +const BodyInvInertiaYY: BodyVars = 37; +const BodyInvInertiaZY: BodyVars = 38; +const BodyInvInertiaXZ: BodyVars = 39; +const BodyInvInertiaYZ: BodyVars = 40; +const BodyInvInertiaZZ: BodyVars = 41; +const BodyRadius: BodyVars = 42; //////// import: "contact.go" alias ContactVars = i32; //enums:enum @@ -120,10 +119,10 @@ const BroadContactVarsN = ContactAPointX; //////// import: "control.go" alias JointControlVars = i32; //enums:enum const JointControlForce: JointControlVars = 0; -const JointTargetStiff: JointControlVars = 1; -const JointTargetPos: JointControlVars = 2; -const JointTargetDamp: JointControlVars = 3; -const JointTargetVel: JointControlVars = 4; +const JointTargetPos: JointControlVars = 1; +const JointTargetStiff: JointControlVars = 2; +const JointTargetVel: JointControlVars = 3; +const JointTargetDamp: JointControlVars = 4; //////// import: "dynamics.go" alias DynamicVars = i32; //enums:enum @@ -163,7 +162,7 @@ const DynContactWeight: DynamicVars = 32; fn DynamicBody(idx: i32) -> i32 { return i32(bitcast(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(0), u32(DynBody))])); } //////// import: "enumgen.go" -const BodyVarsN: BodyVars = 44; +const BodyVarsN: BodyVars = 43; const ContactVarsN: ContactVars = 33; const JointControlVarsN: JointControlVars = 5; const DynamicVarsN: DynamicVars = 33; diff --git a/physics/shaders/StepBodyContactDeltas.wgsl b/physics/shaders/StepBodyContactDeltas.wgsl index ea738b80..be73febf 100644 --- a/physics/shaders/StepBodyContactDeltas.wgsl +++ b/physics/shaders/StepBodyContactDeltas.wgsl @@ -47,46 +47,45 @@ const BodyShape: BodyVars = 0; const BodyDynamic: BodyVars = 1; const BodyWorld: BodyVars = 2; const BodyGroup: BodyVars = 3; -const BodyObject: BodyVars = 4; -const BodyHSizeX: BodyVars = 5; -const BodyHSizeY: BodyVars = 6; -const BodyHSizeZ: BodyVars = 7; -const BodyThick: BodyVars = 8; -const BodyMass: BodyVars = 9; -const BodyInvMass: BodyVars = 10; -const BodyBounce: BodyVars = 11; -const BodyFriction: BodyVars = 12; -const BodyFrictionTortion: BodyVars = 13; -const BodyFrictionRolling: BodyVars = 14; -const BodyPosX: BodyVars = 15; -const BodyPosY: BodyVars = 16; -const BodyPosZ: BodyVars = 17; -const BodyQuatX: BodyVars = 18; -const BodyQuatY: BodyVars = 19; -const BodyQuatZ: BodyVars = 20; -const BodyQuatW: BodyVars = 21; -const BodyComX: BodyVars = 22; -const BodyComY: BodyVars = 23; -const BodyComZ: BodyVars = 24; -const BodyInertiaXX: BodyVars = 25; -const BodyInertiaYX: BodyVars = 26; -const BodyInertiaZX: BodyVars = 27; -const BodyInertiaXY: BodyVars = 28; -const BodyInertiaYY: BodyVars = 29; -const BodyInertiaZY: BodyVars = 30; -const BodyInertiaXZ: BodyVars = 31; -const BodyInertiaYZ: BodyVars = 32; -const BodyInertiaZZ: BodyVars = 33; -const BodyInvInertiaXX: BodyVars = 34; -const BodyInvInertiaYX: BodyVars = 35; -const BodyInvInertiaZX: BodyVars = 36; -const BodyInvInertiaXY: BodyVars = 37; -const BodyInvInertiaYY: BodyVars = 38; -const BodyInvInertiaZY: BodyVars = 39; -const BodyInvInertiaXZ: BodyVars = 40; -const BodyInvInertiaYZ: BodyVars = 41; -const BodyInvInertiaZZ: BodyVars = 42; -const BodyRadius: BodyVars = 43; +const BodyHSizeX: BodyVars = 4; +const BodyHSizeY: BodyVars = 5; +const BodyHSizeZ: BodyVars = 6; +const BodyThick: BodyVars = 7; +const BodyMass: BodyVars = 8; +const BodyInvMass: BodyVars = 9; +const BodyBounce: BodyVars = 10; +const BodyFriction: BodyVars = 11; +const BodyFrictionTortion: BodyVars = 12; +const BodyFrictionRolling: BodyVars = 13; +const BodyPosX: BodyVars = 14; +const BodyPosY: BodyVars = 15; +const BodyPosZ: BodyVars = 16; +const BodyQuatX: BodyVars = 17; +const BodyQuatY: BodyVars = 18; +const BodyQuatZ: BodyVars = 19; +const BodyQuatW: BodyVars = 20; +const BodyComX: BodyVars = 21; +const BodyComY: BodyVars = 22; +const BodyComZ: BodyVars = 23; +const BodyInertiaXX: BodyVars = 24; +const BodyInertiaYX: BodyVars = 25; +const BodyInertiaZX: BodyVars = 26; +const BodyInertiaXY: BodyVars = 27; +const BodyInertiaYY: BodyVars = 28; +const BodyInertiaZY: BodyVars = 29; +const BodyInertiaXZ: BodyVars = 30; +const BodyInertiaYZ: BodyVars = 31; +const BodyInertiaZZ: BodyVars = 32; +const BodyInvInertiaXX: BodyVars = 33; +const BodyInvInertiaYX: BodyVars = 34; +const BodyInvInertiaZX: BodyVars = 35; +const BodyInvInertiaXY: BodyVars = 36; +const BodyInvInertiaYY: BodyVars = 37; +const BodyInvInertiaZY: BodyVars = 38; +const BodyInvInertiaXZ: BodyVars = 39; +const BodyInvInertiaYZ: BodyVars = 40; +const BodyInvInertiaZZ: BodyVars = 41; +const BodyRadius: BodyVars = 42; fn BodyCom(idx: i32) -> vec3 { return vec3(Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComX))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComY))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComZ))]); } @@ -196,10 +195,10 @@ fn StepBodyContactDeltas(i: u32) { //gosl:kernel //////// import: "control.go" alias JointControlVars = i32; //enums:enum const JointControlForce: JointControlVars = 0; -const JointTargetStiff: JointControlVars = 1; -const JointTargetPos: JointControlVars = 2; -const JointTargetDamp: JointControlVars = 3; -const JointTargetVel: JointControlVars = 4; +const JointTargetPos: JointControlVars = 1; +const JointTargetStiff: JointControlVars = 2; +const JointTargetVel: JointControlVars = 3; +const JointTargetDamp: JointControlVars = 4; //////// import: "dynamics.go" alias DynamicVars = i32; //enums:enum @@ -259,7 +258,7 @@ fn SetDynamicAngDelta(idx: i32,cni: i32, angDelta: vec3) { } //////// import: "enumgen.go" -const BodyVarsN: BodyVars = 44; +const BodyVarsN: BodyVars = 43; const ContactVarsN: ContactVars = 33; const JointControlVarsN: JointControlVars = 5; const DynamicVarsN: DynamicVars = 33; diff --git a/physics/shaders/StepBodyContacts.wgsl b/physics/shaders/StepBodyContacts.wgsl index 9877a32d..f0d0de5a 100644 --- a/physics/shaders/StepBodyContacts.wgsl +++ b/physics/shaders/StepBodyContacts.wgsl @@ -47,46 +47,45 @@ const BodyShape: BodyVars = 0; const BodyDynamic: BodyVars = 1; const BodyWorld: BodyVars = 2; const BodyGroup: BodyVars = 3; -const BodyObject: BodyVars = 4; -const BodyHSizeX: BodyVars = 5; -const BodyHSizeY: BodyVars = 6; -const BodyHSizeZ: BodyVars = 7; -const BodyThick: BodyVars = 8; -const BodyMass: BodyVars = 9; -const BodyInvMass: BodyVars = 10; -const BodyBounce: BodyVars = 11; -const BodyFriction: BodyVars = 12; -const BodyFrictionTortion: BodyVars = 13; -const BodyFrictionRolling: BodyVars = 14; -const BodyPosX: BodyVars = 15; -const BodyPosY: BodyVars = 16; -const BodyPosZ: BodyVars = 17; -const BodyQuatX: BodyVars = 18; -const BodyQuatY: BodyVars = 19; -const BodyQuatZ: BodyVars = 20; -const BodyQuatW: BodyVars = 21; -const BodyComX: BodyVars = 22; -const BodyComY: BodyVars = 23; -const BodyComZ: BodyVars = 24; -const BodyInertiaXX: BodyVars = 25; -const BodyInertiaYX: BodyVars = 26; -const BodyInertiaZX: BodyVars = 27; -const BodyInertiaXY: BodyVars = 28; -const BodyInertiaYY: BodyVars = 29; -const BodyInertiaZY: BodyVars = 30; -const BodyInertiaXZ: BodyVars = 31; -const BodyInertiaYZ: BodyVars = 32; -const BodyInertiaZZ: BodyVars = 33; -const BodyInvInertiaXX: BodyVars = 34; -const BodyInvInertiaYX: BodyVars = 35; -const BodyInvInertiaZX: BodyVars = 36; -const BodyInvInertiaXY: BodyVars = 37; -const BodyInvInertiaYY: BodyVars = 38; -const BodyInvInertiaZY: BodyVars = 39; -const BodyInvInertiaXZ: BodyVars = 40; -const BodyInvInertiaYZ: BodyVars = 41; -const BodyInvInertiaZZ: BodyVars = 42; -const BodyRadius: BodyVars = 43; +const BodyHSizeX: BodyVars = 4; +const BodyHSizeY: BodyVars = 5; +const BodyHSizeZ: BodyVars = 6; +const BodyThick: BodyVars = 7; +const BodyMass: BodyVars = 8; +const BodyInvMass: BodyVars = 9; +const BodyBounce: BodyVars = 10; +const BodyFriction: BodyVars = 11; +const BodyFrictionTortion: BodyVars = 12; +const BodyFrictionRolling: BodyVars = 13; +const BodyPosX: BodyVars = 14; +const BodyPosY: BodyVars = 15; +const BodyPosZ: BodyVars = 16; +const BodyQuatX: BodyVars = 17; +const BodyQuatY: BodyVars = 18; +const BodyQuatZ: BodyVars = 19; +const BodyQuatW: BodyVars = 20; +const BodyComX: BodyVars = 21; +const BodyComY: BodyVars = 22; +const BodyComZ: BodyVars = 23; +const BodyInertiaXX: BodyVars = 24; +const BodyInertiaYX: BodyVars = 25; +const BodyInertiaZX: BodyVars = 26; +const BodyInertiaXY: BodyVars = 27; +const BodyInertiaYY: BodyVars = 28; +const BodyInertiaZY: BodyVars = 29; +const BodyInertiaXZ: BodyVars = 30; +const BodyInertiaYZ: BodyVars = 31; +const BodyInertiaZZ: BodyVars = 32; +const BodyInvInertiaXX: BodyVars = 33; +const BodyInvInertiaYX: BodyVars = 34; +const BodyInvInertiaZX: BodyVars = 35; +const BodyInvInertiaXY: BodyVars = 36; +const BodyInvInertiaYY: BodyVars = 37; +const BodyInvInertiaZY: BodyVars = 38; +const BodyInvInertiaXZ: BodyVars = 39; +const BodyInvInertiaYZ: BodyVars = 40; +const BodyInvInertiaZZ: BodyVars = 41; +const BodyRadius: BodyVars = 42; fn GetBodyDynamic(idx: i32) -> i32 { return i32(bitcast(Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyDynamic))])); @@ -334,10 +333,10 @@ fn ContactConstraint(err: f32, q0A: vec4,q0B: vec4, mInvA: f32,mInvB: //////// import: "control.go" alias JointControlVars = i32; //enums:enum const JointControlForce: JointControlVars = 0; -const JointTargetStiff: JointControlVars = 1; -const JointTargetPos: JointControlVars = 2; -const JointTargetDamp: JointControlVars = 3; -const JointTargetVel: JointControlVars = 4; +const JointTargetPos: JointControlVars = 1; +const JointTargetStiff: JointControlVars = 2; +const JointTargetVel: JointControlVars = 3; +const JointTargetDamp: JointControlVars = 4; //////// import: "dynamics.go" alias DynamicVars = i32; //enums:enum @@ -388,7 +387,7 @@ fn DynamicAngDelta(idx: i32,cni: i32) -> vec3 { } //////// import: "enumgen.go" -const BodyVarsN: BodyVars = 44; +const BodyVarsN: BodyVars = 43; const ContactVarsN: ContactVars = 33; const JointControlVarsN: JointControlVars = 5; const DynamicVarsN: DynamicVars = 33; diff --git a/physics/shaders/StepBodyJointDeltas.wgsl b/physics/shaders/StepBodyJointDeltas.wgsl index aba91ee9..0a370d4d 100644 --- a/physics/shaders/StepBodyJointDeltas.wgsl +++ b/physics/shaders/StepBodyJointDeltas.wgsl @@ -43,46 +43,45 @@ const BodyShape: BodyVars = 0; const BodyDynamic: BodyVars = 1; const BodyWorld: BodyVars = 2; const BodyGroup: BodyVars = 3; -const BodyObject: BodyVars = 4; -const BodyHSizeX: BodyVars = 5; -const BodyHSizeY: BodyVars = 6; -const BodyHSizeZ: BodyVars = 7; -const BodyThick: BodyVars = 8; -const BodyMass: BodyVars = 9; -const BodyInvMass: BodyVars = 10; -const BodyBounce: BodyVars = 11; -const BodyFriction: BodyVars = 12; -const BodyFrictionTortion: BodyVars = 13; -const BodyFrictionRolling: BodyVars = 14; -const BodyPosX: BodyVars = 15; -const BodyPosY: BodyVars = 16; -const BodyPosZ: BodyVars = 17; -const BodyQuatX: BodyVars = 18; -const BodyQuatY: BodyVars = 19; -const BodyQuatZ: BodyVars = 20; -const BodyQuatW: BodyVars = 21; -const BodyComX: BodyVars = 22; -const BodyComY: BodyVars = 23; -const BodyComZ: BodyVars = 24; -const BodyInertiaXX: BodyVars = 25; -const BodyInertiaYX: BodyVars = 26; -const BodyInertiaZX: BodyVars = 27; -const BodyInertiaXY: BodyVars = 28; -const BodyInertiaYY: BodyVars = 29; -const BodyInertiaZY: BodyVars = 30; -const BodyInertiaXZ: BodyVars = 31; -const BodyInertiaYZ: BodyVars = 32; -const BodyInertiaZZ: BodyVars = 33; -const BodyInvInertiaXX: BodyVars = 34; -const BodyInvInertiaYX: BodyVars = 35; -const BodyInvInertiaZX: BodyVars = 36; -const BodyInvInertiaXY: BodyVars = 37; -const BodyInvInertiaYY: BodyVars = 38; -const BodyInvInertiaZY: BodyVars = 39; -const BodyInvInertiaXZ: BodyVars = 40; -const BodyInvInertiaYZ: BodyVars = 41; -const BodyInvInertiaZZ: BodyVars = 42; -const BodyRadius: BodyVars = 43; +const BodyHSizeX: BodyVars = 4; +const BodyHSizeY: BodyVars = 5; +const BodyHSizeZ: BodyVars = 6; +const BodyThick: BodyVars = 7; +const BodyMass: BodyVars = 8; +const BodyInvMass: BodyVars = 9; +const BodyBounce: BodyVars = 10; +const BodyFriction: BodyVars = 11; +const BodyFrictionTortion: BodyVars = 12; +const BodyFrictionRolling: BodyVars = 13; +const BodyPosX: BodyVars = 14; +const BodyPosY: BodyVars = 15; +const BodyPosZ: BodyVars = 16; +const BodyQuatX: BodyVars = 17; +const BodyQuatY: BodyVars = 18; +const BodyQuatZ: BodyVars = 19; +const BodyQuatW: BodyVars = 20; +const BodyComX: BodyVars = 21; +const BodyComY: BodyVars = 22; +const BodyComZ: BodyVars = 23; +const BodyInertiaXX: BodyVars = 24; +const BodyInertiaYX: BodyVars = 25; +const BodyInertiaZX: BodyVars = 26; +const BodyInertiaXY: BodyVars = 27; +const BodyInertiaYY: BodyVars = 28; +const BodyInertiaZY: BodyVars = 29; +const BodyInertiaXZ: BodyVars = 30; +const BodyInertiaYZ: BodyVars = 31; +const BodyInertiaZZ: BodyVars = 32; +const BodyInvInertiaXX: BodyVars = 33; +const BodyInvInertiaYX: BodyVars = 34; +const BodyInvInertiaZX: BodyVars = 35; +const BodyInvInertiaXY: BodyVars = 36; +const BodyInvInertiaYY: BodyVars = 37; +const BodyInvInertiaZY: BodyVars = 38; +const BodyInvInertiaXZ: BodyVars = 39; +const BodyInvInertiaYZ: BodyVars = 40; +const BodyInvInertiaZZ: BodyVars = 41; +const BodyRadius: BodyVars = 42; fn BodyCom(idx: i32) -> vec3 { return vec3(Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComX))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComY))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComZ))]); } @@ -137,10 +136,10 @@ const BroadContactVarsN = ContactAPointX; //////// import: "control.go" alias JointControlVars = i32; //enums:enum const JointControlForce: JointControlVars = 0; -const JointTargetStiff: JointControlVars = 1; -const JointTargetPos: JointControlVars = 2; -const JointTargetDamp: JointControlVars = 3; -const JointTargetVel: JointControlVars = 4; +const JointTargetPos: JointControlVars = 1; +const JointTargetStiff: JointControlVars = 2; +const JointTargetVel: JointControlVars = 3; +const JointTargetDamp: JointControlVars = 4; //////// import: "dynamics.go" alias DynamicVars = i32; //enums:enum @@ -200,7 +199,7 @@ fn SetDynamicAngDelta(idx: i32,cni: i32, angDelta: vec3) { } //////// import: "enumgen.go" -const BodyVarsN: BodyVars = 44; +const BodyVarsN: BodyVars = 43; const ContactVarsN: ContactVars = 33; const JointControlVarsN: JointControlVars = 5; const DynamicVarsN: DynamicVars = 33; diff --git a/physics/shaders/StepIntegrateBodies.wgsl b/physics/shaders/StepIntegrateBodies.wgsl index b46be086..c37b4ecf 100644 --- a/physics/shaders/StepIntegrateBodies.wgsl +++ b/physics/shaders/StepIntegrateBodies.wgsl @@ -39,46 +39,45 @@ const BodyShape: BodyVars = 0; const BodyDynamic: BodyVars = 1; const BodyWorld: BodyVars = 2; const BodyGroup: BodyVars = 3; -const BodyObject: BodyVars = 4; -const BodyHSizeX: BodyVars = 5; -const BodyHSizeY: BodyVars = 6; -const BodyHSizeZ: BodyVars = 7; -const BodyThick: BodyVars = 8; -const BodyMass: BodyVars = 9; -const BodyInvMass: BodyVars = 10; -const BodyBounce: BodyVars = 11; -const BodyFriction: BodyVars = 12; -const BodyFrictionTortion: BodyVars = 13; -const BodyFrictionRolling: BodyVars = 14; -const BodyPosX: BodyVars = 15; -const BodyPosY: BodyVars = 16; -const BodyPosZ: BodyVars = 17; -const BodyQuatX: BodyVars = 18; -const BodyQuatY: BodyVars = 19; -const BodyQuatZ: BodyVars = 20; -const BodyQuatW: BodyVars = 21; -const BodyComX: BodyVars = 22; -const BodyComY: BodyVars = 23; -const BodyComZ: BodyVars = 24; -const BodyInertiaXX: BodyVars = 25; -const BodyInertiaYX: BodyVars = 26; -const BodyInertiaZX: BodyVars = 27; -const BodyInertiaXY: BodyVars = 28; -const BodyInertiaYY: BodyVars = 29; -const BodyInertiaZY: BodyVars = 30; -const BodyInertiaXZ: BodyVars = 31; -const BodyInertiaYZ: BodyVars = 32; -const BodyInertiaZZ: BodyVars = 33; -const BodyInvInertiaXX: BodyVars = 34; -const BodyInvInertiaYX: BodyVars = 35; -const BodyInvInertiaZX: BodyVars = 36; -const BodyInvInertiaXY: BodyVars = 37; -const BodyInvInertiaYY: BodyVars = 38; -const BodyInvInertiaZY: BodyVars = 39; -const BodyInvInertiaXZ: BodyVars = 40; -const BodyInvInertiaYZ: BodyVars = 41; -const BodyInvInertiaZZ: BodyVars = 42; -const BodyRadius: BodyVars = 43; +const BodyHSizeX: BodyVars = 4; +const BodyHSizeY: BodyVars = 5; +const BodyHSizeZ: BodyVars = 6; +const BodyThick: BodyVars = 7; +const BodyMass: BodyVars = 8; +const BodyInvMass: BodyVars = 9; +const BodyBounce: BodyVars = 10; +const BodyFriction: BodyVars = 11; +const BodyFrictionTortion: BodyVars = 12; +const BodyFrictionRolling: BodyVars = 13; +const BodyPosX: BodyVars = 14; +const BodyPosY: BodyVars = 15; +const BodyPosZ: BodyVars = 16; +const BodyQuatX: BodyVars = 17; +const BodyQuatY: BodyVars = 18; +const BodyQuatZ: BodyVars = 19; +const BodyQuatW: BodyVars = 20; +const BodyComX: BodyVars = 21; +const BodyComY: BodyVars = 22; +const BodyComZ: BodyVars = 23; +const BodyInertiaXX: BodyVars = 24; +const BodyInertiaYX: BodyVars = 25; +const BodyInertiaZX: BodyVars = 26; +const BodyInertiaXY: BodyVars = 27; +const BodyInertiaYY: BodyVars = 28; +const BodyInertiaZY: BodyVars = 29; +const BodyInertiaXZ: BodyVars = 30; +const BodyInertiaYZ: BodyVars = 31; +const BodyInertiaZZ: BodyVars = 32; +const BodyInvInertiaXX: BodyVars = 33; +const BodyInvInertiaYX: BodyVars = 34; +const BodyInvInertiaZX: BodyVars = 35; +const BodyInvInertiaXY: BodyVars = 36; +const BodyInvInertiaYY: BodyVars = 37; +const BodyInvInertiaZY: BodyVars = 38; +const BodyInvInertiaXZ: BodyVars = 39; +const BodyInvInertiaYZ: BodyVars = 40; +const BodyInvInertiaZZ: BodyVars = 41; +const BodyRadius: BodyVars = 42; fn BodyCom(idx: i32) -> vec3 { return vec3(Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComX))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComY))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComZ))]); } @@ -133,10 +132,10 @@ const BroadContactVarsN = ContactAPointX; //////// import: "control.go" alias JointControlVars = i32; //enums:enum const JointControlForce: JointControlVars = 0; -const JointTargetStiff: JointControlVars = 1; -const JointTargetPos: JointControlVars = 2; -const JointTargetDamp: JointControlVars = 3; -const JointTargetVel: JointControlVars = 4; +const JointTargetPos: JointControlVars = 1; +const JointTargetStiff: JointControlVars = 2; +const JointTargetVel: JointControlVars = 3; +const JointTargetDamp: JointControlVars = 4; //////// import: "dynamics.go" alias DynamicVars = i32; //enums:enum @@ -186,7 +185,7 @@ fn DynamicAngDelta(idx: i32,cni: i32) -> vec3 { return vec3(Dynamics[I fn SetDynamicAngDelta(idx: i32,cni: i32, angDelta: vec3) { Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaX))] = angDelta.x;; Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaY))] = angDelta.y;; Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaZ))] = angDelta.z; } //////// import: "enumgen.go" -const BodyVarsN: BodyVars = 44; +const BodyVarsN: BodyVars = 43; const ContactVarsN: ContactVars = 33; const JointControlVarsN: JointControlVars = 5; const DynamicVarsN: DynamicVars = 33; diff --git a/physics/shaders/StepJointForces.wgsl b/physics/shaders/StepJointForces.wgsl index 8c80837f..9366b781 100644 --- a/physics/shaders/StepJointForces.wgsl +++ b/physics/shaders/StepJointForces.wgsl @@ -45,46 +45,45 @@ const BodyShape: BodyVars = 0; const BodyDynamic: BodyVars = 1; const BodyWorld: BodyVars = 2; const BodyGroup: BodyVars = 3; -const BodyObject: BodyVars = 4; -const BodyHSizeX: BodyVars = 5; -const BodyHSizeY: BodyVars = 6; -const BodyHSizeZ: BodyVars = 7; -const BodyThick: BodyVars = 8; -const BodyMass: BodyVars = 9; -const BodyInvMass: BodyVars = 10; -const BodyBounce: BodyVars = 11; -const BodyFriction: BodyVars = 12; -const BodyFrictionTortion: BodyVars = 13; -const BodyFrictionRolling: BodyVars = 14; -const BodyPosX: BodyVars = 15; -const BodyPosY: BodyVars = 16; -const BodyPosZ: BodyVars = 17; -const BodyQuatX: BodyVars = 18; -const BodyQuatY: BodyVars = 19; -const BodyQuatZ: BodyVars = 20; -const BodyQuatW: BodyVars = 21; -const BodyComX: BodyVars = 22; -const BodyComY: BodyVars = 23; -const BodyComZ: BodyVars = 24; -const BodyInertiaXX: BodyVars = 25; -const BodyInertiaYX: BodyVars = 26; -const BodyInertiaZX: BodyVars = 27; -const BodyInertiaXY: BodyVars = 28; -const BodyInertiaYY: BodyVars = 29; -const BodyInertiaZY: BodyVars = 30; -const BodyInertiaXZ: BodyVars = 31; -const BodyInertiaYZ: BodyVars = 32; -const BodyInertiaZZ: BodyVars = 33; -const BodyInvInertiaXX: BodyVars = 34; -const BodyInvInertiaYX: BodyVars = 35; -const BodyInvInertiaZX: BodyVars = 36; -const BodyInvInertiaXY: BodyVars = 37; -const BodyInvInertiaYY: BodyVars = 38; -const BodyInvInertiaZY: BodyVars = 39; -const BodyInvInertiaXZ: BodyVars = 40; -const BodyInvInertiaYZ: BodyVars = 41; -const BodyInvInertiaZZ: BodyVars = 42; -const BodyRadius: BodyVars = 43; +const BodyHSizeX: BodyVars = 4; +const BodyHSizeY: BodyVars = 5; +const BodyHSizeZ: BodyVars = 6; +const BodyThick: BodyVars = 7; +const BodyMass: BodyVars = 8; +const BodyInvMass: BodyVars = 9; +const BodyBounce: BodyVars = 10; +const BodyFriction: BodyVars = 11; +const BodyFrictionTortion: BodyVars = 12; +const BodyFrictionRolling: BodyVars = 13; +const BodyPosX: BodyVars = 14; +const BodyPosY: BodyVars = 15; +const BodyPosZ: BodyVars = 16; +const BodyQuatX: BodyVars = 17; +const BodyQuatY: BodyVars = 18; +const BodyQuatZ: BodyVars = 19; +const BodyQuatW: BodyVars = 20; +const BodyComX: BodyVars = 21; +const BodyComY: BodyVars = 22; +const BodyComZ: BodyVars = 23; +const BodyInertiaXX: BodyVars = 24; +const BodyInertiaYX: BodyVars = 25; +const BodyInertiaZX: BodyVars = 26; +const BodyInertiaXY: BodyVars = 27; +const BodyInertiaYY: BodyVars = 28; +const BodyInertiaZY: BodyVars = 29; +const BodyInertiaXZ: BodyVars = 30; +const BodyInertiaYZ: BodyVars = 31; +const BodyInertiaZZ: BodyVars = 32; +const BodyInvInertiaXX: BodyVars = 33; +const BodyInvInertiaYX: BodyVars = 34; +const BodyInvInertiaZX: BodyVars = 35; +const BodyInvInertiaXY: BodyVars = 36; +const BodyInvInertiaYY: BodyVars = 37; +const BodyInvInertiaZY: BodyVars = 38; +const BodyInvInertiaXZ: BodyVars = 39; +const BodyInvInertiaYZ: BodyVars = 40; +const BodyInvInertiaZZ: BodyVars = 41; +const BodyRadius: BodyVars = 42; fn BodyCom(idx: i32) -> vec3 { return vec3(Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComX))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComY))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComZ))]); } @@ -129,10 +128,10 @@ const BroadContactVarsN = ContactAPointX; //////// import: "control.go" alias JointControlVars = i32; //enums:enum const JointControlForce: JointControlVars = 0; -const JointTargetStiff: JointControlVars = 1; -const JointTargetPos: JointControlVars = 2; -const JointTargetDamp: JointControlVars = 3; -const JointTargetVel: JointControlVars = 4; +const JointTargetPos: JointControlVars = 1; +const JointTargetStiff: JointControlVars = 2; +const JointTargetVel: JointControlVars = 3; +const JointTargetDamp: JointControlVars = 4; fn JointControl(idx: i32,dof: i32, vr: JointControlVars) -> f32 { return JointControls[Index2D(TensorStrides[100], TensorStrides[101], u32(JointDoFIndex(idx, dof)), u32(vr))]; } @@ -177,7 +176,7 @@ fn DynamicPos(idx: i32,cni: i32) -> vec3 { return vec3(Dynamics[Index3 fn DynamicQuat(idx: i32,cni: i32) -> vec4 { return vec4(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatZ))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatW))]); } //////// import: "enumgen.go" -const BodyVarsN: BodyVars = 44; +const BodyVarsN: BodyVars = 43; const ContactVarsN: ContactVars = 33; const JointControlVarsN: JointControlVars = 5; const DynamicVarsN: DynamicVars = 33; diff --git a/physics/shaders/StepSolveJoints.wgsl b/physics/shaders/StepSolveJoints.wgsl index 09d65e4e..b866044f 100644 --- a/physics/shaders/StepSolveJoints.wgsl +++ b/physics/shaders/StepSolveJoints.wgsl @@ -45,46 +45,45 @@ const BodyShape: BodyVars = 0; const BodyDynamic: BodyVars = 1; const BodyWorld: BodyVars = 2; const BodyGroup: BodyVars = 3; -const BodyObject: BodyVars = 4; -const BodyHSizeX: BodyVars = 5; -const BodyHSizeY: BodyVars = 6; -const BodyHSizeZ: BodyVars = 7; -const BodyThick: BodyVars = 8; -const BodyMass: BodyVars = 9; -const BodyInvMass: BodyVars = 10; -const BodyBounce: BodyVars = 11; -const BodyFriction: BodyVars = 12; -const BodyFrictionTortion: BodyVars = 13; -const BodyFrictionRolling: BodyVars = 14; -const BodyPosX: BodyVars = 15; -const BodyPosY: BodyVars = 16; -const BodyPosZ: BodyVars = 17; -const BodyQuatX: BodyVars = 18; -const BodyQuatY: BodyVars = 19; -const BodyQuatZ: BodyVars = 20; -const BodyQuatW: BodyVars = 21; -const BodyComX: BodyVars = 22; -const BodyComY: BodyVars = 23; -const BodyComZ: BodyVars = 24; -const BodyInertiaXX: BodyVars = 25; -const BodyInertiaYX: BodyVars = 26; -const BodyInertiaZX: BodyVars = 27; -const BodyInertiaXY: BodyVars = 28; -const BodyInertiaYY: BodyVars = 29; -const BodyInertiaZY: BodyVars = 30; -const BodyInertiaXZ: BodyVars = 31; -const BodyInertiaYZ: BodyVars = 32; -const BodyInertiaZZ: BodyVars = 33; -const BodyInvInertiaXX: BodyVars = 34; -const BodyInvInertiaYX: BodyVars = 35; -const BodyInvInertiaZX: BodyVars = 36; -const BodyInvInertiaXY: BodyVars = 37; -const BodyInvInertiaYY: BodyVars = 38; -const BodyInvInertiaZY: BodyVars = 39; -const BodyInvInertiaXZ: BodyVars = 40; -const BodyInvInertiaYZ: BodyVars = 41; -const BodyInvInertiaZZ: BodyVars = 42; -const BodyRadius: BodyVars = 43; +const BodyHSizeX: BodyVars = 4; +const BodyHSizeY: BodyVars = 5; +const BodyHSizeZ: BodyVars = 6; +const BodyThick: BodyVars = 7; +const BodyMass: BodyVars = 8; +const BodyInvMass: BodyVars = 9; +const BodyBounce: BodyVars = 10; +const BodyFriction: BodyVars = 11; +const BodyFrictionTortion: BodyVars = 12; +const BodyFrictionRolling: BodyVars = 13; +const BodyPosX: BodyVars = 14; +const BodyPosY: BodyVars = 15; +const BodyPosZ: BodyVars = 16; +const BodyQuatX: BodyVars = 17; +const BodyQuatY: BodyVars = 18; +const BodyQuatZ: BodyVars = 19; +const BodyQuatW: BodyVars = 20; +const BodyComX: BodyVars = 21; +const BodyComY: BodyVars = 22; +const BodyComZ: BodyVars = 23; +const BodyInertiaXX: BodyVars = 24; +const BodyInertiaYX: BodyVars = 25; +const BodyInertiaZX: BodyVars = 26; +const BodyInertiaXY: BodyVars = 27; +const BodyInertiaYY: BodyVars = 28; +const BodyInertiaZY: BodyVars = 29; +const BodyInertiaXZ: BodyVars = 30; +const BodyInertiaYZ: BodyVars = 31; +const BodyInertiaZZ: BodyVars = 32; +const BodyInvInertiaXX: BodyVars = 33; +const BodyInvInertiaYX: BodyVars = 34; +const BodyInvInertiaZX: BodyVars = 35; +const BodyInvInertiaXY: BodyVars = 36; +const BodyInvInertiaYY: BodyVars = 37; +const BodyInvInertiaZY: BodyVars = 38; +const BodyInvInertiaXZ: BodyVars = 39; +const BodyInvInertiaYZ: BodyVars = 40; +const BodyInvInertiaZZ: BodyVars = 41; +const BodyRadius: BodyVars = 42; fn BodyCom(idx: i32) -> vec3 { return vec3(Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComX))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComY))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComZ))]); } @@ -134,10 +133,10 @@ const BroadContactVarsN = ContactAPointX; //////// import: "control.go" alias JointControlVars = i32; //enums:enum const JointControlForce: JointControlVars = 0; -const JointTargetStiff: JointControlVars = 1; -const JointTargetPos: JointControlVars = 2; -const JointTargetDamp: JointControlVars = 3; -const JointTargetVel: JointControlVars = 4; +const JointTargetPos: JointControlVars = 1; +const JointTargetStiff: JointControlVars = 2; +const JointTargetVel: JointControlVars = 3; +const JointTargetDamp: JointControlVars = 4; fn JointControl(idx: i32,dof: i32, vr: JointControlVars) -> f32 { return JointControls[Index2D(TensorStrides[100], TensorStrides[101], u32(JointDoFIndex(idx, dof)), u32(vr))]; } @@ -188,7 +187,7 @@ fn DynamicAngDelta(idx: i32,cni: i32) -> vec3 { } //////// import: "enumgen.go" -const BodyVarsN: BodyVars = 44; +const BodyVarsN: BodyVars = 43; const ContactVarsN: ContactVars = 33; const JointControlVarsN: JointControlVars = 5; const DynamicVarsN: DynamicVars = 33; diff --git a/physics/typegen.go b/physics/typegen.go index 32123aaa..9edc8c33 100644 --- a/physics/typegen.go +++ b/physics/typegen.go @@ -22,7 +22,7 @@ var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.JointVars", var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.JointDoFVars", IDName: "joint-do-f-vars", Doc: "JointDoFVars are joint DoF state variables stored in tensor.Float32,\none for each DoF."}) -var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.Model", IDName: "model", Doc: "Model contains and manages all of the physics elements.", Directives: []types.Directive{{Tool: "go", Directive: "generate", Args: []string{"core", "generate", "-add-types", "-gosl"}}}, Fields: []types.Field{{Name: "GPU", Doc: "GPU determines whether to use GPU (else CPU)."}, {Name: "Params", Doc: "Params are global parameters."}, {Name: "CurrentWorld", Doc: "CurrentWorld is the [BodyWorld] value to use when creating new bodies.\nSet to -1 to create global elements that interact with everything,\nwhile 0 and positive numbers only interact amongst themselves."}, {Name: "ReplicasStart", Doc: "ReplicasStart is the starting body index for replicated world bodies,\nwhich is needed for viewers to efficiently select a specific world to view.\nThis is the start of the World=0 first instance."}, {Name: "ReplicasN", Doc: "ReplicasN is the number of body elements within each set of\nreplicated world bodies, which is needed for viewers to efficiently select\na specific world to view."}, {Name: "CurrentObject", Doc: "CurrentObject is the [BodyObject] value to use when creating new bodies.\nGenerally just increment when starting a new object. When using world replicas,\nthe object indexes are copied, so objects are indexed by world and object id."}, {Name: "Bodies", Doc: "Bodies are the rigid body elements (dynamic and static),\nspecifying the constant, non-dynamic properties,\nwhich is initial state for dynamics.\n[body][BodyVarsN]"}, {Name: "Joints", Doc: "Joints is a list of permanent joints connecting bodies,\nwhich do not change (no dynamic variables).\n[joint][JointVarsN]"}, {Name: "JointDoFs", Doc: "JointDoFs is a list of joint DoF parameters, allocated per joint.\n[dof][JointDoFVars]"}, {Name: "BodyJoints", Doc: "BodyJoints is a list of joint indexes for each dynamic body, for aggregating.\n[dyn body][parent, child][Params.BodyJointsMax]"}, {Name: "BodyCollidePairs", Doc: "BodyCollidePairs are pairs of Body indexes that could potentially collide\nbased on precomputed collision logic, using World, Group, and Joint indexes.\n[BodyCollidePairsN][2]"}, {Name: "Dynamics", Doc: "Dynamics are the dynamic rigid body elements: these actually move.\nThe first set of variables are for initial values, and the second current.\n[body][cur/next][DynamicVarsN]"}, {Name: "BroadContactsN", Doc: "BroadContactsN has number of points of broad contact\nbetween bodies. [1]"}, {Name: "BroadContacts", Doc: "BroadContacts are the results of broad-phase contact processing,\nestablishing possible points of contact between bodies.\n[ContactsMax][BroadContactVarsN]"}, {Name: "ContactsN", Doc: "ContactsN has number of points of narrow (final) contact\nbetween bodies. [1]"}, {Name: "Contacts", Doc: "Contacts are the results of narrow-phase contact processing,\nwhere only actual contacts with fully-specified values are present.\n[ContactsMax][ContactVarsN]"}, {Name: "JointControls", Doc: "JointControls are dynamic joint control inputs, per joint DoF\n(in correspondence with [JointDoFs]). This can be uploaded to the\nGPU at every step.\n[dof][JointControlVarsN]"}}}) +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.Model", IDName: "model", Doc: "Model contains and manages all of the physics elements.", Directives: []types.Directive{{Tool: "go", Directive: "generate", Args: []string{"core", "generate", "-add-types", "-gosl"}}}, Fields: []types.Field{{Name: "GPU", Doc: "GPU determines whether to use GPU (else CPU)."}, {Name: "Params", Doc: "Params are global parameters."}, {Name: "CurrentWorld", Doc: "CurrentWorld is the [BodyWorld] value to use when creating new bodies.\nSet to -1 to create global elements that interact with everything,\nwhile 0 and positive numbers only interact amongst themselves."}, {Name: "ReplicasStart", Doc: "ReplicasStart is the starting body index for replicated world bodies,\nwhich is needed for viewers to efficiently select a specific world to view.\nThis is the start of the World=0 first instance."}, {Name: "ReplicasN", Doc: "ReplicasN is the number of body elements within each set of\nreplicated world bodies, which is needed for viewers to efficiently select\na specific world to view."}, {Name: "Bodies", Doc: "Bodies are the rigid body elements (dynamic and static),\nspecifying the constant, non-dynamic properties,\nwhich is initial state for dynamics.\n[body][BodyVarsN]"}, {Name: "Joints", Doc: "Joints is a list of permanent joints connecting bodies,\nwhich do not change (no dynamic variables).\n[joint][JointVarsN]"}, {Name: "JointDoFs", Doc: "JointDoFs is a list of joint DoF parameters, allocated per joint.\n[dof][JointDoFVars]"}, {Name: "BodyJoints", Doc: "BodyJoints is a list of joint indexes for each dynamic body, for aggregating.\n[dyn body][parent, child][Params.BodyJointsMax]"}, {Name: "BodyCollidePairs", Doc: "BodyCollidePairs are pairs of Body indexes that could potentially collide\nbased on precomputed collision logic, using World, Group, and Joint indexes.\n[BodyCollidePairsN][2]"}, {Name: "Dynamics", Doc: "Dynamics are the dynamic rigid body elements: these actually move.\nThe first set of variables are for initial values, and the second current.\n[body][cur/next][DynamicVarsN]"}, {Name: "BroadContactsN", Doc: "BroadContactsN has number of points of broad contact\nbetween bodies. [1]"}, {Name: "BroadContacts", Doc: "BroadContacts are the results of broad-phase contact processing,\nestablishing possible points of contact between bodies.\n[ContactsMax][BroadContactVarsN]"}, {Name: "ContactsN", Doc: "ContactsN has number of points of narrow (final) contact\nbetween bodies. [1]"}, {Name: "Contacts", Doc: "Contacts are the results of narrow-phase contact processing,\nwhere only actual contacts with fully-specified values are present.\n[ContactsMax][ContactVarsN]"}, {Name: "JointControls", Doc: "JointControls are dynamic joint control inputs, per joint DoF\n(in correspondence with [JointDoFs]). This can be uploaded to the\nGPU at every step.\n[dof][JointControlVarsN]"}}}) var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.PhysParams", IDName: "phys-params", Doc: "PhysParams are the physics parameters", Directives: []types.Directive{{Tool: "gosl", Directive: "start"}}, Fields: []types.Field{{Name: "Iterations", Doc: "Iterations is the number of integration iterations to perform\nwithin each solver step. Muller et al (2020) report that 1 is best."}, {Name: "Dt", Doc: "Dt is the integration stepsize.\nFor highly kinetic situations (e.g., rapidly moving bouncing balls)\n0.0001 is needed to ensure contact registration. Use SubSteps to\naccomplish a target effective read-out step size."}, {Name: "SubSteps", Doc: "SubSteps is the number of integration steps to take per Step()\nfunction call. These sub steps are taken without any sync to/from\nthe GPU and are therefore much faster."}, {Name: "ContactMargin", Doc: "Contact margin is the extra distance for broadphase collision\naround rigid bodies. This can make some joints potentially unstable if > 0"}, {Name: "ContactRelax", Doc: "ContactRelax is rigid contact relaxation constant.\nHigher values cause errros"}, {Name: "ContactWeighting", Doc: "Contact weighting: balances contact forces?"}, {Name: "Restitution", Doc: "Restitution takes into account bounciness of objects."}, {Name: "JointLinearRelax", Doc: "JointLinearRelax is joint linear relaxation constant."}, {Name: "JointAngularRelax", Doc: "JointAngularRelax is joint angular relaxation constant."}, {Name: "JointLinearComply", Doc: "JointLinearComply is joint linear compliance constant."}, {Name: "JointAngularComply", Doc: "JointAngularComply is joint angular compliance constant."}, {Name: "AngularDamping", Doc: "AngularDamping is damping of angular motion."}, {Name: "SoftRelax", Doc: "SoftRelax is soft-body relaxation constant."}, {Name: "MaxGeomIter", Doc: "MaxGeomIter is number of iterations to perform in shape-based\ngeometry collision computations"}, {Name: "ContactsMax", Doc: "Maximum number of contacts to process at any given point."}, {Name: "Cur", Doc: "Index for the current state (0 or 1, alternates with Next)."}, {Name: "Next", Doc: "Index for the next state (1 or 0, alternates with Cur)."}, {Name: "BodiesN", Doc: "BodiesN is number of rigid bodies."}, {Name: "DynamicsN", Doc: "DynamicsN is number of dynamics bodies."}, {Name: "JointsN", Doc: "JointsN is number of joints."}, {Name: "JointDoFsN", Doc: "JointDoFsN is number of joint DoFs."}, {Name: "BodyJointsMax", Doc: "BodyJointsMax is max number of joints per body + 1 for actual n."}, {Name: "BodyCollidePairsN", Doc: "BodyCollidePairsN is the total number of pre-compiled collision pairs\nto examine."}, {Name: "pad"}, {Name: "Gravity", Doc: "Gravity is the gravity acceleration function"}}}) From 7f2907df58b62604d66163a17fac42ba1664828d Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Sat, 27 Dec 2025 09:21:01 +0100 Subject: [PATCH 57/97] physics: add docs --- physics/README.md | 4 ++- physics/builder/README.md | 4 +++ physics/builder/doc.go | 9 +++++++ physics/doc.go | 9 +++++++ .../cogentcore_org-lab-physics-builder.go | 25 +++++++++++++++++++ .../cogentcore_org-lab-physics-phyxyz.go | 2 +- .../labsymbols/cogentcore_org-lab-physics.go | 4 +-- yaegilab/labsymbols/make | 2 +- 8 files changed, 53 insertions(+), 6 deletions(-) create mode 100644 physics/builder/README.md create mode 100644 physics/builder/doc.go create mode 100644 physics/doc.go create mode 100644 yaegilab/labsymbols/cogentcore_org-lab-physics-builder.go diff --git a/physics/README.md b/physics/README.md index 75fe4998..90db1e8a 100644 --- a/physics/README.md +++ b/physics/README.md @@ -4,7 +4,9 @@ The `physics` engine is a 3D physics simulator for creating virtual environments See [physics docs](https://cogentcore.org/lab/physics) for the main docs. -The [phyxyz](phyxyz) ("physics") visualization sub-package manages a `View` element that links to physics bodies and generates an [xyz](https://cogentcore.org/core/xyz) 3D scenegraph based on the physics bodies, and updates this visualization efficiently as the physics is updated. There is an `Editor` widget that makes it easy to explore physics sims. +The [phyxyz](phyxyz) ("physics") visualization sub-package manages a `Skin` element that links to physics bodies and generates an [xyz](https://cogentcore.org/core/xyz) 3D scenegraph based on the physics bodies, and updates this visualization efficiently as the physics is updated. There is an `Editor` widget that makes it easy to explore physics Models. + +The [builder](builder) package provides a structured, hierarchical description of a `physics.Model` that supports replicating worlds for parallel world execution, and easier manipulation of objects as collections of bodies (e.g., an entire object can be moved and re-oriented in one call). ## TODO diff --git a/physics/builder/README.md b/physics/builder/README.md new file mode 100644 index 00000000..91299f07 --- /dev/null +++ b/physics/builder/README.md @@ -0,0 +1,4 @@ +# physics.builder + +The `physics/builder` package provides a structured, hierarchical description of a `physics.Model` that supports replicating worlds for parallel world execution, and easier manipulation of objects as collections of bodies (e.g., an entire object can be moved and re-oriented in one call). + diff --git a/physics/builder/doc.go b/physics/builder/doc.go new file mode 100644 index 00000000..c7204d7a --- /dev/null +++ b/physics/builder/doc.go @@ -0,0 +1,9 @@ +// Copyright (c) 2025, Cogent Core. All rights reserved. +// Use of this source code is governed by a BSD-style +// license that can be found in the LICENSE file. + +// Package builder provides a structured, hierarchical description of +// a [physics.Model] that supports replicating worlds for parallel world +// execution, and easier manipulation of objects as collections of bodies +// (e.g., an entire object can be moved and re-oriented in one call).. +package builder diff --git a/physics/doc.go b/physics/doc.go new file mode 100644 index 00000000..3ed480cc --- /dev/null +++ b/physics/doc.go @@ -0,0 +1,9 @@ +// Copyright (c) 2025, Cogent Core. All rights reserved. +// Use of this source code is governed by a BSD-style +// license that can be found in the LICENSE file. + +// Package physics is a 3D physics simulator for creating virtual +// environments, which can run on the GPU or CPU using GoSL: +// https://cogentcore.org/lab/gosl +// See https://cogentcore.org/lab/physics for the main docs. +package physics diff --git a/yaegilab/labsymbols/cogentcore_org-lab-physics-builder.go b/yaegilab/labsymbols/cogentcore_org-lab-physics-builder.go new file mode 100644 index 00000000..8c7aeaa4 --- /dev/null +++ b/yaegilab/labsymbols/cogentcore_org-lab-physics-builder.go @@ -0,0 +1,25 @@ +// Code generated by 'yaegi extract cogentcore.org/lab/physics/builder'. DO NOT EDIT. + +package labsymbols + +import ( + "cogentcore.org/lab/physics/builder" + "reflect" +) + +func init() { + Symbols["cogentcore.org/lab/physics/builder/builder"] = map[string]reflect.Value{ + // function, constant and variable definitions + "MakePoseToolbar": reflect.ValueOf(builder.MakePoseToolbar), + "NewBuilder": reflect.ValueOf(builder.NewBuilder), + + // type definitions + "Body": reflect.ValueOf((*builder.Body)(nil)), + "Builder": reflect.ValueOf((*builder.Builder)(nil)), + "DoF": reflect.ValueOf((*builder.DoF)(nil)), + "Joint": reflect.ValueOf((*builder.Joint)(nil)), + "Object": reflect.ValueOf((*builder.Object)(nil)), + "Pose": reflect.ValueOf((*builder.Pose)(nil)), + "World": reflect.ValueOf((*builder.World)(nil)), + } +} diff --git a/yaegilab/labsymbols/cogentcore_org-lab-physics-phyxyz.go b/yaegilab/labsymbols/cogentcore_org-lab-physics-phyxyz.go index 127a1718..cd80b3be 100644 --- a/yaegilab/labsymbols/cogentcore_org-lab-physics-phyxyz.go +++ b/yaegilab/labsymbols/cogentcore_org-lab-physics-phyxyz.go @@ -20,6 +20,6 @@ func init() { "Camera": reflect.ValueOf((*phyxyz.Camera)(nil)), "Editor": reflect.ValueOf((*phyxyz.Editor)(nil)), "Scene": reflect.ValueOf((*phyxyz.Scene)(nil)), - "View": reflect.ValueOf((*phyxyz.View)(nil)), + "Skin": reflect.ValueOf((*phyxyz.Skin)(nil)), } } diff --git a/yaegilab/labsymbols/cogentcore_org-lab-physics.go b/yaegilab/labsymbols/cogentcore_org-lab-physics.go index b55cf7f4..aab01faa 100644 --- a/yaegilab/labsymbols/cogentcore_org-lab-physics.go +++ b/yaegilab/labsymbols/cogentcore_org-lab-physics.go @@ -59,7 +59,6 @@ func init() { "BodyJoints": reflect.ValueOf(&physics.BodyJoints).Elem(), "BodyJointsVar": reflect.ValueOf(physics.BodyJointsVar), "BodyMass": reflect.ValueOf(physics.BodyMass), - "BodyObject": reflect.ValueOf(physics.BodyObject), "BodyPos": reflect.ValueOf(physics.BodyPos), "BodyPosX": reflect.ValueOf(physics.BodyPosX), "BodyPosY": reflect.ValueOf(physics.BodyPosY), @@ -223,7 +222,6 @@ func init() { "GPUVarsValues": reflect.ValueOf(physics.GPUVarsValues), "GetBodyDynamic": reflect.ValueOf(physics.GetBodyDynamic), "GetBodyGroup": reflect.ValueOf(physics.GetBodyGroup), - "GetBodyObject": reflect.ValueOf(physics.GetBodyObject), "GetBodyShape": reflect.ValueOf(physics.GetBodyShape), "GetBodyWorld": reflect.ValueOf(physics.GetBodyWorld), "GetBroadContactA": reflect.ValueOf(physics.GetBroadContactA), @@ -408,10 +406,10 @@ func init() { "SetBodyHSize": reflect.ValueOf(physics.SetBodyHSize), "SetBodyInertia": reflect.ValueOf(physics.SetBodyInertia), "SetBodyInvInertia": reflect.ValueOf(physics.SetBodyInvInertia), - "SetBodyObject": reflect.ValueOf(physics.SetBodyObject), "SetBodyPos": reflect.ValueOf(physics.SetBodyPos), "SetBodyQuat": reflect.ValueOf(physics.SetBodyQuat), "SetBodyShape": reflect.ValueOf(physics.SetBodyShape), + "SetBodyThick": reflect.ValueOf(physics.SetBodyThick), "SetBodyWorld": reflect.ValueOf(physics.SetBodyWorld), "SetBroadContactA": reflect.ValueOf(physics.SetBroadContactA), "SetBroadContactB": reflect.ValueOf(physics.SetBroadContactB), diff --git a/yaegilab/labsymbols/make b/yaegilab/labsymbols/make index d658834e..f78f4426 100755 --- a/yaegilab/labsymbols/make +++ b/yaegilab/labsymbols/make @@ -6,5 +6,5 @@ command extract { } } -extract physics physics/phyxyz plot plot/plots plotcore tensorcore lab +extract physics physics/phyxyz physics/builder plot plot/plots plotcore tensorcore lab From 8ad1bc5fc7542a2790cfffc37394da94f5e06d2d Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Sun, 28 Dec 2025 07:38:05 +0100 Subject: [PATCH 58/97] physics: builder replicate and transform working --- gosl/slmath/quaternion.go | 2 +- physics/builder/body.go | 18 ++++++++----- physics/builder/builder.go | 34 +++++++++++++++++++++++- physics/builder/joint.go | 13 +++++++++ physics/builder/object.go | 41 +++++++++++++++++++++++++++++ physics/builder/pose.go | 6 +++++ physics/builder/world.go | 31 ++++++++++++++++++++++ physics/examples/pendula/pendula.go | 5 ++-- physics/phyxyz/skin.go | 1 + 9 files changed, 141 insertions(+), 10 deletions(-) diff --git a/gosl/slmath/quaternion.go b/gosl/slmath/quaternion.go index 68f978f1..a753ef39 100644 --- a/gosl/slmath/quaternion.go +++ b/gosl/slmath/quaternion.go @@ -59,7 +59,7 @@ func MulQuats(a, b math32.Quat) math32.Quat { return q } -// MulQRTransforms computes the equivalent of matrix multiplication for +// MulSpatialTransforms computes the equivalent of matrix multiplication for // two quat-point spatial transforms: o = a * b func MulSpatialTransforms(aP math32.Vector3, aQ math32.Quat, bP math32.Vector3, bQ math32.Quat, oP *math32.Vector3, oQ *math32.Quat) { // rotate b by a and add a diff --git a/physics/builder/body.go b/physics/builder/body.go index 4c7f8219..7aa91d6c 100644 --- a/physics/builder/body.go +++ b/physics/builder/body.go @@ -107,11 +107,17 @@ func (ob *Object) NewDynamic(shape physics.Shapes, mass float32, hsize, pos math // Use this for Static elements; NewDynamicSkin for dynamic elements. func (ob *Object) NewBodySkin(sc *phyxyz.Scene, name string, shape physics.Shapes, clr string, hsize, pos math32.Vector3, rot math32.Quat) *Body { bd := ob.NewBody(shape, hsize, pos, rot) - sk := sc.NewSkin(shape, name, clr, hsize, pos, rot) - bd.Skin = sk + bd.NewSkin(sc, name, clr) return bd } +// NewSkin adds a new skin for body with given name and color parameters. +func (bd *Body) NewSkin(sc *phyxyz.Scene, name string, clr string) *phyxyz.Skin { + sk := sc.NewSkin(bd.Shape, name, clr, bd.HSize, bd.Pose.Pos, bd.Pose.Quat) + bd.Skin = sk + return sk +} + // NewDynamicSkin adds a new dynamic body with given parameters, // including name and color parameters used for intializing a [phyxyz.Skin] // in given [phyxyz.Scene]. @@ -129,15 +135,15 @@ func (bd *Body) NewPhysicsBody(ml *physics.Model, world int) { var bi, di int32 if bd.Dynamic { bi, di = ml.NewDynamic(bd.Shape, bd.Mass, bd.HSize, bd.Pose.Pos, bd.Pose.Quat) - bd.BodyIndex = bi - bd.DynamicIndex = di } else { bi = ml.NewBody(bd.Shape, bd.HSize, bd.Pose.Pos, bd.Pose.Quat) - bd.DynamicIndex = -1 - bd.BodyIndex = bi + di = -1 } + bd.BodyIndex = bi + bd.DynamicIndex = di physics.SetBodyWorld(bi, int32(world)) physics.SetBodyGroup(bi, int32(bd.Group)) + // fmt.Println("\t\t", bi, di, bd.Pose.Pos, bd.Pose.Quat) if bd.Skin != nil { bd.Skin.BodyIndex = bi bd.Skin.DynamicIndex = di diff --git a/physics/builder/builder.go b/physics/builder/builder.go index efa69cb5..10991a81 100644 --- a/physics/builder/builder.go +++ b/physics/builder/builder.go @@ -7,6 +7,7 @@ package builder //go:generate core generate -add-types -setters import ( + "cogentcore.org/core/math32" "cogentcore.org/lab/physics" "cogentcore.org/lab/physics/phyxyz" ) @@ -46,7 +47,7 @@ func (bl *Builder) NewWorld() *World { wn := 0 idx := len(bl.Worlds) if idx > 0 { - wn = bl.Worlds[idx-1].World + wn = bl.Worlds[idx-1].World + 1 } bl.Worlds = append(bl.Worlds, World{World: wn}) return &bl.Worlds[idx] @@ -57,8 +58,10 @@ func (bl *Builder) NewWorld() *World { func (bl *Builder) Build(ml *physics.Model, sc *phyxyz.Scene) { for wi := range bl.Worlds { wl := bl.World(wi) + // fmt.Println("\n######## World:", wl.World) for oi := range wl.Objects { ob := wl.Object(oi) + // fmt.Println("\n\t#### Object") for bbi := range ob.Bodies { bd := ob.Body(bbi) bd.NewPhysicsBody(ml, wl.World) @@ -70,3 +73,32 @@ func (bl *Builder) Build(ml *physics.Model, sc *phyxyz.Scene) { } } } + +// ReplicateWorld makes copies of given world to form an X,Y grid of +// worlds with given offsets added between world objects. Note that +// worldIdx is the index in Worlds, not the world number. +// Because different worlds do not interact, offsets are not necessary +// and can potentially affect numerical accuracy. Offsets can also be +// established purely in phyxyz.Scene viewing. +// If the given [phyxyz.Scene] is non-nil, then new skins will be made +// for the replicated bodies (else not). +func (bl *Builder) ReplicateWorld(sc *phyxyz.Scene, worldIdx, nY, nX int, Yoff, Xoff math32.Vector3) { + rot := math32.NewQuat(0, 0, 0, 1) + src := bl.World(worldIdx) + for y := range nY { + for x := range nX { + if x == 0 && y == 0 { + continue + } + nw := bl.NewWorld() + wi := nw.World + nw.Copy(src) + nw.World = wi + off := Yoff.MulScalar(float32(y)).Add(Xoff.MulScalar(float32(x))) + nw.Transform(off, rot) + if sc != nil { + nw.CopySkins(sc, src) + } + } + } +} diff --git a/physics/builder/joint.go b/physics/builder/joint.go index e9680f8e..31c21954 100644 --- a/physics/builder/joint.go +++ b/physics/builder/joint.go @@ -13,6 +13,7 @@ import ( // Joint describes a joint between two bodies. type Joint struct { // Parent is index within an Object for parent body. + // -1 for world-anchored parent. Parent int // Parent is index within an Object for parent body. @@ -212,5 +213,17 @@ func (jd *Joint) NewPhysicsJoint(ml *physics.Model, ob *Object) int32 { physics.SetJointTargetVel(ji, di, d.TargetVel, d.TargetDamp) } jd.JointIndex = ji + // fmt.Println("\t\tjoint:", pdi, cdi, jd.Type) + // if pdi < 0 { + // fmt.Println("\t\t\t", jd.PPose.Pos) + // } return ji } + +// Transform applies positional and rotational transform to world anchors. +func (jd *Joint) Transform(pos math32.Vector3, rot math32.Quat) { + if jd.Parent >= 0 { + return + } + jd.PPose.Transform(pos, rot) +} diff --git a/physics/builder/object.go b/physics/builder/object.go index a6441b72..de0f6113 100644 --- a/physics/builder/object.go +++ b/physics/builder/object.go @@ -4,6 +4,11 @@ package builder +import ( + "cogentcore.org/core/math32" + "cogentcore.org/lab/physics/phyxyz" +) + // Object is an object within the [World]. // Each object is a coherent collection of bodies, typically // connected by joints. This is an organizational convenience @@ -24,3 +29,39 @@ func (ob *Object) Body(idx int) *Body { func (ob *Object) Joint(idx int) *Joint { return &ob.Joints[idx] } + +// Copy copies all bodies and joints from given source world into this one. +// (The objects will be identical after, regardless of current starting +// condition). +func (ob *Object) Copy(so *Object) { + ob.Bodies = make([]Body, len(so.Bodies)) + ob.Joints = make([]Joint, len(so.Joints)) + copy(ob.Bodies, so.Bodies) + copy(ob.Joints, so.Joints) + for i := range ob.Bodies { + bd := ob.Body(i) + bd.Skin = nil + } +} + +// CopySkins makes new skins for bodies based on those in source object. +// Which must have same number of bodies. +func (ob *Object) CopySkins(sc *phyxyz.Scene, so *Object) { + for i := range ob.Bodies { + bd := ob.Body(i) + sb := so.Body(i) + bd.NewSkin(sc, sb.Skin.Name, sb.Skin.Color) + } +} + +// Transform applies positional and rotational transforms to all bodies. +func (ob *Object) Transform(pos math32.Vector3, rot math32.Quat) { + for i := range ob.Bodies { + bd := ob.Body(i) + bd.Pose.Transform(pos, rot) + } + for i := range ob.Joints { + jd := ob.Joint(i) + jd.Transform(pos, rot) // only for world-anchored joints + } +} diff --git a/physics/builder/pose.go b/physics/builder/pose.go index 09033875..72b3c107 100644 --- a/physics/builder/pose.go +++ b/physics/builder/pose.go @@ -9,6 +9,7 @@ import ( "cogentcore.org/core/icons" "cogentcore.org/core/math32" "cogentcore.org/core/tree" + "cogentcore.org/lab/gosl/slmath" ) // Pose represents the 3D position and rotation. @@ -36,6 +37,11 @@ func (ps *Pose) FromRel(rel, par *Pose) { ps.Pos = par.Quat.MulVector(rel.Pos).Add(par.Pos) } +// Transform applies positional and rotational transform to pose. +func (ps *Pose) Transform(pos math32.Vector3, rot math32.Quat) { + slmath.MulSpatialTransforms(pos, rot, ps.Pos, ps.Quat, &ps.Pos, &ps.Quat) +} + //////// Moving // Move moves (translates) Pos by given amount, and sets the LinVel to the given diff --git a/physics/builder/world.go b/physics/builder/world.go index 54ac69a5..b3ce7aea 100644 --- a/physics/builder/world.go +++ b/physics/builder/world.go @@ -4,6 +4,11 @@ package builder +import ( + "cogentcore.org/core/math32" + "cogentcore.org/lab/physics/phyxyz" +) + // World is one world within the Builder. type World struct { // World is the world index. -1 = globals, else positive.. are distinct @@ -26,3 +31,29 @@ func (wl *World) NewObject() *Object { wl.Objects = append(wl.Objects, Object{}) return &wl.Objects[idx] } + +// Copy copies all objects from given source world into this one. +// (The worlds will be identical after, regardless of current starting +// condition). +func (wl *World) Copy(ow *World) { + wl.Objects = make([]Object, len(ow.Objects)) + for i := range wl.Objects { + wl.Object(i).Copy(ow.Object(i)) + } +} + +// CopySkins makes new skins for bodies in world, +// based on those in source world, which must be a Copy. +func (wl *World) CopySkins(sc *phyxyz.Scene, ow *World) { + for i := range wl.Objects { + wl.Object(i).CopySkins(sc, ow.Object(i)) + } +} + +// Transform applies positional and rotational transforms to all objects. +func (wl *World) Transform(pos math32.Vector3, rot math32.Quat) { + for i := range wl.Objects { + ob := wl.Object(i) + ob.Transform(pos, rot) + } +} diff --git a/physics/examples/pendula/pendula.go b/physics/examples/pendula/pendula.go index c3c404c6..da0bc55c 100644 --- a/physics/examples/pendula/pendula.go +++ b/physics/examples/pendula/pendula.go @@ -101,14 +101,13 @@ func main() { } stY := 4 * ps.HSize.Y - clr := colors.Names[0] x := -ps.HSize.Y y := stY if ps.StartVert { x = 0 y -= ps.HSize.Y } - pb := obj.NewDynamicSkin(sc, "top", physics.Capsule, clr, ps.Mass, ps.HSize, math32.Vec3(x, y, 0), rleft) + pb := obj.NewDynamicSkin(sc, "top", physics.Capsule, "blue", ps.Mass, ps.HSize, math32.Vec3(x, y, 0), rleft) if !ps.Collide { pb.SetGroup(1) } @@ -137,6 +136,8 @@ func main() { pb = cb botJoint = jd } + bld.ReplicateWorld(sc, 0, 2, 2, math32.Vec3(0, 0, 2), math32.Vec3(2, 0, 0)) + bld.Build(ml, sc) }) diff --git a/physics/phyxyz/skin.go b/physics/phyxyz/skin.go index 7d1602c9..a048624e 100644 --- a/physics/phyxyz/skin.go +++ b/physics/phyxyz/skin.go @@ -83,6 +83,7 @@ func (sk *Skin) UpdateFromPhysics() { sk.Pos = physics.BodyPos(ix) sk.Quat = physics.BodyQuat(ix) } + // fmt.Println("skin:", sk.BodyIndex, sk.DynamicIndex, sk.Name, sk.Pos, sk.Quat) } // UpdatePose updates the xyz node pose from skin. From c3e1b437c9059cb469c8f44536c97e3a7f53a66c Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Sun, 28 Dec 2025 12:22:24 +0100 Subject: [PATCH 59/97] physics: scene, editor support for viewing different replicas --- physics/builder/builder.go | 23 ++++++++++++++++++++++- physics/examples/pendula/pendula.go | 2 +- physics/model.go | 25 +++++++++++++++++++++++++ physics/model.goal | 25 +++++++++++++++++++++++++ physics/phyxyz/editor.go | 24 ++++++++++++++++++++++++ physics/phyxyz/scene.go | 14 +++++++++++++- physics/phyxyz/skin.go | 19 +++++++++++-------- physics/vars.go | 3 +++ 8 files changed, 124 insertions(+), 11 deletions(-) diff --git a/physics/builder/builder.go b/physics/builder/builder.go index 10991a81..64a356e4 100644 --- a/physics/builder/builder.go +++ b/physics/builder/builder.go @@ -15,9 +15,16 @@ import ( // Builder is the global container of [physics.Model] elements, // organized into worlds that are independently updated. type Builder struct { - // Worlds are the independent world elements. Worlds []World + + // ReplicasStart is the starting Worlds index for replicated world bodies. + // Set by ReplicateWorld, and used to set corresponding value in Model. + ReplicasStart int + + // ReplicasN is the total number of replicated Worlds (including source). + // Set by ReplicateWorld, and used to set corresponding value in Model. + ReplicasN int } func NewBuilder() *Builder { @@ -56,6 +63,8 @@ func (bl *Builder) NewWorld() *World { // Build builds a physics model, with optional [phyxyz.Scene] for // visualization (using Skin elements created for bodies). func (bl *Builder) Build(ml *physics.Model, sc *phyxyz.Scene) { + repSt := int32(0) + repN := int32(0) for wi := range bl.Worlds { wl := bl.World(wi) // fmt.Println("\n######## World:", wl.World) @@ -65,6 +74,12 @@ func (bl *Builder) Build(ml *physics.Model, sc *phyxyz.Scene) { for bbi := range ob.Bodies { bd := ob.Body(bbi) bd.NewPhysicsBody(ml, wl.World) + if bl.ReplicasN > 0 && wi == bl.ReplicasStart { + repN++ + if bbi == 0 { + repSt = bd.BodyIndex + } + } } for bji := range ob.Joints { jd := ob.Joint(bji) @@ -72,6 +87,10 @@ func (bl *Builder) Build(ml *physics.Model, sc *phyxyz.Scene) { } } } + if repN > 0 { + ml.ReplicasStart = repSt + ml.ReplicasN = repN + } } // ReplicateWorld makes copies of given world to form an X,Y grid of @@ -101,4 +120,6 @@ func (bl *Builder) ReplicateWorld(sc *phyxyz.Scene, worldIdx, nY, nX int, Yoff, } } } + bl.ReplicasStart = worldIdx + bl.ReplicasN = nY * nX } diff --git a/physics/examples/pendula/pendula.go b/physics/examples/pendula/pendula.go index da0bc55c..5f48b29a 100644 --- a/physics/examples/pendula/pendula.go +++ b/physics/examples/pendula/pendula.go @@ -136,7 +136,7 @@ func main() { pb = cb botJoint = jd } - bld.ReplicateWorld(sc, 0, 2, 2, math32.Vec3(0, 0, 2), math32.Vec3(2, 0, 0)) + bld.ReplicateWorld(nil, 0, 2, 2, math32.Vec3(0, 0, -1), math32.Vec3(1, 0, 0)) bld.Build(ml, sc) }) diff --git a/physics/model.go b/physics/model.go index 551be302..aef4ff66 100644 --- a/physics/model.go +++ b/physics/model.go @@ -163,6 +163,7 @@ func (ml *Model) NewDynamic(shape Shapes, mass float32, hsize, pos math32.Vector // to the GPU. func (ml *Model) SetAsCurrent() { isCur := (Bodies == ml.Bodies) + CurModel = ml ml.SetAsCurrentVars() if GPUInitialized && !isCur { ml.ToGPUInfra() @@ -200,3 +201,27 @@ func (ml *Model) ToGPUInfra() { ToGPUTensorStrides() ToGPU(ParamsVar, BodiesVar, JointsVar, JointDoFsVar, BodyJointsVar, BodyCollidePairsVar, DynamicsVar, BroadContactsNVar, BroadContactsVar, ContactsNVar, ContactsVar, JointControlsVar) } + +// ReplicaWorldsN returns the number of replicated worlds. 0 if none. +func (ml *Model) ReplicaWorldsN() int32 { + if ml.ReplicasN == 0 { + return 0 + } + nbody := int32(ml.Bodies.DimSize(0)) + return (nbody - ml.ReplicasStart) / ml.ReplicasN +} + +// ReplicasIndexes returns the body and dynamics (if dynamic) indexes +// for given replica world and source body index, if ReplicasN is > 0. +// Otherwise, returns bi and corresponding dynamic index. +func (ml *Model) ReplicasIndexes(bi, replica int32) (bodyIdx, dynIdx int32) { + if ml.ReplicasN == 0 { + return bi, GetBodyDynamic(bi) + } + if bi < ml.ReplicasStart || bi >= ml.ReplicasStart+ml.ReplicasN { + return bi, GetBodyDynamic(bi) + } + bodyIdx = (bi - ml.ReplicasStart) + ml.ReplicasStart + replica*ml.ReplicasN + dynIdx = GetBodyDynamic(bodyIdx) + return +} diff --git a/physics/model.goal b/physics/model.goal index 964ba352..0c26a850 100644 --- a/physics/model.goal +++ b/physics/model.goal @@ -161,6 +161,7 @@ func (ml *Model) NewDynamic(shape Shapes, mass float32, hsize, pos math32.Vector // to the GPU. func (ml *Model) SetAsCurrent() { isCur := (Bodies == ml.Bodies) + CurModel = ml ml.SetAsCurrentVars() if GPUInitialized && !isCur { ml.ToGPUInfra() @@ -198,3 +199,27 @@ func (ml *Model) ToGPUInfra() { ToGPUTensorStrides() ToGPU(ParamsVar, BodiesVar, JointsVar, JointDoFsVar, BodyJointsVar, BodyCollidePairsVar, DynamicsVar, BroadContactsNVar, BroadContactsVar, ContactsNVar, ContactsVar, JointControlsVar) } + +// ReplicaWorldsN returns the number of replicated worlds. 0 if none. +func (ml *Model) ReplicaWorldsN() int32 { + if ml.ReplicasN == 0 { + return 0 + } + nbody := int32(ml.Bodies.DimSize(0)) + return (nbody - ml.ReplicasStart) / ml.ReplicasN +} + +// ReplicasIndexes returns the body and dynamics (if dynamic) indexes +// for given replica world and source body index, if ReplicasN is > 0. +// Otherwise, returns bi and corresponding dynamic index. +func (ml *Model) ReplicasIndexes(bi, replica int32) (bodyIdx, dynIdx int32) { + if ml.ReplicasN == 0 { + return bi, GetBodyDynamic(bi) + } + if bi < ml.ReplicasStart || bi >= ml.ReplicasStart+ml.ReplicasN { + return bi, GetBodyDynamic(bi) + } + bodyIdx = (bi - ml.ReplicasStart) + ml.ReplicasStart + replica*ml.ReplicasN + dynIdx = GetBodyDynamic(bodyIdx) + return +} diff --git a/physics/phyxyz/editor.go b/physics/phyxyz/editor.go index afb85e92..32018b7d 100644 --- a/physics/phyxyz/editor.go +++ b/physics/phyxyz/editor.go @@ -48,6 +48,9 @@ type Editor struct { //types:add // Defaults to math32.Vec3(0, 25, 20). CameraPos math32.Vector3 + // Replica is the replica world to view, if replicas are present in model. + Replica int + // IsRunning is true if currently running sim. isRunning bool @@ -257,4 +260,25 @@ func (pe *Editor) MakeToolbar(p *tree.Plan) { }) w.FirstStyler(func(s *styles.Style) { s.SetEnabled(!pe.isRunning) }) }) + + tree.Add(p, func(w *core.Separator) {}) + + tt := "Replica world to view" + tree.Add(p, func(w *core.Text) { w.SetText("Replica:").SetTooltip(tt) }) + + tree.Add(p, func(w *core.Spinner) { + core.Bind(&pe.Replica, w) + w.SetMin(0).SetTooltip(tt) + w.Styler(func(s *styles.Style) { + replN := physics.CurModel.ReplicaWorldsN() + pe.Scene.ReplicasView = replN > 0 + w.SetMax(float32(replN - 1)) + s.SetEnabled(replN > 0) + }) + w.OnChange(func(e events.Event) { + pe.Scene.ReplicasIndex = pe.Replica + pe.Scene.Update() + pe.NeedsRender() + }) + }) } diff --git a/physics/phyxyz/scene.go b/physics/phyxyz/scene.go index 23909e3c..776797fc 100644 --- a/physics/phyxyz/scene.go +++ b/physics/phyxyz/scene.go @@ -30,6 +30,13 @@ type Scene struct { // Skins are the view elements for each body in [physics.Model]. Skins []*Skin + + // ReplicasView enables viewing of different replicated worlds + // using the same skins. + ReplicasView bool + + // ReplicasIndex is the replicated world to view. + ReplicasIndex int } // NewScene returns a new Scene for visualizing a [physics.Model]. @@ -47,6 +54,11 @@ func NewScene(sc *xyz.Scene) *Scene { // (will return if already called). func (sc *Scene) Init(ml *physics.Model) { ml.Config() + if ml.ReplicasN > 0 { + sc.ReplicasView = true + } else { + sc.ReplicasView = false + } if len(sc.Root.Makers.Normal) > 0 { return } @@ -78,7 +90,7 @@ func (sc *Scene) Update() { // physics state (use physics.Model.SetAsCurrent()). func (sc *Scene) UpdateFromPhysics() { for _, sk := range sc.Skins { - sk.UpdateFromPhysics() + sk.UpdateFromPhysics(sc) } } diff --git a/physics/phyxyz/skin.go b/physics/phyxyz/skin.go index a048624e..89b20e2d 100644 --- a/physics/phyxyz/skin.go +++ b/physics/phyxyz/skin.go @@ -72,16 +72,19 @@ func (sc *Scene) NewDynamic(ml *physics.Model, name string, shape physics.Shapes } // UpdateFromPhysics updates the Skin from physics state. -func (sk *Skin) UpdateFromPhysics() { +func (sk *Skin) UpdateFromPhysics(sc *Scene) { params := physics.GetParams(0) - if sk.DynamicIndex >= 0 { - ix := int32(sk.DynamicIndex) - sk.Pos = physics.DynamicPos(ix, params.Cur) - sk.Quat = physics.DynamicQuat(ix, params.Cur) + di := int32(sk.DynamicIndex) + bi := int32(sk.BodyIndex) + if sc.ReplicasView { + bi, di = physics.CurModel.ReplicasIndexes(bi, int32(sc.ReplicasIndex)) + } + if di >= 0 { + sk.Pos = physics.DynamicPos(di, params.Cur) + sk.Quat = physics.DynamicQuat(di, params.Cur) } else { - ix := int32(sk.BodyIndex) - sk.Pos = physics.BodyPos(ix) - sk.Quat = physics.BodyQuat(ix) + sk.Pos = physics.BodyPos(bi) + sk.Quat = physics.BodyQuat(bi) } // fmt.Println("skin:", sk.BodyIndex, sk.DynamicIndex, sk.Name, sk.Pos, sk.Quat) } diff --git a/physics/vars.go b/physics/vars.go index 7f984246..dac4b9c8 100644 --- a/physics/vars.go +++ b/physics/vars.go @@ -86,3 +86,6 @@ var ( //gosl:dims 2 JointControls *tensor.Float32 ) + +// CurModel is the currently active [Model]. +var CurModel *Model From b2f5314703531837e662aa3082392a02862aed4a Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Sun, 28 Dec 2025 12:24:11 +0100 Subject: [PATCH 60/97] physics: replicas fix --- physics/builder/builder.go | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/physics/builder/builder.go b/physics/builder/builder.go index 64a356e4..0d985bad 100644 --- a/physics/builder/builder.go +++ b/physics/builder/builder.go @@ -98,7 +98,7 @@ func (bl *Builder) Build(ml *physics.Model, sc *phyxyz.Scene) { // worldIdx is the index in Worlds, not the world number. // Because different worlds do not interact, offsets are not necessary // and can potentially affect numerical accuracy. Offsets can also be -// established purely in phyxyz.Scene viewing. +// established purely in [phyxyz.Scene] viewing. // If the given [phyxyz.Scene] is non-nil, then new skins will be made // for the replicated bodies (else not). func (bl *Builder) ReplicateWorld(sc *phyxyz.Scene, worldIdx, nY, nX int, Yoff, Xoff math32.Vector3) { @@ -120,6 +120,8 @@ func (bl *Builder) ReplicateWorld(sc *phyxyz.Scene, worldIdx, nY, nX int, Yoff, } } } - bl.ReplicasStart = worldIdx - bl.ReplicasN = nY * nX + if sc == nil { + bl.ReplicasStart = worldIdx + bl.ReplicasN = nY * nX + } } From 6896e71db7f09da06f4fdbdc4fe98930f6a72503 Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Sun, 28 Dec 2025 12:30:37 +0100 Subject: [PATCH 61/97] physics: renderFrom takes replica --- physics/builder/builder.go | 6 +++--- physics/phyxyz/scene.go | 10 +++++++--- 2 files changed, 10 insertions(+), 6 deletions(-) diff --git a/physics/builder/builder.go b/physics/builder/builder.go index 0d985bad..894aae7a 100644 --- a/physics/builder/builder.go +++ b/physics/builder/builder.go @@ -97,10 +97,10 @@ func (bl *Builder) Build(ml *physics.Model, sc *phyxyz.Scene) { // worlds with given offsets added between world objects. Note that // worldIdx is the index in Worlds, not the world number. // Because different worlds do not interact, offsets are not necessary -// and can potentially affect numerical accuracy. Offsets can also be -// established purely in [phyxyz.Scene] viewing. +// and can potentially affect numerical accuracy. // If the given [phyxyz.Scene] is non-nil, then new skins will be made -// for the replicated bodies (else not). +// for the replicated bodies. Otherwise, the [phyxyz.Scene] can view +// different replicas. func (bl *Builder) ReplicateWorld(sc *phyxyz.Scene, worldIdx, nY, nX int, Yoff, Xoff math32.Vector3) { rot := math32.NewQuat(0, 0, 0, 1) src := bl.World(worldIdx) diff --git a/physics/phyxyz/scene.go b/physics/phyxyz/scene.go index 776797fc..f2e10d6e 100644 --- a/physics/phyxyz/scene.go +++ b/physics/phyxyz/scene.go @@ -94,14 +94,18 @@ func (sc *Scene) UpdateFromPhysics() { } } -// RenderFromNode does an offscreen render using given [Skin] +// RenderFrom does an offscreen render using given [Skin] // for the camera position and orientation, returning the render image. // Current scene camera is saved and restored. -func (sc *Scene) RenderFromNode(sk *Skin, cam *Camera) image.Image { +// If ReplicasView is set, then the given replica will be used for rendering. +func (sc *Scene) RenderFrom(sk *Skin, cam *Camera, replica int) image.Image { xysc := sc.Scene - camnm := "physics-view-rendernode-save" + camnm := "scene-renderfrom-save" xysc.SaveCamera(camnm) + rep := sc.ReplicasIndex + sc.ReplicasIndex = replica defer func() { + sc.ReplicasIndex = rep xysc.SetCamera(camnm) xysc.UseMainFrame() }() From 6ff2707b9784827c9267a11b218c8327c592843a Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Sun, 28 Dec 2025 12:44:09 +0100 Subject: [PATCH 62/97] physics: builder PoseToPhysics for updating physics -- todo: use in virtroom --- physics/builder/body.go | 13 ++++++++++++ physics/builder/joint.go | 10 ++++++++++ physics/builder/object.go | 20 ++++++++++++++----- physics/builder/typegen.go | 15 ++++++++++++-- physics/builder/world.go | 11 ++++++++-- physics/enumgen.go | 2 +- physics/vars.go | 6 ++++-- .../labsymbols/cogentcore_org-lab-physics.go | 1 + 8 files changed, 66 insertions(+), 12 deletions(-) diff --git a/physics/builder/body.go b/physics/builder/body.go index 7aa91d6c..a63deba9 100644 --- a/physics/builder/body.go +++ b/physics/builder/body.go @@ -155,3 +155,16 @@ func (bd *Body) NewPhysicsBody(ml *physics.Model, world int) { physics.SetBodyFrictionTortion(bi, bd.FrictionTortion) physics.SetBodyFrictionRolling(bi, bd.FrictionRolling) } + +// PoseToPhysics sets the current body poses to the physics current state. +// For Dynamic bodies, sets dynamic state. Also updates world-anchored joints. +func (bd *Body) PoseToPhysics() { + if bd.DynamicIndex >= 0 { + params := physics.GetParams(0) + physics.SetDynamicPos(bd.DynamicIndex, params.Cur, bd.Pose.Pos) + physics.SetDynamicQuat(bd.DynamicIndex, params.Cur, bd.Pose.Quat) + } else { + physics.SetBodyPos(bd.DynamicIndex, bd.Pose.Pos) + physics.SetBodyQuat(bd.DynamicIndex, bd.Pose.Quat) + } +} diff --git a/physics/builder/joint.go b/physics/builder/joint.go index 31c21954..4b79c833 100644 --- a/physics/builder/joint.go +++ b/physics/builder/joint.go @@ -227,3 +227,13 @@ func (jd *Joint) Transform(pos math32.Vector3, rot math32.Quat) { } jd.PPose.Transform(pos, rot) } + +// PoseToPhysics sets the current body poses to the physics current state. +// For Dynamic bodies, sets dynamic state. Also updates world-anchored joints. +func (jd *Joint) PoseToPhysics() { + if jd.Parent >= 0 { + return + } + physics.SetJointPPos(jd.JointIndex, jd.PPose.Pos) + physics.SetJointPQuat(jd.JointIndex, jd.PPose.Quat) +} diff --git a/physics/builder/object.go b/physics/builder/object.go index de0f6113..4240766f 100644 --- a/physics/builder/object.go +++ b/physics/builder/object.go @@ -54,14 +54,24 @@ func (ob *Object) CopySkins(sc *phyxyz.Scene, so *Object) { } } -// Transform applies positional and rotational transforms to all bodies. +// Transform applies positional and rotational transforms to all bodies, +// and world-anchored joints. func (ob *Object) Transform(pos math32.Vector3, rot math32.Quat) { for i := range ob.Bodies { - bd := ob.Body(i) - bd.Pose.Transform(pos, rot) + ob.Body(i).Pose.Transform(pos, rot) + } + for i := range ob.Joints { + ob.Joint(i).Transform(pos, rot) // only for world-anchored joints + } +} + +// PoseToPhysics sets the current body poses to the physics current state. +// For Dynamic bodies, sets dynamic state. Also updates world-anchored joints. +func (ob *Object) PoseToPhysics() { + for i := range ob.Bodies { + ob.Body(i).PoseToPhysics() } for i := range ob.Joints { - jd := ob.Joint(i) - jd.Transform(pos, rot) // only for world-anchored joints + ob.Joint(i).PoseToPhysics() } } diff --git a/physics/builder/typegen.go b/physics/builder/typegen.go index e74f20c5..73d4e7e8 100644 --- a/physics/builder/typegen.go +++ b/physics/builder/typegen.go @@ -95,16 +95,27 @@ func (t *Body) SetBodyIndex(v int32) *Body { t.BodyIndex = v; return t } // [physics.Model] Dynamics list, once built. func (t *Body) SetDynamicIndex(v int32) *Body { t.DynamicIndex = v; return t } -var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics/builder.Builder", IDName: "builder", Doc: "Builder is the global container of [physics.Model] elements,\norganized into worlds that are independently updated.", Fields: []types.Field{{Name: "Worlds", Doc: "Worlds are the independent world elements."}}}) +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics/builder.Builder", IDName: "builder", Doc: "Builder is the global container of [physics.Model] elements,\norganized into worlds that are independently updated.", Fields: []types.Field{{Name: "Worlds", Doc: "Worlds are the independent world elements."}, {Name: "ReplicasStart", Doc: "ReplicasStart is the starting Worlds index for replicated world bodies.\nSet by ReplicateWorld, and used to set corresponding value in Model."}, {Name: "ReplicasN", Doc: "ReplicasN is the total number of replicated Worlds (including source).\nSet by ReplicateWorld, and used to set corresponding value in Model."}}}) // SetWorlds sets the [Builder.Worlds]: // Worlds are the independent world elements. func (t *Builder) SetWorlds(v ...World) *Builder { t.Worlds = v; return t } -var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics/builder.Joint", IDName: "joint", Doc: "Joint describes a joint between two bodies.", Fields: []types.Field{{Name: "Parent", Doc: "Parent is index within an Object for parent body."}, {Name: "Child", Doc: "Parent is index within an Object for parent body."}, {Name: "Type", Doc: "Type is the type of the joint."}, {Name: "PPose", Doc: "PPose is the parent position and orientation of the joint\nin the parent's body-centered coordinates."}, {Name: "CPose", Doc: "CPose is the child position and orientation of the joint\nin the parent's body-centered coordinates."}, {Name: "LinearDoFN", Doc: "LinearDoFN is the number of linear degrees of freedom (3 max)."}, {Name: "AngularDoFN", Doc: "AngularDoFN is the number of linear degrees of freedom (3 max)."}, {Name: "DoFs", Doc: "DoFs are the degrees-of-freedom for this joint."}, {Name: "JointIndex", Doc: "JointIndex is the index of this joint in [physics.Joints] when built."}}}) +// SetReplicasStart sets the [Builder.ReplicasStart]: +// ReplicasStart is the starting Worlds index for replicated world bodies. +// Set by ReplicateWorld, and used to set corresponding value in Model. +func (t *Builder) SetReplicasStart(v int) *Builder { t.ReplicasStart = v; return t } + +// SetReplicasN sets the [Builder.ReplicasN]: +// ReplicasN is the total number of replicated Worlds (including source). +// Set by ReplicateWorld, and used to set corresponding value in Model. +func (t *Builder) SetReplicasN(v int) *Builder { t.ReplicasN = v; return t } + +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics/builder.Joint", IDName: "joint", Doc: "Joint describes a joint between two bodies.", Fields: []types.Field{{Name: "Parent", Doc: "Parent is index within an Object for parent body.\n-1 for world-anchored parent."}, {Name: "Child", Doc: "Parent is index within an Object for parent body."}, {Name: "Type", Doc: "Type is the type of the joint."}, {Name: "PPose", Doc: "PPose is the parent position and orientation of the joint\nin the parent's body-centered coordinates."}, {Name: "CPose", Doc: "CPose is the child position and orientation of the joint\nin the parent's body-centered coordinates."}, {Name: "LinearDoFN", Doc: "LinearDoFN is the number of linear degrees of freedom (3 max)."}, {Name: "AngularDoFN", Doc: "AngularDoFN is the number of linear degrees of freedom (3 max)."}, {Name: "DoFs", Doc: "DoFs are the degrees-of-freedom for this joint."}, {Name: "JointIndex", Doc: "JointIndex is the index of this joint in [physics.Joints] when built."}}}) // SetParent sets the [Joint.Parent]: // Parent is index within an Object for parent body. +// -1 for world-anchored parent. func (t *Joint) SetParent(v int) *Joint { t.Parent = v; return t } // SetChild sets the [Joint.Child]: diff --git a/physics/builder/world.go b/physics/builder/world.go index b3ce7aea..dc5a1e1e 100644 --- a/physics/builder/world.go +++ b/physics/builder/world.go @@ -53,7 +53,14 @@ func (wl *World) CopySkins(sc *phyxyz.Scene, ow *World) { // Transform applies positional and rotational transforms to all objects. func (wl *World) Transform(pos math32.Vector3, rot math32.Quat) { for i := range wl.Objects { - ob := wl.Object(i) - ob.Transform(pos, rot) + wl.Object(i).Transform(pos, rot) + } +} + +// PoseToPhysics sets the current body poses to the physics current state. +// For Dynamic bodies, sets dynamic state. Also updates world-anchored joints. +func (wl *World) PoseToPhysics() { + for i := range wl.Objects { + wl.Object(i).PoseToPhysics() } } diff --git a/physics/enumgen.go b/physics/enumgen.go index acc74776..674446b8 100644 --- a/physics/enumgen.go +++ b/physics/enumgen.go @@ -109,7 +109,7 @@ const JointControlVarsN JointControlVars = 5 var _JointControlVarsValueMap = map[string]JointControlVars{`JointControlForce`: 0, `JointTargetPos`: 1, `JointTargetStiff`: 2, `JointTargetVel`: 3, `JointTargetDamp`: 4} -var _JointControlVarsDescMap = map[JointControlVars]string{0: `Joint force and torque inputs`, 1: `Joint target position settings: the stiffness parameter determines how strongly the target position target is enforced: 0 = not at all; larger = stronger (e.g., 1000 or higher). Set to 0 to allow the joint to be fully flexible.`, 2: ``, 3: `Joint target velocity settings: the damping parameter determines how strongly the target velocity target is enforced: 0 = not at all; larger = stronger (e.g., 1 is reasonable). Set to 0 to allow the joint to be fully flexible.`, 4: ``} +var _JointControlVarsDescMap = map[JointControlVars]string{0: `Joint force and torque inputs`, 1: `JointTargetPos is the position target value, where 0 is the initial position. For angular joints, this is in radians.`, 2: `JointTargetStiff determines how strongly the target position is enforced: 0 = not at all; larger = stronger (e.g., 1000 or higher). Set to 0 to allow the joint to be fully flexible.`, 3: `JointTargetVel is the velocity target value. For example, 0 effectively damps joint movement in proportion to Damp parameter.`, 4: `JointTargetDamp determines how strongly the target velocity is enforced: 0 = not at all; larger = stronger (e.g., 1 is reasonable). Set to 0 to allow the joint to be fully flexible.`} var _JointControlVarsMap = map[JointControlVars]string{0: `JointControlForce`, 1: `JointTargetPos`, 2: `JointTargetStiff`, 3: `JointTargetVel`, 4: `JointTargetDamp`} diff --git a/physics/vars.go b/physics/vars.go index dac4b9c8..0744e56f 100644 --- a/physics/vars.go +++ b/physics/vars.go @@ -9,6 +9,9 @@ import "cogentcore.org/lab/tensor" // note: add -keep to inspect intermediate .go code //go:generate gosl -exclude=Update,Defaults,ShouldDisplay -max-buffer-size=2147483616 +// CurModel is the currently active [Model]. +var CurModel *Model + //gosl:start // vars are all the global vars for axon GPU / CPU computation. @@ -87,5 +90,4 @@ var ( JointControls *tensor.Float32 ) -// CurModel is the currently active [Model]. -var CurModel *Model +//gosl:end diff --git a/yaegilab/labsymbols/cogentcore_org-lab-physics.go b/yaegilab/labsymbols/cogentcore_org-lab-physics.go index aab01faa..49006bf9 100644 --- a/yaegilab/labsymbols/cogentcore_org-lab-physics.go +++ b/yaegilab/labsymbols/cogentcore_org-lab-physics.go @@ -158,6 +158,7 @@ func init() { "ContactsN": reflect.ValueOf(&physics.ContactsN).Elem(), "ContactsNVar": reflect.ValueOf(physics.ContactsNVar), "ContactsVar": reflect.ValueOf(physics.ContactsVar), + "CurModel": reflect.ValueOf(&physics.CurModel).Elem(), "Cylinder": reflect.ValueOf(physics.Cylinder), "CylinderSDF": reflect.ValueOf(physics.CylinderSDF), "D6": reflect.ValueOf(physics.D6), From c99156b1d1203c2e106272336f857e45fbe5c8a4 Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Mon, 29 Dec 2025 23:19:09 +0100 Subject: [PATCH 63/97] physics: virtroom mostly working --- physics/builder/body.go | 4 +- physics/builder/builder.go | 16 ++- physics/builder/joint.go | 17 +-- physics/builder/object.go | 58 +++++++++-- physics/builder/physics.go | 50 +++++++++ physics/builder/pose.go | 17 ++- physics/builder/world.go | 6 +- physics/examples/virtroom/virtroom.go | 144 +++++++++++++------------- physics/phyxyz/editor.go | 6 +- physics/phyxyz/scene.go | 4 +- 10 files changed, 209 insertions(+), 113 deletions(-) create mode 100644 physics/builder/physics.go diff --git a/physics/builder/body.go b/physics/builder/body.go index a63deba9..da50935c 100644 --- a/physics/builder/body.go +++ b/physics/builder/body.go @@ -161,8 +161,8 @@ func (bd *Body) NewPhysicsBody(ml *physics.Model, world int) { func (bd *Body) PoseToPhysics() { if bd.DynamicIndex >= 0 { params := physics.GetParams(0) - physics.SetDynamicPos(bd.DynamicIndex, params.Cur, bd.Pose.Pos) - physics.SetDynamicQuat(bd.DynamicIndex, params.Cur, bd.Pose.Quat) + physics.SetDynamicPos(bd.DynamicIndex, params.Next, bd.Pose.Pos) + physics.SetDynamicQuat(bd.DynamicIndex, params.Next, bd.Pose.Quat) } else { physics.SetBodyPos(bd.DynamicIndex, bd.Pose.Pos) physics.SetBodyQuat(bd.DynamicIndex, bd.Pose.Quat) diff --git a/physics/builder/builder.go b/physics/builder/builder.go index 894aae7a..fe02453b 100644 --- a/physics/builder/builder.go +++ b/physics/builder/builder.go @@ -94,16 +94,22 @@ func (bl *Builder) Build(ml *physics.Model, sc *phyxyz.Scene) { } // ReplicateWorld makes copies of given world to form an X,Y grid of -// worlds with given offsets added between world objects. Note that -// worldIdx is the index in Worlds, not the world number. +// worlds with given optional offsets (Y, X) added between world objects. +// Note that worldIdx is the index in Worlds, not the world number. // Because different worlds do not interact, offsets are not necessary // and can potentially affect numerical accuracy. // If the given [phyxyz.Scene] is non-nil, then new skins will be made // for the replicated bodies. Otherwise, the [phyxyz.Scene] can view // different replicas. -func (bl *Builder) ReplicateWorld(sc *phyxyz.Scene, worldIdx, nY, nX int, Yoff, Xoff math32.Vector3) { - rot := math32.NewQuat(0, 0, 0, 1) +func (bl *Builder) ReplicateWorld(sc *phyxyz.Scene, worldIdx, nY, nX int, offs ...math32.Vector3) { src := bl.World(worldIdx) + var Yoff, Xoff math32.Vector3 + if len(offs) > 0 { + Yoff = offs[0] + } + if len(offs) > 1 { + Xoff = offs[1] + } for y := range nY { for x := range nX { if x == 0 && y == 0 { @@ -114,7 +120,7 @@ func (bl *Builder) ReplicateWorld(sc *phyxyz.Scene, worldIdx, nY, nX int, Yoff, nw.Copy(src) nw.World = wi off := Yoff.MulScalar(float32(y)).Add(Xoff.MulScalar(float32(x))) - nw.Transform(off, rot) + nw.Move(off) if sc != nil { nw.CopySkins(sc, src) } diff --git a/physics/builder/joint.go b/physics/builder/joint.go index 4b79c833..54b97087 100644 --- a/physics/builder/joint.go +++ b/physics/builder/joint.go @@ -220,20 +220,23 @@ func (jd *Joint) NewPhysicsJoint(ml *physics.Model, ob *Object) int32 { return ji } -// Transform applies positional and rotational transform to world anchors. -func (jd *Joint) Transform(pos math32.Vector3, rot math32.Quat) { - if jd.Parent >= 0 { - return - } - jd.PPose.Transform(pos, rot) +// IsGlobal returns true if this joint has a global world anchor parent. +func (jd *Joint) IsGlobal() bool { + return jd.Parent < 0 } // PoseToPhysics sets the current body poses to the physics current state. // For Dynamic bodies, sets dynamic state. Also updates world-anchored joints. func (jd *Joint) PoseToPhysics() { - if jd.Parent >= 0 { + if !jd.IsGlobal() { return } physics.SetJointPPos(jd.JointIndex, jd.PPose.Pos) physics.SetJointPQuat(jd.JointIndex, jd.PPose.Quat) } + +// SetTargetPos sets the target position for given DoF for +// this joint in the physics model. +func (jd *Joint) SetTargetPos(dof int32, pos, stiff float32) { + physics.SetJointTargetPos(jd.JointIndex, dof, pos, stiff) +} diff --git a/physics/builder/object.go b/physics/builder/object.go index 4240766f..621e867f 100644 --- a/physics/builder/object.go +++ b/physics/builder/object.go @@ -54,24 +54,64 @@ func (ob *Object) CopySkins(sc *phyxyz.Scene, so *Object) { } } -// Transform applies positional and rotational transforms to all bodies, +//////// Transforms + +// PoseToPhysics sets the current body poses to the physics current state. +// For Dynamic bodies, sets dynamic state. Also updates world-anchored joints. +func (ob *Object) PoseToPhysics() { + for i := range ob.Bodies { + ob.Body(i).PoseToPhysics() + } + for i := range ob.Joints { + ob.Joint(i).PoseToPhysics() + } +} + +// Move applies positional and rotational transforms to all bodies, // and world-anchored joints. -func (ob *Object) Transform(pos math32.Vector3, rot math32.Quat) { +func (ob *Object) Move(pos math32.Vector3) { for i := range ob.Bodies { - ob.Body(i).Pose.Transform(pos, rot) + ob.Body(i).Pose.Move(pos) } for i := range ob.Joints { - ob.Joint(i).Transform(pos, rot) // only for world-anchored joints + jd := ob.Joint(i) + if jd.IsGlobal() { + jd.PPose.Move(pos) + } } } -// PoseToPhysics sets the current body poses to the physics current state. -// For Dynamic bodies, sets dynamic state. Also updates world-anchored joints. -func (ob *Object) PoseToPhysics() { +// RotateAround rotates around a given point +func (ob *Object) RotateAround(rot math32.Quat, around math32.Vector3) { for i := range ob.Bodies { - ob.Body(i).PoseToPhysics() + ob.Body(i).Pose.RotateAround(rot, around) } for i := range ob.Joints { - ob.Joint(i).PoseToPhysics() + jd := ob.Joint(i) + if jd.IsGlobal() { + jd.PPose.RotateAround(rot, around) + } } } + +// RotateAroundBody rotates around a given body in object. +func (ob *Object) RotateAroundBody(body int, rot math32.Quat) { + bd := ob.Body(body) + ob.RotateAround(rot, bd.Pose.Pos) +} + +// MoveOnAxis moves (translates) the specified distance on the +// specified local axis, relative to the given body in object. +// The axis is normalized prior to aplying the distance factor. +func (ob *Object) MoveOnAxisBody(body int, x, y, z, dist float32) { + bd := ob.Body(body) + delta := bd.Pose.Quat.MulVector(math32.Vec3(x, y, z).Normal()).MulScalar(dist) + ob.Move(delta) +} + +// RotateOnAxisBody rotates around the specified local axis the +// specified angle in degrees, relative to the given body in the object. +func (ob *Object) RotateOnAxisBody(body int, x, y, z, angle float32) { + rot := math32.NewQuatAxisAngle(math32.Vec3(x, y, z), math32.DegToRad(angle)) + ob.RotateAroundBody(body, rot) +} diff --git a/physics/builder/physics.go b/physics/builder/physics.go new file mode 100644 index 00000000..8eb46b98 --- /dev/null +++ b/physics/builder/physics.go @@ -0,0 +1,50 @@ +// Copyright (c) 2025, Cogent Core. All rights reserved. +// Use of this source code is governed by a BSD-style +// license that can be found in the LICENSE file. + +package builder + +import ( + "cogentcore.org/lab/physics" + "cogentcore.org/lab/physics/phyxyz" +) + +// Physics provides a container and manager for the main physics elements: +// [Builder], [physics.Model], and [phyxyz.Scene]. This is helpful for +// models used within other apps (e.g., an AI simulation), whereas +// [phyxyz.Editor] provides a standalone GUI interface for testing models. +type Physics struct { + // Model has the physics Model. + Model *physics.Model + + // Builder for configuring the Model. + Builder *Builder + + // Scene for visualizing the Model + Scene *phyxyz.Scene +} + +// Build calls Builder.Build with Model and Scene args, +// and then Init on the Scene. +func (ph *Physics) Build() { + ph.Builder.Build(ph.Model, ph.Scene) + ph.Init() +} + +// Init calls Scene.Init with Model. +func (ph *Physics) Init() { + if ph.Scene != nil { + ph.Scene.Init(ph.Model) + } else { + ph.Model.InitState() + } +} + +// Step advances the physics world n steps, and then +// updates the Scene. +func (ph *Physics) Step(n int) { + for range n { + ph.Model.Step() + } + ph.Scene.Update() +} diff --git a/physics/builder/pose.go b/physics/builder/pose.go index 72b3c107..5b65191c 100644 --- a/physics/builder/pose.go +++ b/physics/builder/pose.go @@ -9,7 +9,6 @@ import ( "cogentcore.org/core/icons" "cogentcore.org/core/math32" "cogentcore.org/core/tree" - "cogentcore.org/lab/gosl/slmath" ) // Pose represents the 3D position and rotation. @@ -29,17 +28,10 @@ func (ps *Pose) Defaults() { } } -//////// Pose updates - -// FromRel sets state from relative values compared to a parent state -func (ps *Pose) FromRel(rel, par *Pose) { - ps.Quat = rel.Quat.Mul(par.Quat) - ps.Pos = par.Quat.MulVector(rel.Pos).Add(par.Pos) -} - // Transform applies positional and rotational transform to pose. func (ps *Pose) Transform(pos math32.Vector3, rot math32.Quat) { - slmath.MulSpatialTransforms(pos, rot, ps.Pos, ps.Quat, &ps.Pos, &ps.Quat) + ps.Pos = rot.MulVector(ps.Pos).Add(pos) + ps.Quat = rot.Mul(ps.Quat) } //////// Moving @@ -70,6 +62,11 @@ func (ps *Pose) MoveOnAxisAbs(x, y, z, dist float32) { //types:add //////// Rotating +func (ps *Pose) RotateAround(rot math32.Quat, around math32.Vector3) { + ps.Pos = rot.MulVector(ps.Pos.Sub(around)).Add(around) + ps.Quat = rot.Mul(ps.Quat) +} + // SetEulerRotation sets the rotation in Euler angles (degrees). func (ps *Pose) SetEulerRotation(x, y, z float32) { //types:add ps.Quat.SetFromEuler(math32.Vec3(x, y, z).MulScalar(math32.DegToRadFactor)) diff --git a/physics/builder/world.go b/physics/builder/world.go index dc5a1e1e..7b081f3e 100644 --- a/physics/builder/world.go +++ b/physics/builder/world.go @@ -50,10 +50,10 @@ func (wl *World) CopySkins(sc *phyxyz.Scene, ow *World) { } } -// Transform applies positional and rotational transforms to all objects. -func (wl *World) Transform(pos math32.Vector3, rot math32.Quat) { +// Move moves all objects in world by given delta. +func (wl *World) Move(delta math32.Vector3) { for i := range wl.Objects { - wl.Object(i).Transform(pos, rot) + wl.Object(i).Move(delta) } } diff --git a/physics/examples/virtroom/virtroom.go b/physics/examples/virtroom/virtroom.go index 9be1b518..f41bfb1d 100644 --- a/physics/examples/virtroom/virtroom.go +++ b/physics/examples/virtroom/virtroom.go @@ -24,6 +24,7 @@ import ( "cogentcore.org/core/xyz" "cogentcore.org/core/xyz/xyzcore" "cogentcore.org/lab/physics" + "cogentcore.org/lab/physics/builder" "cogentcore.org/lab/physics/phyxyz" ) @@ -80,24 +81,24 @@ type Env struct { //types:add // color map to use for rendering depth map DepthMap core.ColorMapName - // The [physics World]. - Physics *physics.World - - // Visualization of the physics phyxyz. - World *phyxyz.World + // The core physics elements: Model, Builder, Scene + Physics builder.Physics // 3D visualization of the Scene SceneEditor *xyzcore.SceneEditor // emer object - Emer *phyxyz.View `display:"-"` + Emer *builder.Object `display:"-"` // Right eye of emer - EyeR *phyxyz.View `display:"-"` + EyeR *builder.Body `display:"-"` // snapshot image EyeRImg *core.Image `display:"-"` + // ball joint for the neck. + NeckJoint *builder.Joint + // depth map image DepthImage *core.Image `display:"-"` } @@ -116,23 +117,27 @@ func (ev *Env) Defaults() { } func (ev *Env) MakeWorld(sc *xyz.Scene) { - ev.Physics = physics.NewWorld() + ev.Physics.Model = physics.NewModel() + ev.Physics.Builder = builder.NewBuilder() sc.Background = colors.Scheme.Select.Container xyz.NewAmbient(sc, "ambient", 0.3, xyz.DirectSun) dir := xyz.NewDirectional(sc, "dir", 1, xyz.DirectSun) dir.Pos.Set(0, 2, 1) // default: 0,1,1 = above and behind us (we are at 0,0,X) - ev.World = phyxyz.NewWorld(sc) - ev.MakeRoom("room1", ev.Width, ev.Depth, ev.Height, ev.Thick) - ev.MakeEmer("emer", ev.EmerHt) - ev.World.Init(ev.Physics) - ev.World.Update() + ev.Physics.Scene = phyxyz.NewScene(sc) + wl := ev.Physics.Builder.NewGlobalWorld() + ev.MakeRoom(wl, "room1", ev.Width, ev.Depth, ev.Height, ev.Thick) + ew := ev.Physics.Builder.NewWorld() + ev.MakeEmer(ew, "emer", ev.EmerHt) + // ev.Physics.Builder.ReplicateWorld(1, 8, 2) + ev.Physics.Build() + physics.GetParams(0).Gravity.Y = 0 } // InitWorld does init on world. func (ev *Env) WorldInit() { //types:add - // ev.World.Init() + ev.Physics.Init() } // ConfigView3D makes the 3D view @@ -142,7 +147,7 @@ func (ev *Env) ConfigView3D(sc *xyz.Scene) { // RenderEyeImg returns a snapshot from the perspective of Emer's right eye func (ev *Env) RenderEyeImg() image.Image { - return ev.World.RenderFromNode(ev.EyeR, &ev.Camera) + return ev.Physics.Scene.RenderFrom(ev.EyeR.Skin, &ev.Camera, 0) } // GrabEyeImg takes a snapshot from the perspective of Emer's right eye @@ -175,11 +180,10 @@ func (ev *Env) UpdateView() { } } -// WorldStep does one step of the world -func (ev *Env) WorldStep() { - // pw := ev.Physics - // pw.Update() // only need to call if there are updaters added to world - // pw.WorldRelToAbs() +// ModelStep does one step of the physics model. +func (ev *Env) ModelStep() { + physics.ToGPU(physics.DynamicsVar) + ev.Physics.Step(1) // cts := pw.WorldCollide(physics.DynsTopGps) // ev.Contacts = nil // for _, cl := range cts { @@ -199,106 +203,109 @@ func (ev *Env) WorldStep() { // rot := 100.0 + 90.0*rand.Float32() // ev.Emer.Rel.RotateOnAxis(0, 1, 0, rot) // } - ev.World.Update() ev.GrabEyeImg() ev.UpdateView() } // StepForward moves Emer forward in current facing direction one step, and takes GrabEyeImg func (ev *Env) StepForward() { //types:add - // ev.Emer.Rel.MoveOnAxis(0, 0, 1, -ev.MoveStep) - ev.WorldStep() + ev.Emer.MoveOnAxisBody(0, 0, 0, 1, -ev.MoveStep) + ev.Emer.PoseToPhysics() + ev.ModelStep() } // StepBackward moves Emer backward in current facing direction one step, and takes GrabEyeImg func (ev *Env) StepBackward() { //types:add - // ev.Emer.Rel.MoveOnAxis(0, 0, 1, ev.MoveStep) - ev.WorldStep() + ev.Emer.MoveOnAxisBody(0, 0, 0, 1, ev.MoveStep) + ev.Emer.PoseToPhysics() + ev.ModelStep() } // RotBodyLeft rotates emer left and takes GrabEyeImg func (ev *Env) RotBodyLeft() { //types:add - // ev.Emer.Rel.RotateOnAxis(0, 1, 0, ev.RotStep) - ev.WorldStep() + ev.Emer.RotateOnAxisBody(0, 0, 1, 0, ev.RotStep) + ev.Emer.PoseToPhysics() + ev.ModelStep() } // RotBodyRight rotates emer right and takes GrabEyeImg func (ev *Env) RotBodyRight() { //types:add - // ev.Emer.Rel.RotateOnAxis(0, 1, 0, -ev.RotStep) - ev.WorldStep() + ev.Emer.RotateOnAxisBody(0, 0, 1, 0, -ev.RotStep) + ev.Emer.PoseToPhysics() + ev.ModelStep() } // RotHeadLeft rotates emer left and takes GrabEyeImg func (ev *Env) RotHeadLeft() { //types:add - // hd := ev.Emer.ChildByName("head", 1).(*physics.Group) - // hd.Rel.RotateOnAxis(0, 1, 0, ev.RotStep) - ev.WorldStep() + ev.NeckJoint.SetTargetPos(1, ev.RotStep, 10) + ev.ModelStep() } // RotHeadRight rotates emer right and takes GrabEyeImg func (ev *Env) RotHeadRight() { //types:add - // hd := ev.Emer.ChildByName("head", 1).(*physics.Group) - // hd.Rel.RotateOnAxis(0, 1, 0, -ev.RotStep) - ev.WorldStep() + ev.NeckJoint.SetTargetPos(1, -ev.RotStep, 10) + ev.ModelStep() } // MakeRoom constructs a new room with given params -func (ev *Env) MakeRoom(name string, width, depth, height, thick float32) { - wr := ev.World - wl := ev.Physics - rot := math32.NewQuat(0, 0, 0, 1) +func (ev *Env) MakeRoom(wl *builder.World, name string, width, depth, height, thick float32) { + rot := math32.NewQuatIdentity() hw := width / 2 hd := depth / 2 hh := height / 2 ht := thick / 2 - wr.NewBody(wl, name+"_floor", physics.Box, "grey", math32.Vec3(hw, ht, hd), + obj := wl.NewObject() + sc := ev.Physics.Scene + obj.NewBodySkin(sc, name+"_floor", physics.Box, "grey", math32.Vec3(hw, ht, hd), math32.Vec3(0, -ht, 0), rot) - wr.NewBody(wl, name+"_back-wall", physics.Box, "blue", math32.Vec3(hw, hh, ht), + obj.NewBodySkin(sc, name+"_back-wall", physics.Box, "blue", math32.Vec3(hw, hh, ht), math32.Vec3(0, hh, -hd), rot) - wr.NewBody(wl, name+"_left-wall", physics.Box, "red", math32.Vec3(ht, hh, hd), + obj.NewBodySkin(sc, name+"_left-wall", physics.Box, "red", math32.Vec3(ht, hh, hd), math32.Vec3(-hw, hh, 0), rot) - wr.NewBody(wl, name+"_right-wall", physics.Box, "green", math32.Vec3(ht, hh, hd), + obj.NewBodySkin(sc, name+"_right-wall", physics.Box, "green", math32.Vec3(ht, hh, hd), math32.Vec3(hw, hh, 0), rot) - wr.NewBody(wl, name+"_front-wall", physics.Box, "yellow", math32.Vec3(hw, hh, ht), + obj.NewBodySkin(sc, name+"_front-wall", physics.Box, "yellow", math32.Vec3(hw, hh, ht), math32.Vec3(0, hh, hd), rot) } // MakeEmer constructs a new Emer virtual robot of given height (e.g., 1). -func (ev *Env) MakeEmer(name string, height float32) { - wr := ev.World - wl := ev.Physics +func (ev *Env) MakeEmer(wl *builder.World, name string, height float32) { hh := height / 2 hw := hh * .4 hd := hh * .15 headsz := hd * 1.5 eyesz := headsz * .2 mass := float32(50) // kg - rot := math32.NewQuat(0, 0, 0, 1) - ev.Emer = wr.NewDynamic(wl, name+"_body", physics.Box, "purple", mass, math32.Vec3(hw, hh, hd), - math32.Vec3(0, hh, 0), rot) + rot := math32.NewQuatIdentity() + obj := wl.NewObject() + ev.Emer = obj + sc := ev.Physics.Scene + emr := obj.NewDynamicSkin(sc, name+"_body", physics.Box, "purple", mass, math32.Vec3(hw, hh, hd), math32.Vec3(0, hh, 0), rot) // body := physics.NewCapsule(emr, "body", math32.Vec3(0, hh, 0), hh, hw) // body := physics.NewCylinder(emr, "body", math32.Vec3(0, hh, 0), hh, hw) headPos := math32.Vec3(0, 2*hh+headsz, 0) - head := wr.NewDynamic(wl, name+"_head", physics.Box, "tan", mass*.1, math32.Vec3(headsz, headsz, headsz), - headPos, rot) - head.InitView = func(sld *xyz.Solid) { - head.BoxInit(sld) + head := obj.NewDynamicSkin(sc, name+"_head", physics.Box, "tan", mass*.1, math32.Vec3(headsz, headsz, headsz), headPos, rot) + hdsk := head.Skin + hdsk.InitSkin = func(sld *xyz.Solid) { + hdsk.BoxInit(sld) sld.Updater(func() { - clr := head.Color + clr := hdsk.Color if ev.EmerAngry { clr = "pink" } - head.UpdateColor(clr, sld) + hdsk.UpdateColor(clr, sld) }) } - wl.NewJointBall(ev.Emer.DynamicIndex, head.DynamicIndex, head.Pos, math32.Vec3(0, 0, 0)) - vw := wr.NewDynamic(wl, name+"_eye-l", physics.Box, "green", mass*.001, math32.Vec3(eyesz, eyesz*.5, eyesz*.2), - headPos.Add(math32.Vec3(-headsz*.6, headsz*.1, -(headsz+eyesz*.3))), rot) - wl.NewJointBall(ev.Emer.DynamicIndex, vw.DynamicIndex, vw.Pos, math32.Vec3(0, 0, 0)) - ev.EyeR = wr.NewDynamic(wl, name+"_eye-r", physics.Box, "green", mass*.001, math32.Vec3(eyesz, eyesz*.5, eyesz*.2), - headPos.Add(math32.Vec3(headsz*.6, headsz*.1, -(headsz+eyesz*.3))), rot) - wl.NewJointBall(ev.Emer.DynamicIndex, ev.EyeR.DynamicIndex, ev.EyeR.Pos, math32.Vec3(0, 0, 0)) + ev.NeckJoint = obj.NewJointBall(emr, head, math32.Vec3(0, hh, 0), math32.Vec3(0, -headsz, 0)) + + eyeoff := math32.Vec3(-headsz*.6, headsz*.1, -(headsz + eyesz*.3)) + bd := obj.NewDynamicSkin(sc, name+"_eye-l", physics.Box, "green", mass*.001, math32.Vec3(eyesz, eyesz*.5, eyesz*.2), headPos.Add(eyeoff), rot) + obj.NewJointBall(head, bd, eyeoff, math32.Vec3(0, 0, -eyesz*.3)) + + eyeoff = math32.Vec3(headsz*.6, headsz*.1, -(headsz + eyesz*.3)) + ev.EyeR = obj.NewDynamicSkin(sc, name+"_eye-r", physics.Box, "green", mass*.001, math32.Vec3(eyesz, eyesz*.5, eyesz*.2), headPos.Add(eyeoff), rot) + obj.NewJointBall(head, ev.EyeR, eyeoff, math32.Vec3(0, 0, -eyesz*.3)) } func (ev *Env) ConfigGUI() *core.Body { @@ -307,19 +314,12 @@ func (ev *Env) ConfigGUI() *core.Body { b := core.NewBody("virtroom").SetTitle("Physics Virtual Room") split := core.NewSplits(b) - tv := core.NewTree(core.NewFrame(split)) - sv := core.NewForm(split).SetStruct(ev) + core.NewForm(split).SetStruct(ev) imfr := core.NewFrame(split) tbvw := core.NewTabs(split) scfr, _ := tbvw.NewTab("3D View") - split.SetSplits(.1, .2, .2, .5) - - tv.OnSelect(func(e events.Event) { - if len(tv.SelectedNodes) > 0 { - sv.SetStruct(tv.SelectedNodes[0].AsCoreTree().SyncNode) - } - }) + split.SetSplits(.2, .2, .6) //////// 3D Scene diff --git a/physics/phyxyz/editor.go b/physics/phyxyz/editor.go index 32018b7d..8a4e5e20 100644 --- a/physics/phyxyz/editor.go +++ b/physics/phyxyz/editor.go @@ -22,7 +22,8 @@ import ( ) // Editor provides a basic viewer and parameter controller widget -// for exploring physics models. +// for exploring physics models. It creates and manages its own +// [physics.Model] and [phyxyz.Scene]. type Editor struct { //types:add core.Frame @@ -160,7 +161,6 @@ func (pe *Editor) ConfigModel() { pe.ConfigFunc() } pe.Scene.Init(pe.Model) - pe.Scene.Update() pe.stop = false pe.TimeStep = 0 } @@ -174,8 +174,6 @@ func (pe *Editor) Restart() bool { pe.stop = false pe.TimeStep = 0 pe.Scene.Init(pe.Model) - pe.Scene.Update() - pe.Update() return true } diff --git a/physics/phyxyz/scene.go b/physics/phyxyz/scene.go index f2e10d6e..65807721 100644 --- a/physics/phyxyz/scene.go +++ b/physics/phyxyz/scene.go @@ -51,7 +51,7 @@ func NewScene(sc *xyz.Scene) *Scene { // Init configures the visual world based on Skins, // and calls Config on [physics.Model]. // Call this _once_ after making all the new Skins and Bodies. -// (will return if already called). +// (will return if already called). This calls Update(). func (sc *Scene) Init(ml *physics.Model) { ml.Config() if ml.ReplicasN > 0 { @@ -60,6 +60,7 @@ func (sc *Scene) Init(ml *physics.Model) { sc.ReplicasView = false } if len(sc.Root.Makers.Normal) > 0 { + sc.Update() return } sc.Root.Maker(func(p *tree.Plan) { @@ -67,6 +68,7 @@ func (sc *Scene) Init(ml *physics.Model) { sk.Add(p) } }) + sc.Update() } // Reset resets any existing views, starting fresh for a new configuration. From d26ded0ff707e290b71d7e3ae8964cb638e19810 Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Tue, 30 Dec 2025 09:18:27 +0100 Subject: [PATCH 64/97] physics: docs joint demos, ball fixes: not working for positive values on X, Y, angular limits should be enforced? --- docs/content/physics.md | 147 ++++++++++++++++++- physics/builder/joint.go | 6 + physics/examples/test1/test1.go | 203 +++++---------------------- physics/phyxyz/editor.go | 64 +++++---- physics/shaders/StepSolveJoints.wgsl | 2 +- physics/step_joint.go | 2 +- physics/step_joint.goal | 2 +- 7 files changed, 222 insertions(+), 204 deletions(-) diff --git a/docs/content/physics.md b/docs/content/physics.md index 96954256..cfa8baa0 100644 --- a/docs/content/physics.md +++ b/docs/content/physics.md @@ -40,7 +40,7 @@ ed.SetConfigFunc(func() { rleft := math32.NewQuatAxisAngle(math32.Vec3(0, 0, 1), -math32.Pi/2) pb := sc.NewDynamic(ml, "top", physics.Capsule, "blue", mass, hsz, math32.Vec3(x, stY, 0), rleft) pb.SetBodyGroup(1) // no collide across groups - ji := pb.NewJointRevolute(ml, nil, math32.Vec3(0, stY, 0), math32.Vec3(0, hsz.Y, 0), math32.Vec3(0, 0, 1)) + ji := sc.NewJointRevolute(ml, nil, pb, math32.Vec3(0, stY, 0), math32.Vec3(0, hsz.Y, 0), math32.Vec3(0, 0, 1)) physics.SetJointTargetPos(ji, 0, 0, 0) physics.SetJointTargetVel(ji, 0, 0, 0) @@ -49,7 +49,7 @@ ed.SetConfigFunc(func() { x = -float32(i)*hsz.Y*2 - hsz.Y cb := sc.NewDynamic(ml, "child", physics.Capsule, clr, mass, hsz, math32.Vec3(x, stY, 0), rleft) cb.SetBodyGroup(1+i) - ji = cb.NewJointRevolute(ml, pb, math32.Vec3(0, -hsz.Y, 0), math32.Vec3(0, hsz.Y, 0), math32.Vec3(0, 0, 1)) + ji = sc.NewJointRevolute(ml, pb, cb, math32.Vec3(0, -hsz.Y, 0), math32.Vec3(0, hsz.Y, 0), math32.Vec3(0, 0, 1)) physics.SetJointTargetPos(ji, 0, 0, 0) physics.SetJointTargetVel(ji, 0, 0, 0) pb = cb @@ -64,18 +64,18 @@ The [[doc:physics/phyxyz/Editor]] widget provides the [[doc:physics/Model]] and pb := sc.NewDynamic(ml, "top", physics.Box, "blue", mass, hsz, math32.Vec3(x, stY, 0), rleft) ``` -The `math32.Quat` quaternion provides all the rotational math used in `xyz` and `physics`, and the `rleft` instance represents a -90 degree rotation about the Z (depth axis), which is what causes the pendulum to start in a horizontal orientation. +The `math32.Quat` quaternion provides all the rotational math used in `xyz` and `physics`, and the `rleft` instance represents a -90 degree rotation about the Z (depth) axis, which is what causes the pendulum to start in a horizontal orientation. -The `NewDynamic` method adds a new dynamic body element with a default visualization (this is an `phyxyz` wrapper around the same method in the physics space). Dynamic elements are updated by the physics engine, while `NewBody` would create a static rigid body element that doesn't move (unless you specifically change its position). The return value is a [[doc:physics/phyxyz/View]] which provides the visualization of a physics body. It uses the `Index` of the body to get updated values. The same visualization can be used for any physics sim with the same order of bodies. +The `NewDynamic` method adds a new dynamic body element with a default visualization (this is a `phyxyz` wrapper around the same method in the `physics` package). Dynamic elements are updated by the physics engine, while `NewBody` would create a static rigid body element that doesn't move (unless you specifically change its position). The return value is a [[doc:physics/phyxyz/Skin]] which provides the visualization of a physics body. It uses the `BodyIndex` of the body to get updated values. ```go pb.SetBodyGroup(1) // no collide across groups ``` -The `Group` property of a body can be set to fine-tune collision logic. Positive-numbered groups only collide with each other and any negative-numbered groups, while negative-numbered groups only collide with positive numbered and not within the group. 0 means it doesn't collide with anything. With the crazy dynamics that emerge with multiple arms, it is good to let them all pass through each other. +The `Group` property of a body can be set to fine-tune collision logic. Positive-numbered groups only collide with each other and any negative-numbered groups, while negative-numbered groups only collide with positive numbered and not within their own group. 0 means it doesn't collide with anything. With the crazy dynamics that emerge with multiple arms, it is good to let them all pass through each other. ```go - ji := pb.NewJointRevolute(ml, nil, math32.Vec3(0, stY, 0), math32.Vec3(0, hsz.Y, 0), math32.Vec3(0, 0, 1)) + ji := sc.NewJointRevolute(ml, nil, pb, math32.Vec3(0, stY, 0), math32.Vec3(0, hsz.Y, 0), math32.Vec3(0, 0, 1)) physics.SetJointTargetPos(ji, 0, 0, 0) physics.SetJointTargetVel(ji, 0, 0, 0) ``` @@ -88,7 +88,142 @@ The remaining code just does this same kind of thing for the further links, and ### Joint control +{id="sim_prismatic" title="Prismatic Joint" collapsed="true"} +```Goal +ed := phyxyz.NewEditor(b) +ed.CameraPos = math32.Vec3(0, 10, 10) +ed.Styler(func(s *styles.Style) { + s.Min.Y.Em(40) +}) + +ed.SetConfigFunc(func() { + ml := ed.Model + sc := ed.Scene + hsz := math32.Vec3(1, 2, 0.5) + mass := float32(0.1) + + obj := sc.NewDynamic(ml, "body", physics.Box, "blue", mass, hsz, math32.Vec3(0, hsz.Y, 0), math32.NewQuat(0,0,0,1)) + ji := sc.NewJointPrismatic(ml, nil, obj, math32.Vec3(-5, 0, 0), math32.Vec3(0, hsz.Y, 0), math32.Vec3(1, 0, 0)) +}) + +// variables to control +pos := float32(1) +stiff := float32(10) +vel := float32(0) +damp := float32(10) + +var posStr, stiffStr, velStr, dampStr string + +ed.SetControlFunc(func(timeStep int) { + physics.SetJointTargetPos(0, 0, pos, stiff) + physics.SetJointTargetVel(0, 0, vel, damp) +}) + +func update() { + posStr = fmt.Sprintf("Pos: %g", pos) + stiffStr = fmt.Sprintf("Stiff: %g", stiff) + velStr = fmt.Sprintf("Vel: %g", vel) + dampStr = fmt.Sprintf("Damp: %g", damp) +} + +update() + +func addSlider(label *string, val *float32, maxVal float32) { + tx := core.NewText(b) + tx.Styler(func(s *styles.Style) { + s.Min.X.Ch(40) // clean rendering with variable width content + }) + core.Bind(label, tx) + sld := core.NewSlider(b).SetMin(0).SetMax(maxVal).SetStep(1).SetEnforceStep(true) + sld.SendChangeOnInput() + sld.OnChange(func(e events.Event) { + update() + tx.UpdateRender() + }) + core.Bind(val, sld) +} + +addSlider(&posStr, &pos, 10) +addSlider(&stiffStr, &stiff, 1000) +addSlider(&velStr, &vel, 2) +addSlider(&dampStr, &damp, 1000) +``` + +This simulation allows interactive control over the parameters of a `Prismatic` joint, which sets the linear position of a body along a given axis, in this case along the horizontal (`X`) axis. +Click the `Step 10000` button and then start moving the sliders to see the effects interactively. Here's what you should observe: + +* `Stiff` (stiffness) determines how quickly the joint responds to the position changes. You can make this variable even stronger in practice (e.g., 10,000). + +* `Damp` (damping) opposes `Stiff` in resisting changes, but some amount of damping is essential to prevent oscillations (definitely try Damp = 0). In general a value above 20 or so seems to be necessary for preventing significant oscillations. + +{id="sim_ball" title="Ball Joint" collapsed="true"} +```Goal +ed := phyxyz.NewEditor(b) +ed.CameraPos = math32.Vec3(0, 10, 10) +ed.Styler(func(s *styles.Style) { + s.Min.Y.Em(40) +}) + +ed.SetConfigFunc(func() { + ml := ed.Model + sc := ed.Scene + hsz := math32.Vec3(0.5, 1.5, 0.2) + mass := float32(0.1) + + obj := sc.NewDynamic(ml, "body", physics.Box, "blue", mass, hsz, math32.Vec3(0, hsz.Y, 0), math32.NewQuat(0,0,0,1)) + ji := sc.NewJointBall(ml, nil, obj, math32.Vec3(0, 0, 0), math32.Vec3(0, -hsz.Y, 0)) +}) + +// variables to control +posX := float32(0) +posY := float32(0) +posZ := float32(0) +stiff := float32(1000) +damp := float32(20) + +var posXstr, posYstr, posZstr, stiffStr, dampStr string + +ed.SetControlFunc(func(timeStep int) { + physics.SetJointTargetPos(0, 0, posX, stiff) + physics.SetJointTargetPos(0, 1, posY, stiff) + physics.SetJointTargetPos(0, 2, posZ, stiff) + physics.SetJointTargetVel(0, 0, 0, damp) + physics.SetJointTargetVel(0, 1, 0, damp) + physics.SetJointTargetVel(0, 2, 0, damp) +}) + +func update() { + posXstr = fmt.Sprintf("Pos X: %g", posX) + posYstr = fmt.Sprintf("Pos Y: %g", posY) + posZstr = fmt.Sprintf("Pos Z: %g", posZ) + stiffStr = fmt.Sprintf("Stiff: %g", stiff) + dampStr = fmt.Sprintf("Damp: %g", damp) +} + +update() + +func addSlider(label *string, val *float32, minVal, maxVal float32) { + tx := core.NewText(b) + tx.Styler(func(s *styles.Style) { + s.Min.X.Ch(40) // clean rendering with variable width content + }) + core.Bind(label, tx) + sld := core.NewSlider(b).SetMin(minVal).SetMax(maxVal).SetStep(0.1).SetEnforceStep(true) + sld.SendChangeOnInput() + sld.OnChange(func(e events.Event) { + update() + tx.UpdateRender() + }) + core.Bind(val, sld) +} + +addSlider(&posXstr, &posX, -3, 3) +addSlider(&posYstr, &posY, -3, 3) +addSlider(&posZstr, &posZ, -3, 3) +addSlider(&stiffStr, &stiff, 0, 1000) +addSlider(&dampStr, &damp, 0, 1000) +``` ## GoSL infrastructure diff --git a/physics/builder/joint.go b/physics/builder/joint.go index 54b97087..a22243f6 100644 --- a/physics/builder/joint.go +++ b/physics/builder/joint.go @@ -240,3 +240,9 @@ func (jd *Joint) PoseToPhysics() { func (jd *Joint) SetTargetPos(dof int32, pos, stiff float32) { physics.SetJointTargetPos(jd.JointIndex, dof, pos, stiff) } + +// SetTargetVel sets the target position for given DoF for +// this joint in the physics model. +func (jd *Joint) SetTargetVel(dof int32, vel, damp float32) { + physics.SetJointTargetVel(jd.JointIndex, dof, vel, damp) +} diff --git a/physics/examples/test1/test1.go b/physics/examples/test1/test1.go index b7e8c5e3..01d8e0ba 100644 --- a/physics/examples/test1/test1.go +++ b/physics/examples/test1/test1.go @@ -7,18 +7,8 @@ package main //go:generate core generate -add-types import ( - "fmt" - - "cogentcore.org/core/colors" "cogentcore.org/core/core" - "cogentcore.org/core/events" - "cogentcore.org/core/icons" "cogentcore.org/core/math32" - "cogentcore.org/core/styles" - "cogentcore.org/core/styles/abilities" - "cogentcore.org/core/tree" - "cogentcore.org/core/xyz" - "cogentcore.org/core/xyz/xyzcore" _ "cogentcore.org/lab/gosl/slbool/slboolcore" // include to get gui views "cogentcore.org/lab/physics" "cogentcore.org/lab/physics/phyxyz" @@ -53,57 +43,21 @@ func (b *Test) Defaults() { } func main() { - // gpu.Debug = true - b := core.NewBody("test1").SetTitle("Physics Test") - split := core.NewSplits(b) - fpanel := core.NewFrame(split) - fpanel.Styler(func(s *styles.Style) { - s.Direction = styles.Column - s.Grow.Set(1, 1) - }) - - bpf := core.NewForm(fpanel) - wpf := core.NewForm(fpanel) - - tbvw := core.NewTabs(split) - scfr, _ := tbvw.NewTab("3D View") - split.SetSplits(0.2, 0.8) - - se := xyzcore.NewSceneEditor(scfr) - se.UpdateWidget() - sc := se.SceneXYZ() - - sc.Background = colors.Scheme.Select.Container - xyz.NewAmbient(sc, "ambient", 0.3, xyz.DirectSun) - - dir := xyz.NewDirectional(sc, "dir", 1, xyz.DirectSun) - dir.Pos.Set(0, 2, 1) - - wr := phyxyz.NewWorld(sc) + b := core.NewBody("test1").SetTitle("Physics Test1") + ed := phyxyz.NewEditor(b) + ed.CameraPos = math32.Vec3(0, 3, 3) bs := &Test{} bs.Defaults() - wl := physics.NewWorld() - wl.GPU = false - - params := physics.GetParams(0) - params.Dt = 0.0001 // leaks balls > 0.0005 - params.SubSteps = 100 // major speedup by inner-stepping - // params.Gravity.Y = 0 - params.ContactRelax = 0.2 // 0.1 seems most physical -- 0.2 getting a bit more ke? - params.Restitution.SetBool(false) // not working! - params.ContactMargin = 0 // 0 for restitution - - bpf.SetStruct(bs) - wpf.SetStruct(params) + ed.SetUserParams(bs) - config := func() { - wr.Reset() - wl.Reset() + ed.SetConfigFunc(func() { + ml := ed.Model + sc := ed.Scene rot := math32.NewQuat(0, 0, 0, 1) // thick := float32(0.1) - fl := wr.NewBody(wl, "floor", physics.Plane, "#D0D0D080", math32.Vec3(10, 0, 10), + fl := sc.NewBody(ml, "floor", physics.Plane, "#D0D0D080", math32.Vec3(10, 0, 10), math32.Vec3(0, 0, 0), rot) fl.SetBodyBounce(bs.Bounce) @@ -111,127 +65,40 @@ func main() { width := height * .4 depth := height * .15 _, _ = width, depth - // b1 := wr.NewDynamic(wl, "body", physics.Box, "purple", 1.0, math32.Vec3(height, height, depth), + // b1 := wr.NewDynamic(ml, "body", physics.Box, "purple", 1.0, math32.Vec3(height, height, depth), // math32.Vec3(0, height*2, 0), rot) - b1 := wr.NewDynamic(wl, "body", physics.Sphere, "purple", 1.0, math32.Vec3(height, height, height), - math32.Vec3(0, height*bs.Height, 0), rot) + // b1 := sc.NewDynamic(ml, "body", physics.Sphere, "purple", 1.0, math32.Vec3(height, height, height), math32.Vec3(0, height*bs.Height, 0), rot) + b1 := sc.NewDynamic(ml, "body", physics.Box, "purple", 1.0, math32.Vec3(width, height, depth), math32.Vec3(0, bs.Size, 0), rot) + physics.SetBodyGroup(b1.BodyIndex, 0) + + // rleft := math32.NewQuatAxisAngle(math32.Vec3(0, 0, 1), -math32.Pi/2) + // b2 := sc.NewDynamic(ml, "nose", physics.Capsule, "blue", .001, math32.Vec3(0.1*depth, 0.1*height, 0.1*depth), math32.Vec3(-depth, 2*bs.Size, 0), rleft) // b1.SetBodyBounce(bs.Bounce) _ = b1 - // bj := wl.NewJointRevolute(-1, b1.DynamicIndex, math32.Vec3(0, 0, 0), math32.Vec3(0, -height, 0), math32.Vec3(0, 1, 0)) - // bj := wl.NewJointPrismatic(-1, b1.DynamicIndex, math32.Vec3(0, 0, 0), math32.Vec3(0, -height, 0), math32.Vec3(1, 0, 0)) + // bj := ml.NewJointRevolute(-1, b1.DynamicIndex, math32.Vec3(0, 0, 0), math32.Vec3(0, -height, 0), math32.Vec3(0, 1, 0)) + // bj := ml.NewJointPrismatic(-1, b1.DynamicIndex, math32.Vec3(0, 0, 0), math32.Vec3(0, -height, 0), math32.Vec3(1, 0, 0)) // // // physics.SetJointControlForce(bj, 0, .1) // physics.SetJointTargetPos(bj, 0, 1, 1) // physics.SetJointTargetVel(bj, 0, 0, 1) - wr.Init(wl) - wr.Update() - } - - config() - - cycle := 0 - - updateView := func() { - bpf.Update() - wpf.Update() - if se.IsVisible() { - se.NeedsRender() - } - } - - sc.Camera.Pose.Pos = math32.Vec3(0, 40, 3.5) - sc.Camera.LookAt(math32.Vec3(0, 5, 0), math32.Vec3(0, 1, 0)) - sc.SaveCamera("3") - - sc.Camera.Pose.Pos = math32.Vec3(-1.33, 2.24, 3.55) - sc.Camera.LookAt(math32.Vec3(0, .5, 0), math32.Vec3(0, 1, 0)) - sc.SaveCamera("2") - - sc.Camera.Pose.Pos = math32.Vec3(0, 20, 30) - sc.Camera.LookAt(math32.Vec3(0, 5, 0), math32.Vec3(0, 1, 0)) - sc.SaveCamera("1") - sc.SaveCamera("default") - - isRunning := false - stop := false - - stepNButton := func(p *tree.Plan, n int) { - nm := fmt.Sprintf("Step %d", n) - tree.AddAt(p, nm, func(w *core.Button) { - w.SetText(nm).SetIcon(icons.PlayArrow). - SetTooltip(fmt.Sprintf("Step state %d times", n)). - OnClick(func(e events.Event) { - if isRunning { - return - } - go func() { - isRunning = true - for range n { - wl.Step() - cycle++ - wr.Update() - if se.IsVisible() { - se.AsyncLock() - se.NeedsRender() - se.AsyncUnlock() - // time.Sleep(1 * time.Nanosecond) - } - if stop { - stop = false - break - } - } - isRunning = false - }() - }) - w.Styler(func(s *styles.Style) { - s.SetAbilities(true, abilities.RepeatClickable) - }) - }) - } - - b.AddTopBar(func(bar *core.Frame) { - core.NewToolbar(bar).Maker(func(p *tree.Plan) { - tree.Add(p, func(w *core.Button) { - w.SetText("Init").SetIcon(icons.Reset). - SetTooltip("Reset physics state back to starting."). - OnClick(func(e events.Event) { - if isRunning { - return - } - wl.InitState() - wr.Update() - updateView() - }) - }) - tree.Add(p, func(w *core.Button) { - w.SetText("Stop").SetIcon(icons.Stop). - SetTooltip("Stop running"). - OnClick(func(e events.Event) { - stop = true - }) - }) - tree.Add(p, func(w *core.Separator) {}) - - stepNButton(p, 1) - stepNButton(p, 10) - stepNButton(p, 100) - stepNButton(p, 1000) - stepNButton(p, 10000) - tree.Add(p, func(w *core.Separator) {}) - - tree.Add(p, func(w *core.Button) { - w.SetText("Rebuild").SetIcon(icons.Reset). - SetTooltip("Rebuild the environment, when you change parameters"). - OnClick(func(e events.Event) { - if isRunning { - return - } - config() - updateView() - }) - }) - }) + // ml.NewJointFixed(b1.DynamicIndex, b2.DynamicIndex, math32.Vec3(-depth, bs.Size, 0), math32.Vec3(-0.1*depth, 0, 0)) + bj := ml.NewJointBall(-1, b1.DynamicIndex, math32.Vec3(0, 0, 0), math32.Vec3(0, -height, 0)) + physics.SetJointTargetPos(bj, 0, 0, 1000) + physics.SetJointTargetVel(bj, 0, 0, 20) + physics.SetJointTargetPos(bj, 1, 0, 1000) + physics.SetJointTargetVel(bj, 1, 0, 20) + physics.SetJointTargetPos(bj, 2, 0, 1000) + physics.SetJointTargetVel(bj, 2, 0, 20) }) + + // ed.SetControlFunc(func(timeStep int) { + // if timeStep >= ps.ForceOn && timeStep < ps.ForceOff { + // fmt.Println(timeStep, "\tforce on:", ps.Force) + // physics.SetJointControlForce(botJoint.JointIndex, 0, ps.Force) + // } else { + // physics.SetJointControlForce(botJoint.JointIndex, 0, 0) + // } + // }) + b.RunMainWindow() } diff --git a/physics/phyxyz/editor.go b/physics/phyxyz/editor.go index 8a4e5e20..355942e2 100644 --- a/physics/phyxyz/editor.go +++ b/physics/phyxyz/editor.go @@ -177,6 +177,37 @@ func (pe *Editor) Restart() bool { return true } +// Step steps the world n times, with updates. Must be called as a goroutine. +func (pe *Editor) Step(n int) { + if pe.isRunning { + return + } + pe.isRunning = true + pe.Model.SetAsCurrent() + pe.toolbar.AsyncLock() + pe.toolbar.UpdateRender() + pe.toolbar.AsyncUnlock() + for range n { + if pe.ControlFunc != nil { + pe.ControlFunc(physics.StepsToMsec(pe.TimeStep)) + } + pe.Model.Step() + pe.TimeStep++ + pe.Scene.Update() + pe.editor.AsyncLock() + pe.editor.NeedsRender() + pe.editor.AsyncUnlock() + if pe.stop { + pe.stop = false + break + } + } + pe.isRunning = false + pe.AsyncLock() + pe.Update() + pe.AsyncUnlock() +} + func (pe *Editor) MakeToolbar(p *tree.Plan) { stepNButton := func(n int) { nm := fmt.Sprintf("Step %d", n) @@ -189,31 +220,7 @@ func (pe *Editor) MakeToolbar(p *tree.Plan) { fmt.Println("still running...") return } - go func() { - pe.isRunning = true - pe.toolbar.AsyncLock() - pe.toolbar.UpdateRender() - pe.toolbar.AsyncUnlock() - for range n { - if pe.ControlFunc != nil { - pe.ControlFunc(physics.StepsToMsec(pe.TimeStep)) - } - pe.Model.Step() - pe.TimeStep++ - pe.Scene.Update() - pe.editor.AsyncLock() - pe.editor.NeedsRender() - pe.editor.AsyncUnlock() - if pe.stop { - pe.stop = false - break - } - } - pe.isRunning = false - pe.AsyncLock() - pe.Update() - pe.AsyncUnlock() - }() + go pe.Step(n) }) w.Styler(func(s *styles.Style) { s.SetAbilities(true, abilities.RepeatClickable) @@ -268,8 +275,11 @@ func (pe *Editor) MakeToolbar(p *tree.Plan) { core.Bind(&pe.Replica, w) w.SetMin(0).SetTooltip(tt) w.Styler(func(s *styles.Style) { - replN := physics.CurModel.ReplicaWorldsN() - pe.Scene.ReplicasView = replN > 0 + replN := int32(0) + if physics.CurModel != nil && pe.Scene != nil { + replN = physics.CurModel.ReplicaWorldsN() + pe.Scene.ReplicasView = replN > 0 + } w.SetMax(float32(replN - 1)) s.SetEnabled(replN > 0) }) diff --git a/physics/shaders/StepSolveJoints.wgsl b/physics/shaders/StepSolveJoints.wgsl index b866044f..469a8b69 100644 --- a/physics/shaders/StepSolveJoints.wgsl +++ b/physics/shaders/StepSolveJoints.wgsl @@ -703,7 +703,7 @@ fn StepSolveJoints(i: u32) { //gosl:kernel } } } - if (jt == Fixed || jt == Prismatic || jt == Revolute || jt == D6) { + if (jt == Fixed || jt == Prismatic || jt == Revolute || jt == Ball || jt == D6) { var qP = xwPQ; var qC = xwCQ; if (QuatDot(qP, qC) < 0) { diff --git a/physics/step_joint.go b/physics/step_joint.go index 98e58ff9..37c7a008 100644 --- a/physics/step_joint.go +++ b/physics/step_joint.go @@ -326,7 +326,7 @@ func StepSolveJoints(i uint32) { //gosl:kernel } } } - if jt == Fixed || jt == Prismatic || jt == Revolute || jt == D6 { + if jt == Fixed || jt == Prismatic || jt == Revolute || jt == Ball || jt == D6 { qP := xwPQ qC := xwCQ // make quats lie in same hemisphere diff --git a/physics/step_joint.goal b/physics/step_joint.goal index 6f5e703e..85e8a7a3 100644 --- a/physics/step_joint.goal +++ b/physics/step_joint.goal @@ -324,7 +324,7 @@ func StepSolveJoints(i uint32) { //gosl:kernel } } } - if jt == Fixed || jt == Prismatic || jt == Revolute || jt == D6 { + if jt == Fixed || jt == Prismatic || jt == Revolute || jt == Ball || jt == D6 { qP := xwPQ qC := xwCQ // make quats lie in same hemisphere From 955d6b7a7872db884289583fbcafe65ceef57534 Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Tue, 30 Dec 2025 09:24:20 +0100 Subject: [PATCH 65/97] physics: mod update --- docs/content/physics.md | 4 ++-- go.mod | 2 +- go.sum | 4 ++-- yaegilab/labsymbols/cogentcore_org-lab-physics-builder.go | 1 + 4 files changed, 6 insertions(+), 5 deletions(-) diff --git a/docs/content/physics.md b/docs/content/physics.md index cfa8baa0..5c36e400 100644 --- a/docs/content/physics.md +++ b/docs/content/physics.md @@ -102,7 +102,7 @@ ed.SetConfigFunc(func() { hsz := math32.Vec3(1, 2, 0.5) mass := float32(0.1) - obj := sc.NewDynamic(ml, "body", physics.Box, "blue", mass, hsz, math32.Vec3(0, hsz.Y, 0), math32.NewQuat(0,0,0,1)) + obj := sc.NewDynamic(ml, "body", physics.Box, "blue", mass, hsz, math32.Vec3(0, hsz.Y, 0), math32.NewQuatIdentity()) ji := sc.NewJointPrismatic(ml, nil, obj, math32.Vec3(-5, 0, 0), math32.Vec3(0, hsz.Y, 0), math32.Vec3(1, 0, 0)) }) @@ -171,7 +171,7 @@ ed.SetConfigFunc(func() { hsz := math32.Vec3(0.5, 1.5, 0.2) mass := float32(0.1) - obj := sc.NewDynamic(ml, "body", physics.Box, "blue", mass, hsz, math32.Vec3(0, hsz.Y, 0), math32.NewQuat(0,0,0,1)) + obj := sc.NewDynamic(ml, "body", physics.Box, "blue", mass, hsz, math32.Vec3(0, hsz.Y, 0), math32.NewQuatIdentity()) ji := sc.NewJointBall(ml, nil, obj, math32.Vec3(0, 0, 0), math32.Vec3(0, -hsz.Y, 0)) }) diff --git a/go.mod b/go.mod index 79c9c110..03464773 100644 --- a/go.mod +++ b/go.mod @@ -10,7 +10,7 @@ go 1.23.4 // https://github.com/googleapis/go-genproto/issues/1015 require ( - cogentcore.org/core v0.3.13-0.20251223055455-950754144c98 + cogentcore.org/core v0.3.13-0.20251230082220-8c1c3d46495c github.com/alecthomas/assert/v2 v2.6.0 github.com/cogentcore/readline v0.1.3 github.com/cogentcore/yaegi v0.0.0-20250622201820-b7838bdd95eb diff --git a/go.sum b/go.sum index 0850edc2..20fda15a 100644 --- a/go.sum +++ b/go.sum @@ -1,5 +1,5 @@ -cogentcore.org/core v0.3.13-0.20251223055455-950754144c98 h1:EMClYWakND3PL5OnsJsUhVuyG9A0JU6IU9iQYAeJk4k= -cogentcore.org/core v0.3.13-0.20251223055455-950754144c98/go.mod h1:eDHnTCy1sBhAKN9NPsSCnBW3VAnwQBNA9nbGMo9r+Xs= +cogentcore.org/core v0.3.13-0.20251230082220-8c1c3d46495c h1:rEIgjick0EYRPkkEdmCP9RSAg5EmTn0jCMyeA107ifQ= +cogentcore.org/core v0.3.13-0.20251230082220-8c1c3d46495c/go.mod h1:eDHnTCy1sBhAKN9NPsSCnBW3VAnwQBNA9nbGMo9r+Xs= github.com/Bios-Marcel/wastebasket/v2 v2.0.3 h1:TkoDPcSqluhLGE+EssHu7UGmLgUEkWg7kNyHyyJ3Q9g= github.com/Bios-Marcel/wastebasket/v2 v2.0.3/go.mod h1:769oPCv6eH7ugl90DYIsWwjZh4hgNmMS3Zuhe1bH6KU= github.com/BurntSushi/toml v0.3.1/go.mod h1:xHWCNGjB5oqiDr8zfno3MHue2Ht5sIBksp03qcyfWMU= diff --git a/yaegilab/labsymbols/cogentcore_org-lab-physics-builder.go b/yaegilab/labsymbols/cogentcore_org-lab-physics-builder.go index 8c7aeaa4..7db80314 100644 --- a/yaegilab/labsymbols/cogentcore_org-lab-physics-builder.go +++ b/yaegilab/labsymbols/cogentcore_org-lab-physics-builder.go @@ -19,6 +19,7 @@ func init() { "DoF": reflect.ValueOf((*builder.DoF)(nil)), "Joint": reflect.ValueOf((*builder.Joint)(nil)), "Object": reflect.ValueOf((*builder.Object)(nil)), + "Physics": reflect.ValueOf((*builder.Physics)(nil)), "Pose": reflect.ValueOf((*builder.Pose)(nil)), "World": reflect.ValueOf((*builder.World)(nil)), } From 189f340e154056fbd1d4526924049410d03aad76 Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Wed, 31 Dec 2025 00:00:39 +0100 Subject: [PATCH 66/97] physics: key bugfix in joint limits compute-- fixes ball joint. get current state before updating. head rotation kinda working in virtroom --- physics/builder/body.go | 16 ++++++++++++++-- physics/builder/joint.go | 21 +++++++++++++++++++-- physics/builder/object.go | 11 +++++++++++ physics/control.go | 16 ++++++++++++++-- physics/control.goal | 16 ++++++++++++++-- physics/examples/test1/test1.go | 1 + physics/examples/virtroom/virtroom.go | 11 +++++++++-- physics/shaders/StepSolveJoints.wgsl | 2 +- physics/step_joint.go | 2 +- physics/step_joint.goal | 2 +- 10 files changed, 85 insertions(+), 13 deletions(-) diff --git a/physics/builder/body.go b/physics/builder/body.go index da50935c..9cb9386a 100644 --- a/physics/builder/body.go +++ b/physics/builder/body.go @@ -164,7 +164,19 @@ func (bd *Body) PoseToPhysics() { physics.SetDynamicPos(bd.DynamicIndex, params.Next, bd.Pose.Pos) physics.SetDynamicQuat(bd.DynamicIndex, params.Next, bd.Pose.Quat) } else { - physics.SetBodyPos(bd.DynamicIndex, bd.Pose.Pos) - physics.SetBodyQuat(bd.DynamicIndex, bd.Pose.Quat) + physics.SetBodyPos(bd.BodyIndex, bd.Pose.Pos) + physics.SetBodyQuat(bd.BodyIndex, bd.Pose.Quat) + } +} + +// PoseFromPhysics gets the current body poses from the physics current state. +func (bd *Body) PoseFromPhysics() { + if bd.DynamicIndex >= 0 { + params := physics.GetParams(0) + bd.Pose.Pos = physics.DynamicPos(bd.DynamicIndex, params.Next) + bd.Pose.Quat = physics.DynamicQuat(bd.DynamicIndex, params.Next) + } else { + bd.Pose.Pos = physics.BodyPos(bd.BodyIndex) + bd.Pose.Quat = physics.BodyQuat(bd.BodyIndex) } } diff --git a/physics/builder/joint.go b/physics/builder/joint.go index a22243f6..a2293368 100644 --- a/physics/builder/joint.go +++ b/physics/builder/joint.go @@ -225,8 +225,8 @@ func (jd *Joint) IsGlobal() bool { return jd.Parent < 0 } -// PoseToPhysics sets the current body poses to the physics current state. -// For Dynamic bodies, sets dynamic state. Also updates world-anchored joints. +// PoseToPhysics sets the current world-anchored joint pose +// to the physics current state. func (jd *Joint) PoseToPhysics() { if !jd.IsGlobal() { return @@ -235,12 +235,29 @@ func (jd *Joint) PoseToPhysics() { physics.SetJointPQuat(jd.JointIndex, jd.PPose.Quat) } +// PoseFromPhysics gets the current world-anchored joint pose +// from the physics current state. +func (jd *Joint) PoseFromPhysics() { + if !jd.IsGlobal() { + return + } + jd.PPose.Pos = physics.JointPPos(jd.JointIndex) + jd.PPose.Quat = physics.JointPQuat(jd.JointIndex) +} + // SetTargetPos sets the target position for given DoF for // this joint in the physics model. func (jd *Joint) SetTargetPos(dof int32, pos, stiff float32) { physics.SetJointTargetPos(jd.JointIndex, dof, pos, stiff) } +// AddTargetPos adds to the target position for given DoF for +// this joint in the physics model, setting stiffness. +func (jd *Joint) AddTargetPos(dof int32, pos, stiff float32) { + cpos := physics.GetJointTargetPos(jd.JointIndex, dof) + physics.SetJointTargetPos(jd.JointIndex, dof, cpos+pos, stiff) +} + // SetTargetVel sets the target position for given DoF for // this joint in the physics model. func (jd *Joint) SetTargetVel(dof int32, vel, damp float32) { diff --git a/physics/builder/object.go b/physics/builder/object.go index 621e867f..ee0693aa 100644 --- a/physics/builder/object.go +++ b/physics/builder/object.go @@ -67,6 +67,17 @@ func (ob *Object) PoseToPhysics() { } } +// PoseFromPhysics gets the current body poses from the physics current state. +// Also updates world-anchored joints. +func (ob *Object) PoseFromPhysics() { + for i := range ob.Bodies { + ob.Body(i).PoseFromPhysics() + } + for i := range ob.Joints { + ob.Joint(i).PoseFromPhysics() + } +} + // Move applies positional and rotational transforms to all bodies, // and world-anchored joints. func (ob *Object) Move(pos math32.Vector3) { diff --git a/physics/control.go b/physics/control.go index bf1823cf..53239298 100644 --- a/physics/control.go +++ b/physics/control.go @@ -53,19 +53,31 @@ func SetJointControlForce(idx, dof int32, value float32) { // SetJointTargetPos sets the target position and stiffness // for given joint, DoF to given values. Stiffness determines // how strongly the joint constraint is enforced -// (0 = not at all; 1 = as strongly as possible). +// (0 = not at all; 1000+ = strongly). func SetJointTargetPos(idx, dof int32, pos, stiff float32) { SetJointControl(idx, dof, JointTargetPos, pos) SetJointControl(idx, dof, JointTargetStiff, stiff) } +// GetJointTargetPos returns the target position +// for given joint, DoF. +func GetJointTargetPos(idx, dof int32) float32 { + return JointControl(idx, dof, JointTargetPos) +} + // SetJointTargetVel sets the target velocity and damping // for given joint, DoF to given values. Damping determines // how strongly the joint constraint is enforced -// (0 = not at all; 1 = as strongly as possible). +// (0 = not at all; 1000+ = strongly). func SetJointTargetVel(idx, dof int32, vel, damp float32) { SetJointControl(idx, dof, JointTargetVel, vel) SetJointControl(idx, dof, JointTargetDamp, damp) } +// GetJointTargetVel returns the target velocity +// for given joint, DoF. +func GetJointTargetVel(idx, dof int32) float32 { + return JointControl(idx, dof, JointTargetVel) +} + //gosl:end diff --git a/physics/control.goal b/physics/control.goal index cee48a46..752bb9ef 100644 --- a/physics/control.goal +++ b/physics/control.goal @@ -51,19 +51,31 @@ func SetJointControlForce(idx, dof int32, value float32) { // SetJointTargetPos sets the target position and stiffness // for given joint, DoF to given values. Stiffness determines // how strongly the joint constraint is enforced -// (0 = not at all; 1 = as strongly as possible). +// (0 = not at all; 1000+ = strongly). func SetJointTargetPos(idx, dof int32, pos, stiff float32) { SetJointControl(idx, dof, JointTargetPos, pos) SetJointControl(idx, dof, JointTargetStiff, stiff) } +// GetJointTargetPos returns the target position +// for given joint, DoF. +func GetJointTargetPos(idx, dof int32) float32 { + return JointControl(idx, dof, JointTargetPos) +} + // SetJointTargetVel sets the target velocity and damping // for given joint, DoF to given values. Damping determines // how strongly the joint constraint is enforced -// (0 = not at all; 1 = as strongly as possible). +// (0 = not at all; 1000+ = strongly). func SetJointTargetVel(idx, dof int32, vel, damp float32) { SetJointControl(idx, dof, JointTargetVel, vel) SetJointControl(idx, dof, JointTargetDamp, damp) } +// GetJointTargetVel returns the target velocity +// for given joint, DoF. +func GetJointTargetVel(idx, dof int32) float32 { + return JointControl(idx, dof, JointTargetVel) +} + //gosl:end diff --git a/physics/examples/test1/test1.go b/physics/examples/test1/test1.go index 01d8e0ba..c790b07a 100644 --- a/physics/examples/test1/test1.go +++ b/physics/examples/test1/test1.go @@ -54,6 +54,7 @@ func main() { ed.SetConfigFunc(func() { ml := ed.Model + ml.GPU = false sc := ed.Scene rot := math32.NewQuat(0, 0, 0, 1) // thick := float32(0.1) diff --git a/physics/examples/virtroom/virtroom.go b/physics/examples/virtroom/virtroom.go index f41bfb1d..b0caa2b6 100644 --- a/physics/examples/virtroom/virtroom.go +++ b/physics/examples/virtroom/virtroom.go @@ -138,6 +138,7 @@ func (ev *Env) MakeWorld(sc *xyz.Scene) { // InitWorld does init on world. func (ev *Env) WorldInit() { //types:add ev.Physics.Init() + ev.UpdateView() } // ConfigView3D makes the 3D view @@ -209,6 +210,7 @@ func (ev *Env) ModelStep() { // StepForward moves Emer forward in current facing direction one step, and takes GrabEyeImg func (ev *Env) StepForward() { //types:add + ev.Emer.PoseFromPhysics() ev.Emer.MoveOnAxisBody(0, 0, 0, 1, -ev.MoveStep) ev.Emer.PoseToPhysics() ev.ModelStep() @@ -216,6 +218,7 @@ func (ev *Env) StepForward() { //types:add // StepBackward moves Emer backward in current facing direction one step, and takes GrabEyeImg func (ev *Env) StepBackward() { //types:add + ev.Emer.PoseFromPhysics() ev.Emer.MoveOnAxisBody(0, 0, 0, 1, ev.MoveStep) ev.Emer.PoseToPhysics() ev.ModelStep() @@ -223,6 +226,7 @@ func (ev *Env) StepBackward() { //types:add // RotBodyLeft rotates emer left and takes GrabEyeImg func (ev *Env) RotBodyLeft() { //types:add + ev.Emer.PoseFromPhysics() ev.Emer.RotateOnAxisBody(0, 0, 1, 0, ev.RotStep) ev.Emer.PoseToPhysics() ev.ModelStep() @@ -230,6 +234,7 @@ func (ev *Env) RotBodyLeft() { //types:add // RotBodyRight rotates emer right and takes GrabEyeImg func (ev *Env) RotBodyRight() { //types:add + ev.Emer.PoseFromPhysics() ev.Emer.RotateOnAxisBody(0, 0, 1, 0, -ev.RotStep) ev.Emer.PoseToPhysics() ev.ModelStep() @@ -237,13 +242,15 @@ func (ev *Env) RotBodyRight() { //types:add // RotHeadLeft rotates emer left and takes GrabEyeImg func (ev *Env) RotHeadLeft() { //types:add - ev.NeckJoint.SetTargetPos(1, ev.RotStep, 10) + ev.Emer.PoseFromPhysics() + ev.NeckJoint.AddTargetPos(1, math32.DegToRad(ev.RotStep), 1000) ev.ModelStep() } // RotHeadRight rotates emer right and takes GrabEyeImg func (ev *Env) RotHeadRight() { //types:add - ev.NeckJoint.SetTargetPos(1, -ev.RotStep, 10) + ev.Emer.PoseFromPhysics() + ev.NeckJoint.AddTargetPos(1, -math32.DegToRad(ev.RotStep), 1000) ev.ModelStep() } diff --git a/physics/shaders/StepSolveJoints.wgsl b/physics/shaders/StepSolveJoints.wgsl index 469a8b69..5d3612b5 100644 --- a/physics/shaders/StepSolveJoints.wgsl +++ b/physics/shaders/StepSolveJoints.wgsl @@ -861,6 +861,6 @@ fn JointAxisLimitsUpdate(dof: i32, axis: vec3, lower: f32,upper: f32, axisL *axisLimitsA = up; } else { *axisLimitsD = Min3(*axisLimitsD, lo); - *axisLimitsA = Max3(*axisLimitsD, up); + *axisLimitsA = Max3(*axisLimitsA, up); } } \ No newline at end of file diff --git a/physics/step_joint.go b/physics/step_joint.go index 37c7a008..42417e8f 100644 --- a/physics/step_joint.go +++ b/physics/step_joint.go @@ -530,7 +530,7 @@ func JointAxisLimitsUpdate(dof int32, axis math32.Vector3, lower, upper float32, *axisLimitsA = up } else { *axisLimitsD = slmath.Min3(*axisLimitsD, lo) - *axisLimitsA = slmath.Max3(*axisLimitsD, up) + *axisLimitsA = slmath.Max3(*axisLimitsA, up) } } diff --git a/physics/step_joint.goal b/physics/step_joint.goal index 85e8a7a3..0058771c 100644 --- a/physics/step_joint.goal +++ b/physics/step_joint.goal @@ -528,7 +528,7 @@ func JointAxisLimitsUpdate(dof int32, axis math32.Vector3, lower, upper float32, *axisLimitsA = up } else { *axisLimitsD = slmath.Min3(*axisLimitsD, lo) - *axisLimitsA = slmath.Max3(*axisLimitsD, up) + *axisLimitsA = slmath.Max3(*axisLimitsA, up) } } From ef9d722bae04a2fd7f144f713f0455dbf5c73432 Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Wed, 31 Dec 2025 09:27:57 +0100 Subject: [PATCH 67/97] physics: target angle method and clamping angle range, ball docs. --- docs/content/physics.md | 20 +++++++++++-------- go.mod | 2 +- go.sum | 4 ++-- physics/control.go | 17 +++++++++++++++- physics/control.goal | 20 +++++++++++++++++-- .../labsymbols/cogentcore_org-lab-physics.go | 3 +++ 6 files changed, 52 insertions(+), 14 deletions(-) diff --git a/docs/content/physics.md b/docs/content/physics.md index 5c36e400..ce1964fa 100644 --- a/docs/content/physics.md +++ b/docs/content/physics.md @@ -179,15 +179,15 @@ ed.SetConfigFunc(func() { posX := float32(0) posY := float32(0) posZ := float32(0) -stiff := float32(1000) +stiff := float32(500) // note: higher values can get unstable for large angles damp := float32(20) var posXstr, posYstr, posZstr, stiffStr, dampStr string ed.SetControlFunc(func(timeStep int) { - physics.SetJointTargetPos(0, 0, posX, stiff) - physics.SetJointTargetPos(0, 1, posY, stiff) - physics.SetJointTargetPos(0, 2, posZ, stiff) + physics.SetJointTargetAngle(0, 0, posX, stiff) + physics.SetJointTargetAngle(0, 1, posY, stiff) + physics.SetJointTargetAngle(0, 2, posZ, stiff) physics.SetJointTargetVel(0, 0, 0, damp) physics.SetJointTargetVel(0, 1, 0, damp) physics.SetJointTargetVel(0, 2, 0, damp) @@ -209,7 +209,7 @@ func addSlider(label *string, val *float32, minVal, maxVal float32) { s.Min.X.Ch(40) // clean rendering with variable width content }) core.Bind(label, tx) - sld := core.NewSlider(b).SetMin(minVal).SetMax(maxVal).SetStep(0.1).SetEnforceStep(true) + sld := core.NewSlider(b).SetMin(minVal).SetMax(maxVal).SetStep(1).SetEnforceStep(true) sld.SendChangeOnInput() sld.OnChange(func(e events.Event) { update() @@ -218,13 +218,17 @@ func addSlider(label *string, val *float32, minVal, maxVal float32) { core.Bind(val, sld) } -addSlider(&posXstr, &posX, -3, 3) -addSlider(&posYstr, &posY, -3, 3) -addSlider(&posZstr, &posZ, -3, 3) +addSlider(&posXstr, &posX, -179, 179) +addSlider(&posYstr, &posY, -179, 179) +addSlider(&posZstr, &posZ, -179, 179) addSlider(&stiffStr, &stiff, 0, 1000) addSlider(&dampStr, &damp, 0, 1000) ``` +The above `Ball` joint example demonstrates a 3 angular degrees-of-freedom joint, using the `SetJointTargetAngle` function that takes degrees as input, and automatically wraps the values in the -180..180 degree (-PI..PI) range, which is the natural range of position values for angular joints. + +You can see that the control can become a bit unstable at extreme angles and angle combinations. Increasing damping and reducing stiffness can help in these situations. + ## GoSL infrastructure As discussed in [[GoSL]], to run equivalent code on the GPU and the CPU (i.e., standard Go), all of the data needs to be represented in large arrays, implemented via [[tensor]]s, and all processing occurs via _parallel for loops_ that effectively process each element of these data arrays in parallel. Enum types are used to define the variables as the last inner-most dimension in the data tensors, e.g., [[doc:physics.BodyVars]], with accessor functions to get the relevant `math32` types (e.g., `math32.Vector3`) across the X,Y,Z components. diff --git a/go.mod b/go.mod index 03464773..a25b59e0 100644 --- a/go.mod +++ b/go.mod @@ -10,7 +10,7 @@ go 1.23.4 // https://github.com/googleapis/go-genproto/issues/1015 require ( - cogentcore.org/core v0.3.13-0.20251230082220-8c1c3d46495c + cogentcore.org/core v0.3.13-0.20251231082551-665c5f304b02 github.com/alecthomas/assert/v2 v2.6.0 github.com/cogentcore/readline v0.1.3 github.com/cogentcore/yaegi v0.0.0-20250622201820-b7838bdd95eb diff --git a/go.sum b/go.sum index 20fda15a..5d04868a 100644 --- a/go.sum +++ b/go.sum @@ -1,5 +1,5 @@ -cogentcore.org/core v0.3.13-0.20251230082220-8c1c3d46495c h1:rEIgjick0EYRPkkEdmCP9RSAg5EmTn0jCMyeA107ifQ= -cogentcore.org/core v0.3.13-0.20251230082220-8c1c3d46495c/go.mod h1:eDHnTCy1sBhAKN9NPsSCnBW3VAnwQBNA9nbGMo9r+Xs= +cogentcore.org/core v0.3.13-0.20251231082551-665c5f304b02 h1:PhP95iKVXeVXIS2bnPi9hEz2F5xSN4hfcF2Lo7Y7Ytw= +cogentcore.org/core v0.3.13-0.20251231082551-665c5f304b02/go.mod h1:eDHnTCy1sBhAKN9NPsSCnBW3VAnwQBNA9nbGMo9r+Xs= github.com/Bios-Marcel/wastebasket/v2 v2.0.3 h1:TkoDPcSqluhLGE+EssHu7UGmLgUEkWg7kNyHyyJ3Q9g= github.com/Bios-Marcel/wastebasket/v2 v2.0.3/go.mod h1:769oPCv6eH7ugl90DYIsWwjZh4hgNmMS3Zuhe1bH6KU= github.com/BurntSushi/toml v0.3.1/go.mod h1:xHWCNGjB5oqiDr8zfno3MHue2Ht5sIBksp03qcyfWMU= diff --git a/physics/control.go b/physics/control.go index 53239298..3d79389c 100644 --- a/physics/control.go +++ b/physics/control.go @@ -6,6 +6,8 @@ package physics +import "cogentcore.org/core/math32" + //gosl:start // JointControlVars are external joint control input variables stored in tensor.Float32. @@ -53,12 +55,25 @@ func SetJointControlForce(idx, dof int32, value float32) { // SetJointTargetPos sets the target position and stiffness // for given joint, DoF to given values. Stiffness determines // how strongly the joint constraint is enforced -// (0 = not at all; 1000+ = strongly). +// (0 = not at all; 1000+ = strongly; angular use lower!). +// For angular joints, values are in radians, see also +// [SetJointTargetAngle]. func SetJointTargetPos(idx, dof int32, pos, stiff float32) { SetJointControl(idx, dof, JointTargetPos, pos) SetJointControl(idx, dof, JointTargetStiff, stiff) } +// SetJointTargetAngle sets the target angular position +// and stiffness for given joint, DoF to given values. +// Stiffness determines how strongly the joint constraint is enforced +// (0 = not at all; 100+ = strongly; angular = lower values). +// Angle is in Degrees, not radians. Usable range is within -180..180 +// which is enforced, and values near the edge can be unstable. +func SetJointTargetAngle(idx, dof int32, angDeg, stiff float32) { + pos := math32.WrapPi(math32.DegToRad(angDeg)) + SetJointTargetPos(idx, dof, pos, stiff) +} + // GetJointTargetPos returns the target position // for given joint, DoF. func GetJointTargetPos(idx, dof int32) float32 { diff --git a/physics/control.goal b/physics/control.goal index 752bb9ef..a8632dc5 100644 --- a/physics/control.goal +++ b/physics/control.goal @@ -4,6 +4,8 @@ package physics +import "cogentcore.org/core/math32" + //gosl:start // JointControlVars are external joint control input variables stored in tensor.Float32. @@ -49,14 +51,28 @@ func SetJointControlForce(idx, dof int32, value float32) { } // SetJointTargetPos sets the target position and stiffness -// for given joint, DoF to given values. Stiffness determines -// how strongly the joint constraint is enforced +// for given joint, DoF to given values. +// Stiffness determines how strongly the joint constraint is enforced // (0 = not at all; 1000+ = strongly). +// For angular joints, values are in radians, see also +// [SetJointTargetAngle]. func SetJointTargetPos(idx, dof int32, pos, stiff float32) { SetJointControl(idx, dof, JointTargetPos, pos) SetJointControl(idx, dof, JointTargetStiff, stiff) } +// SetJointTargetAngle sets the target angular position +// and stiffness for given joint, DoF to given values. +// Stiffness determines how strongly the joint constraint is enforced +// (0 = not at all; 1000+ = strongly). +// Angle is in Degrees, not radians. Usable range is within -180..180 +// which is enforced, and values near the edge can be unstable at higher +// stiffness levels. +func SetJointTargetAngle(idx, dof int32, angDeg, stiff float32) { + pos := math32.WrapPi(math32.DegToRad(angDeg)) + SetJointTargetPos(idx, dof, pos, stiff) +} + // GetJointTargetPos returns the target position // for given joint, DoF. func GetJointTargetPos(idx, dof int32) float32 { diff --git a/yaegilab/labsymbols/cogentcore_org-lab-physics.go b/yaegilab/labsymbols/cogentcore_org-lab-physics.go index 49006bf9..3f7b4e3d 100644 --- a/yaegilab/labsymbols/cogentcore_org-lab-physics.go +++ b/yaegilab/labsymbols/cogentcore_org-lab-physics.go @@ -234,6 +234,8 @@ func init() { "GetJointAngularDoFN": reflect.ValueOf(physics.GetJointAngularDoFN), "GetJointEnabled": reflect.ValueOf(physics.GetJointEnabled), "GetJointLinearDoFN": reflect.ValueOf(physics.GetJointLinearDoFN), + "GetJointTargetPos": reflect.ValueOf(physics.GetJointTargetPos), + "GetJointTargetVel": reflect.ValueOf(physics.GetJointTargetVel), "GetJointType": reflect.ValueOf(physics.GetJointType), "GetParams": reflect.ValueOf(physics.GetParams), "GroupsCollide": reflect.ValueOf(physics.GroupsCollide), @@ -461,6 +463,7 @@ func init() { "SetJointPQuat": reflect.ValueOf(physics.SetJointPQuat), "SetJointPTorque": reflect.ValueOf(physics.SetJointPTorque), "SetJointParent": reflect.ValueOf(physics.SetJointParent), + "SetJointTargetAngle": reflect.ValueOf(physics.SetJointTargetAngle), "SetJointTargetPos": reflect.ValueOf(physics.SetJointTargetPos), "SetJointTargetVel": reflect.ValueOf(physics.SetJointTargetVel), "SetJointType": reflect.ValueOf(physics.SetJointType), From e1d00e3851bd739e91a04d1bf4500ad5c8df7994 Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Thu, 1 Jan 2026 10:32:57 +0100 Subject: [PATCH 68/97] physics: virtroom fixed typo in dim set; forward, backward working, body rotation mostly ok. --- gosl/slmath/vector3.go | 2 +- physics/builder/joint.go | 40 ++++++++++++---- physics/builder/physics.go | 12 +++-- physics/builder/typegen.go | 14 ++++++ physics/control.go | 11 +++-- physics/enumgen.go | 10 ++-- physics/examples/balls/balls.go | 2 +- physics/examples/pendula/pendula.go | 2 +- physics/examples/test1/test1.go | 2 +- physics/examples/virtroom/typegen.go | 2 +- physics/examples/virtroom/virtroom.go | 53 +++++++++++++++------ physics/joint.go | 54 +++++++++++++++------- physics/joint.goal | 54 +++++++++++++++------- physics/shaders/CollisionBroad.wgsl | 3 +- physics/shaders/CollisionNarrow.wgsl | 3 +- physics/shaders/DynamicsCurToNext.wgsl | 3 +- physics/shaders/ForcesFromJoints.wgsl | 3 +- physics/shaders/InitDynamics.wgsl | 3 +- physics/shaders/StepBodyContactDeltas.wgsl | 3 +- physics/shaders/StepBodyContacts.wgsl | 3 +- physics/shaders/StepBodyJointDeltas.wgsl | 3 +- physics/shaders/StepIntegrateBodies.wgsl | 3 +- physics/shaders/StepJointForces.wgsl | 3 +- physics/shaders/StepSolveJoints.wgsl | 5 +- physics/shapegeom_test.go | 2 +- 25 files changed, 208 insertions(+), 87 deletions(-) diff --git a/gosl/slmath/vector3.go b/gosl/slmath/vector3.go index 888e316a..45ed8d0f 100644 --- a/gosl/slmath/vector3.go +++ b/gosl/slmath/vector3.go @@ -104,7 +104,7 @@ func SetDim3(v math32.Vector3, dim int32, val float32) math32.Vector3 { if dim == 1 { nv.Y = val } - if dim == 3 { + if dim == 2 { nv.Z = val } return nv diff --git a/physics/builder/joint.go b/physics/builder/joint.go index a2293368..16f5226d 100644 --- a/physics/builder/joint.go +++ b/physics/builder/joint.go @@ -81,19 +81,22 @@ func (ob *Object) newJoint(typ physics.JointTypes, parent, child *Body, ppos, cp pidx = parent.ObjectIndex } idx := len(ob.Joints) - jt := Joint{Parent: pidx, Child: child.ObjectIndex, Type: typ, LinearDoFN: linDoF, AngularDoFN: angDoF} - jt.PPose.Pos = ppos - jt.CPose.Pos = cpos + ob.Joints = append(ob.Joints, Joint{Parent: pidx, Child: child.ObjectIndex, Type: typ, LinearDoFN: linDoF, AngularDoFN: angDoF}) + jd := ob.Joint(idx) + jd.PPose.Pos = ppos + jd.CPose.Pos = cpos ndof := linDoF + angDoF if ndof > 0 { - jt.DoFs = make([]DoF, linDoF+angDoF) + jd.DoFs = make([]DoF, linDoF+angDoF) for i := range ndof { - jt.DoFs[i].Limit.Min = -physics.JointLimitUnlimited - jt.DoFs[i].Limit.Max = physics.JointLimitUnlimited + dof := jd.DoF(i) + dof.Limit.Min = -physics.JointLimitUnlimited + dof.Limit.Max = physics.JointLimitUnlimited + dof.TargetStiff = 1000 + dof.TargetDamp = 100 } } - ob.Joints = append(ob.Joints, jt) - return &ob.Joints[idx] + return jd } // NewJointFixed adds a new Fixed joint as a child of given parent. @@ -168,11 +171,24 @@ func (ob *Object) NewJointDistance(parent, child *Body, ppos, cpos math32.Vector // to these positions as well). // Sets relative rotation matricies to identity by default. // Use [SetJointDoF] to set the remaining DoF parameters. -func (ob *Object) NewJointFree(ml *physics.Model, parent, child *Body, ppos, cpos math32.Vector3) *Joint { +func (ob *Object) NewJointFree(parent, child *Body, ppos, cpos math32.Vector3) *Joint { jt := ob.newJoint(physics.Free, parent, child, ppos, cpos, 0, 0) return jt } +// NewJointPlaneXZ adds a new 3 DoF Planar motion joint suitable for +// controlling the motion of a body on the standard X-Z play (Y = up). +// The first two linear DoF control position in X, Z, and the third +// angular DoF controls rotation in the plane (along the Y axis). +// Use -1 for parent to add a world-anchored joint (typical). +// ppos, cpos are the relative positions from the parent, child. +// Sets relative rotation matricies to identity by default. +// Use [SetJointDoF] to set the remaining DoF parameters. +func (ob *Object) NewJointPlaneXZ(parent, child *Body, ppos, cpos math32.Vector3) *Joint { + jt := ob.newJoint(physics.PlaneXZ, parent, child, ppos, cpos, 2, 1) + return jt +} + // NewPhysicsJoint makes the physics joint for joint func (jd *Joint) NewPhysicsJoint(ml *physics.Model, ob *Object) int32 { pi := jd.Parent @@ -195,6 +211,8 @@ func (jd *Joint) NewPhysicsJoint(ml *physics.Model, ob *Object) int32 { ji = ml.NewJointBall(pdi, cdi, jd.PPose.Pos, jd.CPose.Pos) case physics.Free: ji = ml.NewJointFree(pdi, cdi, jd.PPose.Pos, jd.CPose.Pos) + case physics.PlaneXZ: + ji = ml.NewJointPlaneXZ(pdi, cdi, jd.PPose.Pos, jd.CPose.Pos) } for i := range jd.LinearDoFN { d := jd.DoF(i) @@ -203,14 +221,16 @@ func (jd *Joint) NewPhysicsJoint(ml *physics.Model, ob *Object) int32 { physics.SetJointDoF(ji, di, physics.JointLimitUpper, d.Limit.Max) physics.SetJointTargetPos(ji, di, d.TargetPos, d.TargetStiff) physics.SetJointTargetVel(ji, di, d.TargetVel, d.TargetDamp) + d.Axis = physics.JointAxis(ji, di) } for i := range jd.AngularDoFN { - d := jd.DoF(i) di := int32(i + jd.LinearDoFN) + d := jd.DoF(int(di)) physics.SetJointDoF(ji, di, physics.JointLimitLower, d.Limit.Min) physics.SetJointDoF(ji, di, physics.JointLimitUpper, d.Limit.Max) physics.SetJointTargetPos(ji, di, d.TargetPos, d.TargetStiff) physics.SetJointTargetVel(ji, di, d.TargetVel, d.TargetDamp) + d.Axis = physics.JointAxis(ji, di) } jd.JointIndex = ji // fmt.Println("\t\tjoint:", pdi, cdi, jd.Type) diff --git a/physics/builder/physics.go b/physics/builder/physics.go index 8eb46b98..e13b5037 100644 --- a/physics/builder/physics.go +++ b/physics/builder/physics.go @@ -40,11 +40,17 @@ func (ph *Physics) Init() { } } -// Step advances the physics world n steps, and then -// updates the Scene. +// Step advances the physics world n steps, updating the scene every time. func (ph *Physics) Step(n int) { + for range n { + ph.Model.Step() + ph.Scene.Update() + } +} + +// StepQuiet advances the physics world n steps. +func (ph *Physics) StepQuiet(n int) { for range n { ph.Model.Step() } - ph.Scene.Update() } diff --git a/physics/builder/typegen.go b/physics/builder/typegen.go index 73d4e7e8..e5d11a45 100644 --- a/physics/builder/typegen.go +++ b/physics/builder/typegen.go @@ -195,6 +195,20 @@ func (t *Object) SetBodies(v ...Body) *Object { t.Bodies = v; return t } // Joint indexes here refer strictly within bodies. func (t *Object) SetJoints(v ...Joint) *Object { t.Joints = v; return t } +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics/builder.Physics", IDName: "physics", Doc: "Physics provides a container and manager for the main physics elements:\n[Builder], [physics.Model], and [phyxyz.Scene]. This is helpful for\nmodels used within other apps (e.g., an AI simulation), whereas\n[phyxyz.Editor] provides a standalone GUI interface for testing models.", Fields: []types.Field{{Name: "Model", Doc: "Model has the physics Model."}, {Name: "Builder", Doc: "Builder for configuring the Model."}, {Name: "Scene", Doc: "Scene for visualizing the Model"}}}) + +// SetModel sets the [Physics.Model]: +// Model has the physics Model. +func (t *Physics) SetModel(v *physics.Model) *Physics { t.Model = v; return t } + +// SetBuilder sets the [Physics.Builder]: +// Builder for configuring the Model. +func (t *Physics) SetBuilder(v *Builder) *Physics { t.Builder = v; return t } + +// SetScene sets the [Physics.Scene]: +// Scene for visualizing the Model +func (t *Physics) SetScene(v *phyxyz.Scene) *Physics { t.Scene = v; return t } + var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics/builder.Pose", IDName: "pose", Doc: "Pose represents the 3D position and rotation.", Methods: []types.Method{{Name: "MoveOnAxis", Doc: "MoveOnAxis moves (translates) the specified distance on the specified local axis,\nrelative to the current rotation orientation.\nThe axis is normalized prior to aplying the distance factor.\nSets the LinVel to motion vector.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z", "dist"}}, {Name: "MoveOnAxisAbs", Doc: "MoveOnAxisAbs moves (translates) the specified distance on the specified local axis,\nin absolute X,Y,Z coordinates (does not apply the Quat rotation factor.\nThe axis is normalized prior to aplying the distance factor.\nSets the LinVel to motion vector.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z", "dist"}}, {Name: "SetEulerRotation", Doc: "SetEulerRotation sets the rotation in Euler angles (degrees).", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z"}}, {Name: "EulerRotation", Doc: "EulerRotation returns the current rotation in Euler angles (degrees).", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Returns: []string{"Vector3"}}, {Name: "SetAxisRotation", Doc: "SetAxisRotation sets rotation from local axis and angle in degrees.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z", "angle"}}, {Name: "RotateOnAxis", Doc: "RotateOnAxis rotates around the specified local axis the specified angle in degrees.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z", "angle"}}, {Name: "RotateEuler", Doc: "RotateEuler rotates by given Euler angles (in degrees) relative to existing rotation.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Args: []string{"x", "y", "z"}}}, Fields: []types.Field{{Name: "Pos", Doc: "Pos is the position of center of mass of object."}, {Name: "Quat", Doc: "Quat is the rotation specified as a quaternion."}}}) // SetPos sets the [Pose.Pos]: diff --git a/physics/control.go b/physics/control.go index 3d79389c..296af7ff 100644 --- a/physics/control.go +++ b/physics/control.go @@ -53,9 +53,9 @@ func SetJointControlForce(idx, dof int32, value float32) { } // SetJointTargetPos sets the target position and stiffness -// for given joint, DoF to given values. Stiffness determines -// how strongly the joint constraint is enforced -// (0 = not at all; 1000+ = strongly; angular use lower!). +// for given joint, DoF to given values. +// Stiffness determines how strongly the joint constraint is enforced +// (0 = not at all; 1000+ = strongly). // For angular joints, values are in radians, see also // [SetJointTargetAngle]. func SetJointTargetPos(idx, dof int32, pos, stiff float32) { @@ -66,9 +66,10 @@ func SetJointTargetPos(idx, dof int32, pos, stiff float32) { // SetJointTargetAngle sets the target angular position // and stiffness for given joint, DoF to given values. // Stiffness determines how strongly the joint constraint is enforced -// (0 = not at all; 100+ = strongly; angular = lower values). +// (0 = not at all; 1000+ = strongly). // Angle is in Degrees, not radians. Usable range is within -180..180 -// which is enforced, and values near the edge can be unstable. +// which is enforced, and values near the edge can be unstable at higher +// stiffness levels. func SetJointTargetAngle(idx, dof int32, angDeg, stiff float32) { pos := math32.WrapPi(math32.DegToRad(angDeg)) SetJointTargetPos(idx, dof, pos, stiff) diff --git a/physics/enumgen.go b/physics/enumgen.go index 674446b8..22d5b164 100644 --- a/physics/enumgen.go +++ b/physics/enumgen.go @@ -237,20 +237,20 @@ func (i GPUVars) MarshalText() ([]byte, error) { return []byte(i.String()), nil // UnmarshalText implements the [encoding.TextUnmarshaler] interface. func (i *GPUVars) UnmarshalText(text []byte) error { return enums.UnmarshalText(i, text, "GPUVars") } -var _JointTypesValues = []JointTypes{0, 1, 2, 3, 4, 5, 6} +var _JointTypesValues = []JointTypes{0, 1, 2, 3, 4, 5, 6, 7} // JointTypesN is the highest valid value for type JointTypes, plus one. // //gosl:start -const JointTypesN JointTypes = 7 +const JointTypesN JointTypes = 8 //gosl:end -var _JointTypesValueMap = map[string]JointTypes{`Prismatic`: 0, `Revolute`: 1, `Ball`: 2, `Fixed`: 3, `Free`: 4, `Distance`: 5, `D6`: 6} +var _JointTypesValueMap = map[string]JointTypes{`Prismatic`: 0, `Revolute`: 1, `Ball`: 2, `Fixed`: 3, `Free`: 4, `Distance`: 5, `D6`: 6, `PlaneXZ`: 7} -var _JointTypesDescMap = map[JointTypes]string{0: `Prismatic allows translation along a single axis (slider): 1 DoF.`, 1: `Revolute allows rotation about a single axis (axel): 1 DoF.`, 2: `Ball allows rotation about all three axes (3 DoF, quaternion).`, 3: `Fixed locks all relative motion: 0 DoF.`, 4: `Free allows full 6-DoF motion (translation and rotation).`, 5: `Distance keeps two bodies a distance within joint limits: 6 DoF.`, 6: `D6 is a generic 6-DoF joint.`} +var _JointTypesDescMap = map[JointTypes]string{0: `Prismatic allows translation along a single axis (slider): 1 DoF.`, 1: `Revolute allows rotation about a single axis (axel): 1 DoF.`, 2: `Ball allows rotation about all three axes (3 DoF, quaternion).`, 3: `Fixed locks all relative motion: 0 DoF.`, 4: `Free allows full 6-DoF motion (translation and rotation).`, 5: `Distance keeps two bodies a distance within joint limits: 6 DoF.`, 6: `D6 is a generic 6-DoF joint.`, 7: `PlaneXZ is a version of D6 for navigation in the X-Z plane, which creates 2 linear DoF (X, Z) and 1 angular DoF (around Y axis).`} -var _JointTypesMap = map[JointTypes]string{0: `Prismatic`, 1: `Revolute`, 2: `Ball`, 3: `Fixed`, 4: `Free`, 5: `Distance`, 6: `D6`} +var _JointTypesMap = map[JointTypes]string{0: `Prismatic`, 1: `Revolute`, 2: `Ball`, 3: `Fixed`, 4: `Free`, 5: `Distance`, 6: `D6`, 7: `PlaneXZ`} // String returns the string representation of this JointTypes value. func (i JointTypes) String() string { return enums.String(i, _JointTypesMap) } diff --git a/physics/examples/balls/balls.go b/physics/examples/balls/balls.go index cd8ea1ba..e44ffed4 100644 --- a/physics/examples/balls/balls.go +++ b/physics/examples/balls/balls.go @@ -73,7 +73,7 @@ func main() { ed.SetConfigFunc(func() { ml := ed.Model sc := ed.Scene - rot := math32.NewQuat(0, 0, 0, 1) + rot := math32.NewQuatIdentity() sc.NewBody(ml, "floor", physics.Plane, "#D0D0D080", math32.Vec3(0, 0, 0), math32.Vec3(0, 0, 0), rot) diff --git a/physics/examples/pendula/pendula.go b/physics/examples/pendula/pendula.go index 5f48b29a..5b407398 100644 --- a/physics/examples/pendula/pendula.go +++ b/physics/examples/pendula/pendula.go @@ -93,7 +93,7 @@ func main() { ml := ed.Model sc := ed.Scene - rot := math32.NewQuat(0, 0, 0, 1) + rot := math32.NewQuatIdentity() rleft := math32.NewQuatAxisAngle(math32.Vec3(0, 0, 1), -math32.Pi/2) if ps.StartVert { diff --git a/physics/examples/test1/test1.go b/physics/examples/test1/test1.go index c790b07a..a8492e9e 100644 --- a/physics/examples/test1/test1.go +++ b/physics/examples/test1/test1.go @@ -56,7 +56,7 @@ func main() { ml := ed.Model ml.GPU = false sc := ed.Scene - rot := math32.NewQuat(0, 0, 0, 1) + rot := math32.NewQuatIdentity() // thick := float32(0.1) fl := sc.NewBody(ml, "floor", physics.Plane, "#D0D0D080", math32.Vec3(10, 0, 10), math32.Vec3(0, 0, 0), rot) diff --git a/physics/examples/virtroom/typegen.go b/physics/examples/virtroom/typegen.go index 20f9a010..0cdecccd 100644 --- a/physics/examples/virtroom/typegen.go +++ b/physics/examples/virtroom/typegen.go @@ -6,4 +6,4 @@ import ( "cogentcore.org/core/types" ) -var _ = types.AddType(&types.Type{Name: "main.Env", IDName: "env", Doc: "Env encapsulates the virtual environment", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Methods: []types.Method{{Name: "WorldInit", Doc: "InitWorld does init on world.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "GrabEyeImg", Doc: "GrabEyeImg takes a snapshot from the perspective of Emer's right eye", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "StepForward", Doc: "StepForward moves Emer forward in current facing direction one step, and takes GrabEyeImg", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "StepBackward", Doc: "StepBackward moves Emer backward in current facing direction one step, and takes GrabEyeImg", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "RotBodyLeft", Doc: "RotBodyLeft rotates emer left and takes GrabEyeImg", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "RotBodyRight", Doc: "RotBodyRight rotates emer right and takes GrabEyeImg", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "RotHeadLeft", Doc: "RotHeadLeft rotates emer left and takes GrabEyeImg", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "RotHeadRight", Doc: "RotHeadRight rotates emer right and takes GrabEyeImg", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}}, Fields: []types.Field{{Name: "EmerAngry", Doc: "if true, emer is angry: changes face color"}, {Name: "EmerHt", Doc: "height of emer"}, {Name: "MoveStep", Doc: "how far to move every step"}, {Name: "RotStep", Doc: "how far to rotate every step"}, {Name: "Width", Doc: "width of room"}, {Name: "Depth", Doc: "depth of room"}, {Name: "Height", Doc: "height of room"}, {Name: "Thick", Doc: "thickness of walls of room"}, {Name: "DepthVals", Doc: "current depth map"}, {Name: "Camera", Doc: "offscreen render camera settings"}, {Name: "DepthMap", Doc: "color map to use for rendering depth map"}, {Name: "World", Doc: "The whole physics World, including visualization."}, {Name: "SceneEditor", Doc: "3D visualization of the Scene"}, {Name: "Emer", Doc: "emer object"}, {Name: "EyeR", Doc: "Right eye of emer"}, {Name: "Contacts", Doc: "contacts from last step, for body"}, {Name: "EyeRImg", Doc: "snapshot image"}, {Name: "DepthImage", Doc: "depth map image"}}}) +var _ = types.AddType(&types.Type{Name: "main.Env", IDName: "env", Doc: "Env encapsulates the virtual environment", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Methods: []types.Method{{Name: "WorldInit", Doc: "InitWorld does init on world.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "GrabEyeImg", Doc: "GrabEyeImg takes a snapshot from the perspective of Emer's right eye", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "ModelStep", Doc: "ModelStep does one step of the physics model.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "StepForward", Doc: "StepForward moves Emer forward in current facing direction one step,\nand takes GrabEyeImg", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "StepBackward", Doc: "StepBackward moves Emer backward in current facing direction one step, and takes GrabEyeImg", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "RotBodyLeft", Doc: "RotBodyLeft rotates emer left and takes GrabEyeImg", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "RotBodyRight", Doc: "RotBodyRight rotates emer right and takes GrabEyeImg", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "RotHeadLeft", Doc: "RotHeadLeft rotates emer left and takes GrabEyeImg", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "RotHeadRight", Doc: "RotHeadRight rotates emer right and takes GrabEyeImg", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}}, Fields: []types.Field{{Name: "EmerAngry", Doc: "if true, emer is angry: changes face color"}, {Name: "EmerHt", Doc: "height of emer"}, {Name: "MoveStep", Doc: "how far to move every step"}, {Name: "RotStep", Doc: "how far to rotate every step"}, {Name: "Width", Doc: "width of room"}, {Name: "Depth", Doc: "depth of room"}, {Name: "Height", Doc: "height of room"}, {Name: "Thick", Doc: "thickness of walls of room"}, {Name: "DepthVals", Doc: "current depth map"}, {Name: "Camera", Doc: "offscreen render camera settings"}, {Name: "DepthMap", Doc: "color map to use for rendering depth map"}, {Name: "Physics", Doc: "The core physics elements: Model, Builder, Scene"}, {Name: "SceneEditor", Doc: "3D visualization of the Scene"}, {Name: "Emer", Doc: "emer object"}, {Name: "EmerJoint", Doc: "emer PlaneXZ joint for controlling motion"}, {Name: "EyeR", Doc: "Right eye of emer"}, {Name: "EyeRImg", Doc: "snapshot image"}, {Name: "NeckJoint", Doc: "ball joint for the neck."}, {Name: "DepthImage", Doc: "depth map image"}}}) diff --git a/physics/examples/virtroom/virtroom.go b/physics/examples/virtroom/virtroom.go index b0caa2b6..6afc7112 100644 --- a/physics/examples/virtroom/virtroom.go +++ b/physics/examples/virtroom/virtroom.go @@ -60,6 +60,9 @@ type Env struct { //types:add // how far to rotate every step RotStep float32 + // number of model steps to take + ModelSteps int + // width of room Width float32 @@ -90,6 +93,9 @@ type Env struct { //types:add // emer object Emer *builder.Object `display:"-"` + // emer PlaneXZ joint for controlling motion + EmerJoint *builder.Joint + // Right eye of emer EyeR *builder.Body `display:"-"` @@ -111,6 +117,7 @@ func (ev *Env) Defaults() { ev.EmerHt = 1 ev.MoveStep = ev.EmerHt * .2 ev.RotStep = 15 + ev.ModelSteps = 100 ev.DepthMap = core.ColorMapName("ColdHot") ev.Camera.Defaults() ev.Camera.FOV = 90 @@ -119,6 +126,7 @@ func (ev *Env) Defaults() { func (ev *Env) MakeWorld(sc *xyz.Scene) { ev.Physics.Model = physics.NewModel() ev.Physics.Builder = builder.NewBuilder() + ev.Physics.Model.GPU = false sc.Background = colors.Scheme.Select.Container xyz.NewAmbient(sc, "ambient", 0.3, xyz.DirectSun) @@ -182,9 +190,9 @@ func (ev *Env) UpdateView() { } // ModelStep does one step of the physics model. -func (ev *Env) ModelStep() { - physics.ToGPU(physics.DynamicsVar) - ev.Physics.Step(1) +func (ev *Env) ModelStep() { //types:add + // physics.ToGPU(physics.DynamicsVar) + ev.Physics.Step(ev.ModelSteps) // cts := pw.WorldCollide(physics.DynsTopGps) // ev.Contacts = nil // for _, cl := range cts { @@ -208,41 +216,47 @@ func (ev *Env) ModelStep() { ev.UpdateView() } -// StepForward moves Emer forward in current facing direction one step, and takes GrabEyeImg +// StepForward moves Emer forward in current facing direction one step, +// and takes GrabEyeImg func (ev *Env) StepForward() { //types:add + // doesn't integrate well with joints.. ev.Emer.PoseFromPhysics() - ev.Emer.MoveOnAxisBody(0, 0, 0, 1, -ev.MoveStep) - ev.Emer.PoseToPhysics() + // ev.Emer.MoveOnAxisBody(0, 0, 0, 1, -ev.MoveStep) + // ev.Emer.PoseToPhysics() + ev.EmerJoint.AddTargetPos(1, -ev.MoveStep, 1000) // z axis ev.ModelStep() } // StepBackward moves Emer backward in current facing direction one step, and takes GrabEyeImg func (ev *Env) StepBackward() { //types:add ev.Emer.PoseFromPhysics() - ev.Emer.MoveOnAxisBody(0, 0, 0, 1, ev.MoveStep) - ev.Emer.PoseToPhysics() + // ev.Emer.MoveOnAxisBody(0, 0, 0, 1, ev.MoveStep) + // ev.Emer.PoseToPhysics() + ev.EmerJoint.AddTargetPos(1, ev.MoveStep, 1000) ev.ModelStep() } // RotBodyLeft rotates emer left and takes GrabEyeImg func (ev *Env) RotBodyLeft() { //types:add ev.Emer.PoseFromPhysics() - ev.Emer.RotateOnAxisBody(0, 0, 1, 0, ev.RotStep) - ev.Emer.PoseToPhysics() + // ev.Emer.RotateOnAxisBody(0, 0, 1, 0, ev.RotStep) + // ev.Emer.PoseToPhysics() + ev.EmerJoint.AddTargetPos(2, math32.DegToRad(ev.RotStep), 1000) ev.ModelStep() } // RotBodyRight rotates emer right and takes GrabEyeImg func (ev *Env) RotBodyRight() { //types:add ev.Emer.PoseFromPhysics() - ev.Emer.RotateOnAxisBody(0, 0, 1, 0, -ev.RotStep) - ev.Emer.PoseToPhysics() + // ev.Emer.RotateOnAxisBody(0, 0, 1, 0, -ev.RotStep) + // ev.Emer.PoseToPhysics() + ev.EmerJoint.AddTargetPos(2, math32.DegToRad(-ev.RotStep), 1000) ev.ModelStep() } // RotHeadLeft rotates emer left and takes GrabEyeImg func (ev *Env) RotHeadLeft() { //types:add - ev.Emer.PoseFromPhysics() + // ev.Emer.PoseFromPhysics() ev.NeckJoint.AddTargetPos(1, math32.DegToRad(ev.RotStep), 1000) ev.ModelStep() } @@ -282,7 +296,7 @@ func (ev *Env) MakeEmer(wl *builder.World, name string, height float32) { hd := hh * .15 headsz := hd * 1.5 eyesz := headsz * .2 - mass := float32(50) // kg + mass := float32(1) // kg rot := math32.NewQuatIdentity() obj := wl.NewObject() ev.Emer = obj @@ -290,9 +304,12 @@ func (ev *Env) MakeEmer(wl *builder.World, name string, height float32) { emr := obj.NewDynamicSkin(sc, name+"_body", physics.Box, "purple", mass, math32.Vec3(hw, hh, hd), math32.Vec3(0, hh, 0), rot) // body := physics.NewCapsule(emr, "body", math32.Vec3(0, hh, 0), hh, hw) // body := physics.NewCylinder(emr, "body", math32.Vec3(0, hh, 0), hh, hw) + ev.EmerJoint = obj.NewJointPlaneXZ(nil, emr, math32.Vec3(0, 0, 0), math32.Vec3(0, -hh, 0)) + emr.Group = 0 headPos := math32.Vec3(0, 2*hh+headsz, 0) head := obj.NewDynamicSkin(sc, name+"_head", physics.Box, "tan", mass*.1, math32.Vec3(headsz, headsz, headsz), headPos, rot) + head.Group = 0 hdsk := head.Skin hdsk.InitSkin = func(sld *xyz.Solid) { hdsk.BoxInit(sld) @@ -308,10 +325,12 @@ func (ev *Env) MakeEmer(wl *builder.World, name string, height float32) { eyeoff := math32.Vec3(-headsz*.6, headsz*.1, -(headsz + eyesz*.3)) bd := obj.NewDynamicSkin(sc, name+"_eye-l", physics.Box, "green", mass*.001, math32.Vec3(eyesz, eyesz*.5, eyesz*.2), headPos.Add(eyeoff), rot) + bd.Group = 0 obj.NewJointBall(head, bd, eyeoff, math32.Vec3(0, 0, -eyesz*.3)) eyeoff = math32.Vec3(headsz*.6, headsz*.1, -(headsz + eyesz*.3)) ev.EyeR = obj.NewDynamicSkin(sc, name+"_eye-r", physics.Box, "green", mass*.001, math32.Vec3(eyesz, eyesz*.5, eyesz*.2), headPos.Add(eyeoff), rot) + ev.EyeR.Group = 0 obj.NewJointBall(head, ev.EyeR, eyeoff, math32.Vec3(0, 0, -eyesz*.3)) } @@ -388,6 +407,12 @@ func (ev *Env) MakeToolbar(p *tree.Plan) { }) tree.Add(p, func(w *core.Separator) {}) + tree.Add(p, func(w *core.FuncButton) { + w.SetFunc(ev.ModelStep).SetText("Step").SetIcon(icons.SkipNext). + Styler(func(s *styles.Style) { + s.SetAbilities(true, abilities.RepeatClickable) + }) + }) tree.Add(p, func(w *core.FuncButton) { w.SetFunc(ev.StepForward).SetText("Fwd").SetIcon(icons.SkipNext). Styler(func(s *styles.Style) { diff --git a/physics/joint.go b/physics/joint.go index aa3f5285..965b3abd 100644 --- a/physics/joint.go +++ b/physics/joint.go @@ -7,6 +7,7 @@ package physics import ( + // "fmt" "math" "cogentcore.org/core/math32" @@ -41,6 +42,10 @@ const ( // D6 is a generic 6-DoF joint. D6 + + // PlaneXZ is a version of D6 for navigation in the X-Z plane, + // which creates 2 linear DoF (X, Z) and 1 angular DoF (around Y axis). + PlaneXZ ) // JointVars are joint state variables stored in tensor.Float32. @@ -372,7 +377,7 @@ func SetJointDoF(idx, dof int32, vr JointDoFVars, value float32) { //gosl:end func (ml *Model) JointDefaults(idx int32) { - rot := math32.NewQuat(0, 0, 0, 1) + rot := math32.NewQuatIdentity() SetJointPQuat(idx, rot) SetJointCQuat(idx, rot) } @@ -438,6 +443,22 @@ func (ml *Model) NewJointBall(parent, child int32, ppos, cpos math32.Vector3) in return idx } +// NewJointPlaneXZ adds a new 3 DoF Planar motion joint suitable for +// controlling the motion of a body on the standard X-Z plane (Y = up). +// The first two linear DoF control position in X, Z, and the third +// angular DoF controls rotation in the plane (along the Y axis). +// Use -1 for parent to add a world-anchored joint (typical). +// ppos, cpos are the relative positions from the parent, child. +// Sets relative rotation matricies to identity by default. +// Use [SetJointDoF] to set the remaining DoF parameters. +func (ml *Model) NewJointPlaneXZ(parent, child int32, ppos, cpos math32.Vector3) int32 { + idx := ml.NewJointD6(parent, child, ppos, cpos, 2, 1) + ml.newJointDoF(idx, 0, math32.Vec3(1, 0, 0)) + ml.newJointDoF(idx, 1, math32.Vec3(0, 0, 1)) + ml.newJointDoF(idx, 2, math32.Vec3(0, 1, 0)) + return idx +} + // NewJointDistance adds a new Distance joint (6 DoF) // between parent and child dynamic object indexes, // with distance constrained only on the first linear X axis. @@ -465,27 +486,28 @@ func (ml *Model) NewJointDistance(parent, child int32, ppos, cpos math32.Vector3 return idx } -// NewJointFree adds a new Free joint -// between parent and child dynamic object indexes, -// with distance constrained only on the first linear X axis. +// NewJointD6 adds a new D6 6 DoF joint with given number of actual +// linear and angular degrees-of-freedom, +// between parent and child dynamic object indexes. +// Use -1 for parent to add a world-anchored joint. +// ppos, cpos are the relative positions from the parent, child. +// Sets relative rotation matricies to identity by default. +func (ml *Model) NewJointD6(parent, child int32, ppos, cpos math32.Vector3, linDoF, angDoF int32) int32 { + idx := ml.newJoint(D6, parent, child, ppos, cpos) + SetJointLinearDoFN(idx, linDoF) + SetJointAngularDoFN(idx, angDoF) + return idx +} + +// NewJointFree adds a new Free joint (of which there is little point) +// between parent and child dynamic object indexes. // Use -1 for parent to add a world-anchored joint. // ppos, cpos are the relative positions from the parent, child. // Sets relative rotation matricies to identity by default. -// Use [SetJointDoF] to set the remaining DoF parameters. func (ml *Model) NewJointFree(parent, child int32, ppos, cpos math32.Vector3) int32 { - idx := ml.newJoint(Distance, parent, child, ppos, cpos) + idx := ml.newJoint(Free, parent, child, ppos, cpos) SetJointLinearDoFN(idx, 0) SetJointAngularDoFN(idx, 0) - for d := range math32.W { - axis := math32.Vector3{} - axis.SetDim(d, 1) - ml.newJointDoF(idx, int32(d), axis) - } - for d := range math32.W { - axis := math32.Vector3{} - axis.SetDim(d, 1) - ml.newJointDoF(idx, int32(d), axis) - } return idx } diff --git a/physics/joint.goal b/physics/joint.goal index 79a8d7b6..d931421d 100644 --- a/physics/joint.goal +++ b/physics/joint.goal @@ -5,6 +5,7 @@ package physics import ( + // "fmt" "math" "cogentcore.org/core/math32" @@ -39,6 +40,10 @@ const ( // D6 is a generic 6-DoF joint. D6 + + // PlaneXZ is a version of D6 for navigation in the X-Z plane, + // which creates 2 linear DoF (X, Z) and 1 angular DoF (around Y axis). + PlaneXZ ) // JointVars are joint state variables stored in tensor.Float32. @@ -372,7 +377,7 @@ func SetJointDoF(idx, dof int32, vr JointDoFVars, value float32) { //gosl:end func (ml *Model) JointDefaults(idx int32) { - rot := math32.NewQuat(0, 0, 0, 1) + rot := math32.NewQuatIdentity() SetJointPQuat(idx, rot) SetJointCQuat(idx, rot) } @@ -438,6 +443,22 @@ func (ml *Model) NewJointBall(parent, child int32, ppos, cpos math32.Vector3) in return idx } +// NewJointPlaneXZ adds a new 3 DoF Planar motion joint suitable for +// controlling the motion of a body on the standard X-Z plane (Y = up). +// The first two linear DoF control position in X, Z, and the third +// angular DoF controls rotation in the plane (along the Y axis). +// Use -1 for parent to add a world-anchored joint (typical). +// ppos, cpos are the relative positions from the parent, child. +// Sets relative rotation matricies to identity by default. +// Use [SetJointDoF] to set the remaining DoF parameters. +func (ml *Model) NewJointPlaneXZ(parent, child int32, ppos, cpos math32.Vector3) int32 { + idx := ml.NewJointD6(parent, child, ppos, cpos, 2, 1) + ml.newJointDoF(idx, 0, math32.Vec3(1,0,0)) + ml.newJointDoF(idx, 1, math32.Vec3(0,0,1)) + ml.newJointDoF(idx, 2, math32.Vec3(0,1,0)) + return idx +} + // NewJointDistance adds a new Distance joint (6 DoF) // between parent and child dynamic object indexes, // with distance constrained only on the first linear X axis. @@ -465,27 +486,28 @@ func (ml *Model) NewJointDistance(parent, child int32, ppos, cpos math32.Vector3 return idx } -// NewJointFree adds a new Free joint -// between parent and child dynamic object indexes, -// with distance constrained only on the first linear X axis. +// NewJointD6 adds a new D6 6 DoF joint with given number of actual +// linear and angular degrees-of-freedom, +// between parent and child dynamic object indexes. +// Use -1 for parent to add a world-anchored joint. +// ppos, cpos are the relative positions from the parent, child. +// Sets relative rotation matricies to identity by default. +func (ml *Model) NewJointD6(parent, child int32, ppos, cpos math32.Vector3, linDoF, angDoF int32) int32 { + idx := ml.newJoint(D6, parent, child, ppos, cpos) + SetJointLinearDoFN(idx, linDoF) + SetJointAngularDoFN(idx, angDoF) + return idx +} + +// NewJointFree adds a new Free joint (of which there is little point) +// between parent and child dynamic object indexes. // Use -1 for parent to add a world-anchored joint. // ppos, cpos are the relative positions from the parent, child. // Sets relative rotation matricies to identity by default. -// Use [SetJointDoF] to set the remaining DoF parameters. func (ml *Model) NewJointFree(parent, child int32, ppos, cpos math32.Vector3) int32 { - idx := ml.newJoint(Distance, parent, child, ppos, cpos) + idx := ml.newJoint(Free, parent, child, ppos, cpos) SetJointLinearDoFN(idx, 0) SetJointAngularDoFN(idx, 0) - for d := range math32.W { - axis := math32.Vector3{} - axis.SetDim(d, 1) - ml.newJointDoF(idx, int32(d), axis) - } - for d := range math32.W { - axis := math32.Vector3{} - axis.SetDim(d, 1) - ml.newJointDoF(idx, int32(d), axis) - } return idx } diff --git a/physics/shaders/CollisionBroad.wgsl b/physics/shaders/CollisionBroad.wgsl index 041e3353..2d733d3e 100644 --- a/physics/shaders/CollisionBroad.wgsl +++ b/physics/shaders/CollisionBroad.wgsl @@ -277,7 +277,7 @@ const ContactVarsN: ContactVars = 33; const JointControlVarsN: JointControlVars = 5; const DynamicVarsN: DynamicVars = 33; const GPUVarsN: GPUVars = 12; -const JointTypesN: JointTypes = 7; +const JointTypesN: JointTypes = 8; const JointVarsN: JointVars = 50; const JointDoFVarsN: JointDoFVars = 5; const ShapesN: Shapes = 6; @@ -292,6 +292,7 @@ const Fixed: JointTypes = 3; const Free: JointTypes = 4; const Distance: JointTypes = 5; const D6: JointTypes = 6; +const PlaneXZ: JointTypes = 7; alias JointVars = i32; //enums:enum const JointType: JointVars = 0; const JointEnabled: JointVars = 1; diff --git a/physics/shaders/CollisionNarrow.wgsl b/physics/shaders/CollisionNarrow.wgsl index 08b9c3d2..48f68a23 100644 --- a/physics/shaders/CollisionNarrow.wgsl +++ b/physics/shaders/CollisionNarrow.wgsl @@ -366,7 +366,7 @@ const ContactVarsN: ContactVars = 33; const JointControlVarsN: JointControlVars = 5; const DynamicVarsN: DynamicVars = 33; const GPUVarsN: GPUVars = 12; -const JointTypesN: JointTypes = 7; +const JointTypesN: JointTypes = 8; const JointVarsN: JointVars = 50; const JointDoFVarsN: JointDoFVars = 5; const ShapesN: Shapes = 6; @@ -381,6 +381,7 @@ const Fixed: JointTypes = 3; const Free: JointTypes = 4; const Distance: JointTypes = 5; const D6: JointTypes = 6; +const PlaneXZ: JointTypes = 7; alias JointVars = i32; //enums:enum const JointType: JointVars = 0; const JointEnabled: JointVars = 1; diff --git a/physics/shaders/DynamicsCurToNext.wgsl b/physics/shaders/DynamicsCurToNext.wgsl index 497ccecb..d4b54ab3 100644 --- a/physics/shaders/DynamicsCurToNext.wgsl +++ b/physics/shaders/DynamicsCurToNext.wgsl @@ -160,7 +160,7 @@ const ContactVarsN: ContactVars = 33; const JointControlVarsN: JointControlVars = 5; const DynamicVarsN: DynamicVars = 33; const GPUVarsN: GPUVars = 12; -const JointTypesN: JointTypes = 7; +const JointTypesN: JointTypes = 8; const JointVarsN: JointVars = 50; const JointDoFVarsN: JointDoFVars = 5; const ShapesN: Shapes = 6; @@ -175,6 +175,7 @@ const Fixed: JointTypes = 3; const Free: JointTypes = 4; const Distance: JointTypes = 5; const D6: JointTypes = 6; +const PlaneXZ: JointTypes = 7; alias JointVars = i32; //enums:enum const JointType: JointVars = 0; const JointEnabled: JointVars = 1; diff --git a/physics/shaders/ForcesFromJoints.wgsl b/physics/shaders/ForcesFromJoints.wgsl index 23a0e341..75d5c234 100644 --- a/physics/shaders/ForcesFromJoints.wgsl +++ b/physics/shaders/ForcesFromJoints.wgsl @@ -178,7 +178,7 @@ const ContactVarsN: ContactVars = 33; const JointControlVarsN: JointControlVars = 5; const DynamicVarsN: DynamicVars = 33; const GPUVarsN: GPUVars = 12; -const JointTypesN: JointTypes = 7; +const JointTypesN: JointTypes = 8; const JointVarsN: JointVars = 50; const JointDoFVarsN: JointDoFVars = 5; const ShapesN: Shapes = 6; @@ -193,6 +193,7 @@ const Fixed: JointTypes = 3; const Free: JointTypes = 4; const Distance: JointTypes = 5; const D6: JointTypes = 6; +const PlaneXZ: JointTypes = 7; alias JointVars = i32; //enums:enum const JointType: JointVars = 0; const JointEnabled: JointVars = 1; diff --git a/physics/shaders/InitDynamics.wgsl b/physics/shaders/InitDynamics.wgsl index 0accff03..68bd7d19 100644 --- a/physics/shaders/InitDynamics.wgsl +++ b/physics/shaders/InitDynamics.wgsl @@ -167,7 +167,7 @@ const ContactVarsN: ContactVars = 33; const JointControlVarsN: JointControlVars = 5; const DynamicVarsN: DynamicVars = 33; const GPUVarsN: GPUVars = 12; -const JointTypesN: JointTypes = 7; +const JointTypesN: JointTypes = 8; const JointVarsN: JointVars = 50; const JointDoFVarsN: JointDoFVars = 5; const ShapesN: Shapes = 6; @@ -182,6 +182,7 @@ const Fixed: JointTypes = 3; const Free: JointTypes = 4; const Distance: JointTypes = 5; const D6: JointTypes = 6; +const PlaneXZ: JointTypes = 7; alias JointVars = i32; //enums:enum const JointType: JointVars = 0; const JointEnabled: JointVars = 1; diff --git a/physics/shaders/StepBodyContactDeltas.wgsl b/physics/shaders/StepBodyContactDeltas.wgsl index be73febf..9adec380 100644 --- a/physics/shaders/StepBodyContactDeltas.wgsl +++ b/physics/shaders/StepBodyContactDeltas.wgsl @@ -263,7 +263,7 @@ const ContactVarsN: ContactVars = 33; const JointControlVarsN: JointControlVars = 5; const DynamicVarsN: DynamicVars = 33; const GPUVarsN: GPUVars = 12; -const JointTypesN: JointTypes = 7; +const JointTypesN: JointTypes = 8; const JointVarsN: JointVars = 50; const JointDoFVarsN: JointDoFVars = 5; const ShapesN: Shapes = 6; @@ -278,6 +278,7 @@ const Fixed: JointTypes = 3; const Free: JointTypes = 4; const Distance: JointTypes = 5; const D6: JointTypes = 6; +const PlaneXZ: JointTypes = 7; alias JointVars = i32; //enums:enum const JointType: JointVars = 0; const JointEnabled: JointVars = 1; diff --git a/physics/shaders/StepBodyContacts.wgsl b/physics/shaders/StepBodyContacts.wgsl index f0d0de5a..a4419564 100644 --- a/physics/shaders/StepBodyContacts.wgsl +++ b/physics/shaders/StepBodyContacts.wgsl @@ -392,7 +392,7 @@ const ContactVarsN: ContactVars = 33; const JointControlVarsN: JointControlVars = 5; const DynamicVarsN: DynamicVars = 33; const GPUVarsN: GPUVars = 12; -const JointTypesN: JointTypes = 7; +const JointTypesN: JointTypes = 8; const JointVarsN: JointVars = 50; const JointDoFVarsN: JointDoFVars = 5; const ShapesN: Shapes = 6; @@ -407,6 +407,7 @@ const Fixed: JointTypes = 3; const Free: JointTypes = 4; const Distance: JointTypes = 5; const D6: JointTypes = 6; +const PlaneXZ: JointTypes = 7; alias JointVars = i32; //enums:enum const JointType: JointVars = 0; const JointEnabled: JointVars = 1; diff --git a/physics/shaders/StepBodyJointDeltas.wgsl b/physics/shaders/StepBodyJointDeltas.wgsl index 0a370d4d..60c4a074 100644 --- a/physics/shaders/StepBodyJointDeltas.wgsl +++ b/physics/shaders/StepBodyJointDeltas.wgsl @@ -204,7 +204,7 @@ const ContactVarsN: ContactVars = 33; const JointControlVarsN: JointControlVars = 5; const DynamicVarsN: DynamicVars = 33; const GPUVarsN: GPUVars = 12; -const JointTypesN: JointTypes = 7; +const JointTypesN: JointTypes = 8; const JointVarsN: JointVars = 50; const JointDoFVarsN: JointDoFVars = 5; const ShapesN: Shapes = 6; @@ -219,6 +219,7 @@ const Fixed: JointTypes = 3; const Free: JointTypes = 4; const Distance: JointTypes = 5; const D6: JointTypes = 6; +const PlaneXZ: JointTypes = 7; alias JointVars = i32; //enums:enum const JointType: JointVars = 0; const JointEnabled: JointVars = 1; diff --git a/physics/shaders/StepIntegrateBodies.wgsl b/physics/shaders/StepIntegrateBodies.wgsl index c37b4ecf..407ece0a 100644 --- a/physics/shaders/StepIntegrateBodies.wgsl +++ b/physics/shaders/StepIntegrateBodies.wgsl @@ -190,7 +190,7 @@ const ContactVarsN: ContactVars = 33; const JointControlVarsN: JointControlVars = 5; const DynamicVarsN: DynamicVars = 33; const GPUVarsN: GPUVars = 12; -const JointTypesN: JointTypes = 7; +const JointTypesN: JointTypes = 8; const JointVarsN: JointVars = 50; const JointDoFVarsN: JointDoFVars = 5; const ShapesN: Shapes = 6; @@ -205,6 +205,7 @@ const Fixed: JointTypes = 3; const Free: JointTypes = 4; const Distance: JointTypes = 5; const D6: JointTypes = 6; +const PlaneXZ: JointTypes = 7; alias JointVars = i32; //enums:enum const JointType: JointVars = 0; const JointEnabled: JointVars = 1; diff --git a/physics/shaders/StepJointForces.wgsl b/physics/shaders/StepJointForces.wgsl index 9366b781..7f26e18c 100644 --- a/physics/shaders/StepJointForces.wgsl +++ b/physics/shaders/StepJointForces.wgsl @@ -181,7 +181,7 @@ const ContactVarsN: ContactVars = 33; const JointControlVarsN: JointControlVars = 5; const DynamicVarsN: DynamicVars = 33; const GPUVarsN: GPUVars = 12; -const JointTypesN: JointTypes = 7; +const JointTypesN: JointTypes = 8; const JointVarsN: JointVars = 50; const JointDoFVarsN: JointDoFVars = 5; const ShapesN: Shapes = 6; @@ -196,6 +196,7 @@ const Fixed: JointTypes = 3; const Free: JointTypes = 4; const Distance: JointTypes = 5; const D6: JointTypes = 6; +const PlaneXZ: JointTypes = 7; alias JointVars = i32; //enums:enum const JointType: JointVars = 0; const JointEnabled: JointVars = 1; diff --git a/physics/shaders/StepSolveJoints.wgsl b/physics/shaders/StepSolveJoints.wgsl index 5d3612b5..088069de 100644 --- a/physics/shaders/StepSolveJoints.wgsl +++ b/physics/shaders/StepSolveJoints.wgsl @@ -192,7 +192,7 @@ const ContactVarsN: ContactVars = 33; const JointControlVarsN: JointControlVars = 5; const DynamicVarsN: DynamicVars = 33; const GPUVarsN: GPUVars = 12; -const JointTypesN: JointTypes = 7; +const JointTypesN: JointTypes = 8; const JointVarsN: JointVars = 50; const JointDoFVarsN: JointDoFVars = 5; const ShapesN: Shapes = 6; @@ -207,6 +207,7 @@ const Fixed: JointTypes = 3; const Free: JointTypes = 4; const Distance: JointTypes = 5; const D6: JointTypes = 6; +const PlaneXZ: JointTypes = 7; alias JointVars = i32; //enums:enum const JointType: JointVars = 0; const JointEnabled: JointVars = 1; @@ -512,7 +513,7 @@ fn SetDim3(v: vec3, dim: i32, val: f32) -> vec3 { if (dim == 1) { nv.y = val; } - if (dim == 3) { + if (dim == 2) { nv.z = val; }return nv; } diff --git a/physics/shapegeom_test.go b/physics/shapegeom_test.go index 28cfaaaa..faec5204 100644 --- a/physics/shapegeom_test.go +++ b/physics/shapegeom_test.go @@ -39,7 +39,7 @@ func TestSphereSphere(t *testing.T) { tol := 1e-5 - rot := math32.NewQuat(0, 0, 0, 1) + rot := math32.NewQuatIdentity() for _, tc := range tests { gdA := GeomData{Shape: Sphere, Radius: tc.radiusA, Size: math32.Vector3{tc.radiusA, 0, 0}, WbR: tc.posA, WbQ: rot} gdB := GeomData{Shape: Sphere, Radius: tc.radiusB, Size: math32.Vector3{tc.radiusB, 0, 0}, WbR: tc.posB, WbQ: rot} From e68abe7935f6692e958d7aba0bbc239b489c8e77 Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Fri, 2 Jan 2026 21:33:03 +0100 Subject: [PATCH 69/97] physics: slmath.ClampMagnitude3, MaxForce, prevents numerical instability. --- gosl/slmath/vector3.go | 21 +++ physics/builder/builder.go | 13 ++ physics/builder/joint.go | 146 ++++++++++++++++----- physics/builder/object.go | 7 + physics/builder/physics.go | 13 +- physics/builder/typegen.go | 54 +++++--- physics/examples/pendula/pendula.go | 6 +- physics/examples/virtroom/typegen.go | 2 +- physics/examples/virtroom/virtroom.go | 30 +++-- physics/params.go | 7 +- physics/phyxyz/scene.go | 6 + physics/shaders/CollisionBroad.wgsl | 2 +- physics/shaders/CollisionNarrow.wgsl | 2 +- physics/shaders/DynamicsCurToNext.wgsl | 2 +- physics/shaders/ForcesFromJoints.wgsl | 2 +- physics/shaders/InitDynamics.wgsl | 2 +- physics/shaders/StepBodyContactDeltas.wgsl | 2 +- physics/shaders/StepBodyContacts.wgsl | 2 +- physics/shaders/StepBodyJointDeltas.wgsl | 2 +- physics/shaders/StepIntegrateBodies.wgsl | 21 ++- physics/shaders/StepJointForces.wgsl | 2 +- physics/shaders/StepSolveJoints.wgsl | 5 +- physics/step_body.go | 8 ++ physics/step_body.goal | 8 ++ physics/step_joint.go | 44 ++++++- physics/step_joint.goal | 41 +++++- physics/typegen.go | 2 +- 27 files changed, 364 insertions(+), 88 deletions(-) diff --git a/gosl/slmath/vector3.go b/gosl/slmath/vector3.go index 45ed8d0f..aee4cac3 100644 --- a/gosl/slmath/vector3.go +++ b/gosl/slmath/vector3.go @@ -76,6 +76,27 @@ func Clamp3(v, min, max math32.Vector3) math32.Vector3 { return r } +// ClampMagnitude3 clamps the magnitude of the components below given value. +func ClampMagnitude3(v math32.Vector3, mag float32) math32.Vector3 { + r := v + if r.X < -mag { + r.X = -mag + } else if r.X > mag { + r.X = mag + } + if r.Y < -mag { + r.Y = -mag + } else if r.Y > mag { + r.Y = mag + } + if r.Z < -mag { + r.Z = -mag + } else if r.Z > mag { + r.Z = mag + } + return r +} + // Normal3 returns this vector divided by its length (its unit vector). func Normal3(v math32.Vector3) math32.Vector3 { return v.DivScalar(Length3(v)) diff --git a/physics/builder/builder.go b/physics/builder/builder.go index fe02453b..37b527d5 100644 --- a/physics/builder/builder.go +++ b/physics/builder/builder.go @@ -93,6 +93,19 @@ func (bl *Builder) Build(ml *physics.Model, sc *phyxyz.Scene) { } } +// InitState initializes the current state variables in the builder. +// This does not call InitState in physics, because that depends on +// whether the Sccene is being used. +func (bl *Builder) InitState() { + for wi := range bl.Worlds { + wl := bl.World(wi) + for oi := range wl.Objects { + ob := wl.Object(oi) + ob.InitState() + } + } +} + // ReplicateWorld makes copies of given world to form an X,Y grid of // worlds with given optional offsets (Y, X) added between world objects. // Note that worldIdx is the index in Worlds, not the world number. diff --git a/physics/builder/joint.go b/physics/builder/joint.go index 16f5226d..e8182fad 100644 --- a/physics/builder/joint.go +++ b/physics/builder/joint.go @@ -43,6 +43,35 @@ type Joint struct { JointIndex int32 } +// Controls are the per degrees-of-freedom (DoF) joint control inputs. +type Controls struct { + // Force is the force input driving the joint. + Force float32 + + // Pos is the position target value, where 0 is the initial + // position. For angular joints, this is in radians. + Pos float32 + + // Stiff determines how strongly the target position + // is enforced: 0 = not at all; larger = stronger (e.g., 1000 or higher). + // Set to 0 to allow the joint to be fully flexible. + Stiff float32 + + // Vel is the velocity target value. For example, 0 + // effectively damps joint movement in proportion to Damp parameter. + Vel float32 + + // Damp determines how strongly the target velocity is enforced: + // 0 = not at all; larger = stronger (e.g., 1 is reasonable). + // Set to 0 to allow the joint to be fully flexible. + Damp float32 +} + +func (ct *Controls) Defaults() { + ct.Stiff = 1000 + ct.Damp = 20 +} + // DoF is a degree-of-freedom for a [Joint]. type DoF struct { // Axis is the axis of articulation. @@ -51,23 +80,23 @@ type DoF struct { // Limit has the limits for motion of this DoF. Limit minmax.F32 - // TargetPos is the position target value, where 0 is the initial - // position. For angular joints, this is in radians. - TargetPos float32 + // Init are the initial control values. + Init Controls - // TargetStiff determines how strongly the target position - // is enforced: 0 = not at all; larger = stronger (e.g., 1000 or higher). - // Set to 0 to allow the joint to be fully flexible. - TargetStiff float32 + // Current are the current control values (based on method calls). + Current Controls +} - // TargetVel is the velocity target value. For example, 0 - // effectively damps joint movement in proportion to Damp parameter. - TargetVel float32 +func (df *DoF) Defaults() { + df.Limit.Min = -physics.JointLimitUnlimited + df.Limit.Max = physics.JointLimitUnlimited + df.Init.Defaults() + df.Current.Defaults() +} + +func (df *DoF) InitState() { + df.Current = df.Init - // TargetDamp determines how strongly the target velocity is enforced: - // 0 = not at all; larger = stronger (e.g., 1 is reasonable). - // Set to 0 to allow the joint to be fully flexible. - TargetDamp float32 } func (jd *Joint) DoF(idx int) *DoF { @@ -90,10 +119,7 @@ func (ob *Object) newJoint(typ physics.JointTypes, parent, child *Body, ppos, cp jd.DoFs = make([]DoF, linDoF+angDoF) for i := range ndof { dof := jd.DoF(i) - dof.Limit.Min = -physics.JointLimitUnlimited - dof.Limit.Max = physics.JointLimitUnlimited - dof.TargetStiff = 1000 - dof.TargetDamp = 100 + dof.Defaults() } } return jd @@ -219,8 +245,8 @@ func (jd *Joint) NewPhysicsJoint(ml *physics.Model, ob *Object) int32 { di := int32(i) physics.SetJointDoF(ji, di, physics.JointLimitLower, d.Limit.Min) physics.SetJointDoF(ji, di, physics.JointLimitUpper, d.Limit.Max) - physics.SetJointTargetPos(ji, di, d.TargetPos, d.TargetStiff) - physics.SetJointTargetVel(ji, di, d.TargetVel, d.TargetDamp) + physics.SetJointTargetPos(ji, di, d.Init.Pos, d.Init.Stiff) + physics.SetJointTargetVel(ji, di, d.Init.Vel, d.Init.Damp) d.Axis = physics.JointAxis(ji, di) } for i := range jd.AngularDoFN { @@ -228,8 +254,8 @@ func (jd *Joint) NewPhysicsJoint(ml *physics.Model, ob *Object) int32 { d := jd.DoF(int(di)) physics.SetJointDoF(ji, di, physics.JointLimitLower, d.Limit.Min) physics.SetJointDoF(ji, di, physics.JointLimitUpper, d.Limit.Max) - physics.SetJointTargetPos(ji, di, d.TargetPos, d.TargetStiff) - physics.SetJointTargetVel(ji, di, d.TargetVel, d.TargetDamp) + physics.SetJointTargetPos(ji, di, d.Init.Pos, d.Init.Stiff) + physics.SetJointTargetVel(ji, di, d.Init.Vel, d.Init.Damp) d.Axis = physics.JointAxis(ji, di) } jd.JointIndex = ji @@ -245,6 +271,17 @@ func (jd *Joint) IsGlobal() bool { return jd.Parent < 0 } +// InitState initializes current state variables in the Joint. +func (jd *Joint) InitState() { + ji := jd.JointIndex + for di := range jd.DoFs { + d := jd.DoF(di) + d.InitState() + physics.SetJointTargetPos(ji, int32(di), d.Init.Pos, d.Init.Stiff) + physics.SetJointTargetVel(ji, int32(di), d.Init.Vel, d.Init.Damp) + } +} + // PoseToPhysics sets the current world-anchored joint pose // to the physics current state. func (jd *Joint) PoseToPhysics() { @@ -265,21 +302,70 @@ func (jd *Joint) PoseFromPhysics() { jd.PPose.Quat = physics.JointPQuat(jd.JointIndex) } +// SetTargetVel sets the target position for given DoF for +// this joint in the physics model. Records into [DoF.Current]. +func (jd *Joint) SetTargetVel(dof int32, vel, damp float32) { + d := jd.DoF(int(dof)) + d.Current.Vel = vel + d.Current.Damp = damp + physics.SetJointTargetVel(jd.JointIndex, dof, vel, damp) +} + // SetTargetPos sets the target position for given DoF for -// this joint in the physics model. +// this joint in the physics model. Records into [DoF.Current]. func (jd *Joint) SetTargetPos(dof int32, pos, stiff float32) { + d := jd.DoF(int(dof)) + d.Current.Pos = pos + d.Current.Stiff = stiff physics.SetJointTargetPos(jd.JointIndex, dof, pos, stiff) } -// AddTargetPos adds to the target position for given DoF for +// AddTargetPos adds to the Current target position for given DoF for // this joint in the physics model, setting stiffness. func (jd *Joint) AddTargetPos(dof int32, pos, stiff float32) { - cpos := physics.GetJointTargetPos(jd.JointIndex, dof) - physics.SetJointTargetPos(jd.JointIndex, dof, cpos+pos, stiff) + d := jd.DoF(int(dof)) + d.Current.Pos += pos + d.Current.Stiff = stiff + physics.SetJointTargetPos(jd.JointIndex, dof, d.Current.Pos, stiff) } -// SetTargetVel sets the target position for given DoF for -// this joint in the physics model. -func (jd *Joint) SetTargetVel(dof int32, vel, damp float32) { - physics.SetJointTargetVel(jd.JointIndex, dof, vel, damp) +// SetTargetAngle sets the target angular position +// and stiffness for given joint, DoF to given values. +// Stiffness determines how strongly the joint constraint is enforced +// (0 = not at all; 1000+ = strongly). +// Angle is in Degrees, not radians. Usable range is within -180..180 +// which is enforced, and values near the edge can be unstable at higher +// stiffness levels. +func (jd *Joint) SetTargetAngle(dof int32, angDeg, stiff float32) { + pos := math32.WrapPi(math32.DegToRad(angDeg)) + d := jd.DoF(int(dof)) + d.Current.Pos = pos + d.Current.Stiff = stiff + physics.SetJointTargetPos(jd.JointIndex, dof, pos, stiff) +} + +// AddTargetAngle adds to the Current target angular position, +// and sets stiffness for given joint, DoF to given values. +// Stiffness determines how strongly the joint constraint is enforced +// (0 = not at all; 1000+ = strongly). +// Angle is in Degrees, not radians. Usable range is within -180..180 +// which is enforced, and values near the edge can be unstable at higher +// stiffness levels. +func (jd *Joint) AddTargetAngle(dof int32, angDeg, stiff float32) { + d := jd.DoF(int(dof)) + d.Current.Pos = math32.WrapPi(d.Current.Pos + math32.DegToRad(angDeg)) + d.Current.Stiff = stiff + physics.SetJointTargetPos(jd.JointIndex, dof, d.Current.Pos, stiff) +} + +// AddPlaneXZPos adds to the Current target X and Z axis positions for +// a PlaneXZ joint, using the current Y axis rotation angle to project +// along the current angle direction. angOff provides an angle offset to +// add to the Y axis angle. +func (jd *Joint) AddPlaneXZPos(angOff, delta, stiff float32) { + ang := angOff - jd.DoF(2).Current.Pos + dx := delta * math32.Cos(ang) + dz := delta * math32.Sin(ang) + jd.AddTargetPos(0, dx, stiff) + jd.AddTargetPos(1, dz, stiff) } diff --git a/physics/builder/object.go b/physics/builder/object.go index ee0693aa..028c0f40 100644 --- a/physics/builder/object.go +++ b/physics/builder/object.go @@ -54,6 +54,13 @@ func (ob *Object) CopySkins(sc *phyxyz.Scene, so *Object) { } } +// InitState initializes current state variables in the object. +func (ob *Object) InitState() { + for i := range ob.Joints { + ob.Joint(i).InitState() + } +} + //////// Transforms // PoseToPhysics sets the current body poses to the physics current state. diff --git a/physics/builder/physics.go b/physics/builder/physics.go index e13b5037..b7ca3f92 100644 --- a/physics/builder/physics.go +++ b/physics/builder/physics.go @@ -28,16 +28,21 @@ type Physics struct { // and then Init on the Scene. func (ph *Physics) Build() { ph.Builder.Build(ph.Model, ph.Scene) - ph.Init() + if ph.Scene != nil { + ph.Scene.Init(ph.Model) + } } -// Init calls Scene.Init with Model. -func (ph *Physics) Init() { +// InitState calls Scene.InitState or Model.InitState and Builder InitState. +func (ph *Physics) InitState() { if ph.Scene != nil { - ph.Scene.Init(ph.Model) + ph.Scene.InitState(ph.Model) } else { ph.Model.InitState() } + if ph.Builder != nil { + ph.Builder.InitState() + } } // Step advances the physics world n steps, updating the scene every time. diff --git a/physics/builder/typegen.go b/physics/builder/typegen.go index e5d11a45..52a3a3ff 100644 --- a/physics/builder/typegen.go +++ b/physics/builder/typegen.go @@ -152,37 +152,51 @@ func (t *Joint) SetDoFs(v ...DoF) *Joint { t.DoFs = v; return t } // JointIndex is the index of this joint in [physics.Joints] when built. func (t *Joint) SetJointIndex(v int32) *Joint { t.JointIndex = v; return t } -var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics/builder.DoF", IDName: "do-f", Doc: "DoF is a degree-of-freedom for a [Joint].", Fields: []types.Field{{Name: "Axis", Doc: "Axis is the axis of articulation."}, {Name: "Limit", Doc: "Limit has the limits for motion of this DoF."}, {Name: "TargetPos", Doc: "TargetPos is the position target value, where 0 is the initial\nposition. For angular joints, this is in radians."}, {Name: "TargetStiff", Doc: "TargetStiff determines how strongly the target position\nis enforced: 0 = not at all; larger = stronger (e.g., 1000 or higher).\nSet to 0 to allow the joint to be fully flexible."}, {Name: "TargetVel", Doc: "TargetVel is the velocity target value. For example, 0\neffectively damps joint movement in proportion to Damp parameter."}, {Name: "TargetDamp", Doc: "TargetDamp determines how strongly the target velocity is enforced:\n0 = not at all; larger = stronger (e.g., 1 is reasonable).\nSet to 0 to allow the joint to be fully flexible."}}}) +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics/builder.Controls", IDName: "controls", Doc: "Controls are the per degrees-of-freedom (DoF) joint control inputs.", Fields: []types.Field{{Name: "Force", Doc: "Force is the force input driving the joint."}, {Name: "Pos", Doc: "Pos is the position target value, where 0 is the initial\nposition. For angular joints, this is in radians."}, {Name: "Stiff", Doc: "Stiff determines how strongly the target position\nis enforced: 0 = not at all; larger = stronger (e.g., 1000 or higher).\nSet to 0 to allow the joint to be fully flexible."}, {Name: "Vel", Doc: "Vel is the velocity target value. For example, 0\neffectively damps joint movement in proportion to Damp parameter."}, {Name: "Damp", Doc: "Damp determines how strongly the target velocity is enforced:\n0 = not at all; larger = stronger (e.g., 1 is reasonable).\nSet to 0 to allow the joint to be fully flexible."}}}) -// SetAxis sets the [DoF.Axis]: -// Axis is the axis of articulation. -func (t *DoF) SetAxis(v math32.Vector3) *DoF { t.Axis = v; return t } +// SetForce sets the [Controls.Force]: +// Force is the force input driving the joint. +func (t *Controls) SetForce(v float32) *Controls { t.Force = v; return t } -// SetLimit sets the [DoF.Limit]: -// Limit has the limits for motion of this DoF. -func (t *DoF) SetLimit(v minmax.F32) *DoF { t.Limit = v; return t } - -// SetTargetPos sets the [DoF.TargetPos]: -// TargetPos is the position target value, where 0 is the initial +// SetPos sets the [Controls.Pos]: +// Pos is the position target value, where 0 is the initial // position. For angular joints, this is in radians. -func (t *DoF) SetTargetPos(v float32) *DoF { t.TargetPos = v; return t } +func (t *Controls) SetPos(v float32) *Controls { t.Pos = v; return t } -// SetTargetStiff sets the [DoF.TargetStiff]: -// TargetStiff determines how strongly the target position +// SetStiff sets the [Controls.Stiff]: +// Stiff determines how strongly the target position // is enforced: 0 = not at all; larger = stronger (e.g., 1000 or higher). // Set to 0 to allow the joint to be fully flexible. -func (t *DoF) SetTargetStiff(v float32) *DoF { t.TargetStiff = v; return t } +func (t *Controls) SetStiff(v float32) *Controls { t.Stiff = v; return t } -// SetTargetVel sets the [DoF.TargetVel]: -// TargetVel is the velocity target value. For example, 0 +// SetVel sets the [Controls.Vel]: +// Vel is the velocity target value. For example, 0 // effectively damps joint movement in proportion to Damp parameter. -func (t *DoF) SetTargetVel(v float32) *DoF { t.TargetVel = v; return t } +func (t *Controls) SetVel(v float32) *Controls { t.Vel = v; return t } -// SetTargetDamp sets the [DoF.TargetDamp]: -// TargetDamp determines how strongly the target velocity is enforced: +// SetDamp sets the [Controls.Damp]: +// Damp determines how strongly the target velocity is enforced: // 0 = not at all; larger = stronger (e.g., 1 is reasonable). // Set to 0 to allow the joint to be fully flexible. -func (t *DoF) SetTargetDamp(v float32) *DoF { t.TargetDamp = v; return t } +func (t *Controls) SetDamp(v float32) *Controls { t.Damp = v; return t } + +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics/builder.DoF", IDName: "do-f", Doc: "DoF is a degree-of-freedom for a [Joint].", Fields: []types.Field{{Name: "Axis", Doc: "Axis is the axis of articulation."}, {Name: "Limit", Doc: "Limit has the limits for motion of this DoF."}, {Name: "Init", Doc: "Init are the initial control values."}, {Name: "Current", Doc: "Current are the current control values (based on method calls)."}}}) + +// SetAxis sets the [DoF.Axis]: +// Axis is the axis of articulation. +func (t *DoF) SetAxis(v math32.Vector3) *DoF { t.Axis = v; return t } + +// SetLimit sets the [DoF.Limit]: +// Limit has the limits for motion of this DoF. +func (t *DoF) SetLimit(v minmax.F32) *DoF { t.Limit = v; return t } + +// SetInit sets the [DoF.Init]: +// Init are the initial control values. +func (t *DoF) SetInit(v Controls) *DoF { t.Init = v; return t } + +// SetCurrent sets the [DoF.Current]: +// Current are the current control values (based on method calls). +func (t *DoF) SetCurrent(v Controls) *DoF { t.Current = v; return t } var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics/builder.Object", IDName: "object", Doc: "Object is an object within the [World].\nEach object is a coherent collection of bodies, typically\nconnected by joints. This is an organizational convenience\nfor positioning elements; has no physical implications.", Fields: []types.Field{{Name: "Bodies", Doc: "Bodies are the bodies in the object."}, {Name: "Joints", Doc: "Joints are joints connecting object bodies.\nJoint indexes here refer strictly within bodies."}}}) diff --git a/physics/examples/pendula/pendula.go b/physics/examples/pendula/pendula.go index 5b407398..e5f2ccc9 100644 --- a/physics/examples/pendula/pendula.go +++ b/physics/examples/pendula/pendula.go @@ -115,8 +115,7 @@ func main() { targ := math32.DegToRad(float32(ps.TargetDegFromVert)) jd := obj.NewJointRevolute(nil, pb, math32.Vec3(0, stY, 0), math32.Vec3(0, ps.HSize.Y, 0), math32.Vec3(0, 0, 1)) - jd.DoF(0).SetTargetPos(targ).SetTargetStiff(ps.Stiff). - SetTargetVel(0).SetTargetDamp(ps.Damp) + jd.DoF(0).Init.SetPos(targ).SetStiff(ps.Stiff).SetVel(0).SetDamp(ps.Damp) for i := 1; i < ps.NPendula; i++ { clr := colors.Names[12+i%len(colors.Names)] @@ -131,8 +130,7 @@ func main() { cb.SetGroup(1 + i) } jd = obj.NewJointRevolute(pb, cb, math32.Vec3(0, -ps.HSize.Y, 0), math32.Vec3(0, ps.HSize.Y, 0), math32.Vec3(0, 0, 1)) - jd.DoF(0).SetTargetPos(targ).SetTargetStiff(ps.Stiff). - SetTargetVel(0).SetTargetDamp(ps.Damp) + jd.DoF(0).Init.SetPos(targ).SetStiff(ps.Stiff).SetVel(0).SetDamp(ps.Damp) pb = cb botJoint = jd } diff --git a/physics/examples/virtroom/typegen.go b/physics/examples/virtroom/typegen.go index 0cdecccd..712d128c 100644 --- a/physics/examples/virtroom/typegen.go +++ b/physics/examples/virtroom/typegen.go @@ -6,4 +6,4 @@ import ( "cogentcore.org/core/types" ) -var _ = types.AddType(&types.Type{Name: "main.Env", IDName: "env", Doc: "Env encapsulates the virtual environment", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Methods: []types.Method{{Name: "WorldInit", Doc: "InitWorld does init on world.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "GrabEyeImg", Doc: "GrabEyeImg takes a snapshot from the perspective of Emer's right eye", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "ModelStep", Doc: "ModelStep does one step of the physics model.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "StepForward", Doc: "StepForward moves Emer forward in current facing direction one step,\nand takes GrabEyeImg", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "StepBackward", Doc: "StepBackward moves Emer backward in current facing direction one step, and takes GrabEyeImg", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "RotBodyLeft", Doc: "RotBodyLeft rotates emer left and takes GrabEyeImg", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "RotBodyRight", Doc: "RotBodyRight rotates emer right and takes GrabEyeImg", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "RotHeadLeft", Doc: "RotHeadLeft rotates emer left and takes GrabEyeImg", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "RotHeadRight", Doc: "RotHeadRight rotates emer right and takes GrabEyeImg", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}}, Fields: []types.Field{{Name: "EmerAngry", Doc: "if true, emer is angry: changes face color"}, {Name: "EmerHt", Doc: "height of emer"}, {Name: "MoveStep", Doc: "how far to move every step"}, {Name: "RotStep", Doc: "how far to rotate every step"}, {Name: "Width", Doc: "width of room"}, {Name: "Depth", Doc: "depth of room"}, {Name: "Height", Doc: "height of room"}, {Name: "Thick", Doc: "thickness of walls of room"}, {Name: "DepthVals", Doc: "current depth map"}, {Name: "Camera", Doc: "offscreen render camera settings"}, {Name: "DepthMap", Doc: "color map to use for rendering depth map"}, {Name: "Physics", Doc: "The core physics elements: Model, Builder, Scene"}, {Name: "SceneEditor", Doc: "3D visualization of the Scene"}, {Name: "Emer", Doc: "emer object"}, {Name: "EmerJoint", Doc: "emer PlaneXZ joint for controlling motion"}, {Name: "EyeR", Doc: "Right eye of emer"}, {Name: "EyeRImg", Doc: "snapshot image"}, {Name: "NeckJoint", Doc: "ball joint for the neck."}, {Name: "DepthImage", Doc: "depth map image"}}}) +var _ = types.AddType(&types.Type{Name: "main.Env", IDName: "env", Doc: "Env encapsulates the virtual environment", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Methods: []types.Method{{Name: "InitState", Doc: "Initstate reinitializes the physics model state.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "GrabEyeImg", Doc: "GrabEyeImg takes a snapshot from the perspective of Emer's right eye", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "ModelStep", Doc: "ModelStep does one step of the physics model.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "StepForward", Doc: "StepForward moves Emer forward in current facing direction one step,\nand takes GrabEyeImg", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "StepBackward", Doc: "StepBackward moves Emer backward in current facing direction one step, and takes GrabEyeImg", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "RotBodyLeft", Doc: "RotBodyLeft rotates emer left and takes GrabEyeImg", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "RotBodyRight", Doc: "RotBodyRight rotates emer right and takes GrabEyeImg", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "RotHeadLeft", Doc: "RotHeadLeft rotates emer left and takes GrabEyeImg", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "RotHeadRight", Doc: "RotHeadRight rotates emer right and takes GrabEyeImg", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}}, Fields: []types.Field{{Name: "EmerAngry", Doc: "if true, emer is angry: changes face color"}, {Name: "EmerHt", Doc: "height of emer"}, {Name: "MoveStep", Doc: "how far to move every step"}, {Name: "RotStep", Doc: "how far to rotate every step"}, {Name: "ModelSteps", Doc: "number of model steps to take"}, {Name: "Width", Doc: "width of room"}, {Name: "Depth", Doc: "depth of room"}, {Name: "Height", Doc: "height of room"}, {Name: "Thick", Doc: "thickness of walls of room"}, {Name: "DepthVals", Doc: "current depth map"}, {Name: "Camera", Doc: "offscreen render camera settings"}, {Name: "DepthMap", Doc: "color map to use for rendering depth map"}, {Name: "Physics", Doc: "The core physics elements: Model, Builder, Scene"}, {Name: "SceneEditor", Doc: "3D visualization of the Scene"}, {Name: "Emer", Doc: "emer object"}, {Name: "EmerJoint", Doc: "emer PlaneXZ joint for controlling motion"}, {Name: "EyeR", Doc: "Right eye of emer"}, {Name: "EyeRImg", Doc: "snapshot image"}, {Name: "NeckJoint", Doc: "ball joint for the neck."}, {Name: "DepthImage", Doc: "depth map image"}}}) diff --git a/physics/examples/virtroom/virtroom.go b/physics/examples/virtroom/virtroom.go index 6afc7112..13c6412f 100644 --- a/physics/examples/virtroom/virtroom.go +++ b/physics/examples/virtroom/virtroom.go @@ -117,7 +117,7 @@ func (ev *Env) Defaults() { ev.EmerHt = 1 ev.MoveStep = ev.EmerHt * .2 ev.RotStep = 15 - ev.ModelSteps = 100 + ev.ModelSteps = 10 ev.DepthMap = core.ColorMapName("ColdHot") ev.Camera.Defaults() ev.Camera.FOV = 90 @@ -140,12 +140,16 @@ func (ev *Env) MakeWorld(sc *xyz.Scene) { ev.MakeEmer(ew, "emer", ev.EmerHt) // ev.Physics.Builder.ReplicateWorld(1, 8, 2) ev.Physics.Build() - physics.GetParams(0).Gravity.Y = 0 + params := physics.GetParams(0) + params.Gravity.Y = 0 + params.MaxForce = 1.0e3 + params.AngularDamping = 0.5 + // params.SubSteps = 1 } -// InitWorld does init on world. -func (ev *Env) WorldInit() { //types:add - ev.Physics.Init() +// Initstate reinitializes the physics model state. +func (ev *Env) InitState() { //types:add + ev.Physics.InitState() ev.UpdateView() } @@ -219,11 +223,11 @@ func (ev *Env) ModelStep() { //types:add // StepForward moves Emer forward in current facing direction one step, // and takes GrabEyeImg func (ev *Env) StepForward() { //types:add - // doesn't integrate well with joints.. ev.Emer.PoseFromPhysics() + // doesn't integrate well with joints.. // ev.Emer.MoveOnAxisBody(0, 0, 0, 1, -ev.MoveStep) // ev.Emer.PoseToPhysics() - ev.EmerJoint.AddTargetPos(1, -ev.MoveStep, 1000) // z axis + ev.EmerJoint.AddPlaneXZPos(math32.Pi*.5, -ev.MoveStep, 1000) ev.ModelStep() } @@ -232,7 +236,7 @@ func (ev *Env) StepBackward() { //types:add ev.Emer.PoseFromPhysics() // ev.Emer.MoveOnAxisBody(0, 0, 0, 1, ev.MoveStep) // ev.Emer.PoseToPhysics() - ev.EmerJoint.AddTargetPos(1, ev.MoveStep, 1000) + ev.EmerJoint.AddPlaneXZPos(math32.Pi*.5, ev.MoveStep, 1000) ev.ModelStep() } @@ -324,14 +328,14 @@ func (ev *Env) MakeEmer(wl *builder.World, name string, height float32) { ev.NeckJoint = obj.NewJointBall(emr, head, math32.Vec3(0, hh, 0), math32.Vec3(0, -headsz, 0)) eyeoff := math32.Vec3(-headsz*.6, headsz*.1, -(headsz + eyesz*.3)) - bd := obj.NewDynamicSkin(sc, name+"_eye-l", physics.Box, "green", mass*.001, math32.Vec3(eyesz, eyesz*.5, eyesz*.2), headPos.Add(eyeoff), rot) + bd := obj.NewDynamicSkin(sc, name+"_eye-l", physics.Box, "green", mass*.01, math32.Vec3(eyesz, eyesz*.5, eyesz*.2), headPos.Add(eyeoff), rot) bd.Group = 0 - obj.NewJointBall(head, bd, eyeoff, math32.Vec3(0, 0, -eyesz*.3)) + obj.NewJointFixed(head, bd, eyeoff, math32.Vec3(0, 0, -eyesz*.3)) eyeoff = math32.Vec3(headsz*.6, headsz*.1, -(headsz + eyesz*.3)) - ev.EyeR = obj.NewDynamicSkin(sc, name+"_eye-r", physics.Box, "green", mass*.001, math32.Vec3(eyesz, eyesz*.5, eyesz*.2), headPos.Add(eyeoff), rot) + ev.EyeR = obj.NewDynamicSkin(sc, name+"_eye-r", physics.Box, "green", mass*.01, math32.Vec3(eyesz, eyesz*.5, eyesz*.2), headPos.Add(eyeoff), rot) ev.EyeR.Group = 0 - obj.NewJointBall(head, ev.EyeR, eyeoff, math32.Vec3(0, 0, -eyesz*.3)) + obj.NewJointFixed(head, ev.EyeR, eyeoff, math32.Vec3(0, 0, -eyesz*.3)) } func (ev *Env) ConfigGUI() *core.Body { @@ -400,7 +404,7 @@ func (ev *Env) ConfigGUI() *core.Body { func (ev *Env) MakeToolbar(p *tree.Plan) { tree.Add(p, func(w *core.FuncButton) { - w.SetFunc(ev.WorldInit).SetText("Init").SetIcon(icons.Update) + w.SetFunc(ev.InitState).SetText("Init").SetIcon(icons.Update) }) tree.Add(p, func(w *core.FuncButton) { w.SetFunc(ev.GrabEyeImg).SetText("Grab Image").SetIcon(icons.Image) diff --git a/physics/params.go b/physics/params.go index f27fee8d..8c5f81e7 100644 --- a/physics/params.go +++ b/physics/params.go @@ -61,6 +61,10 @@ type PhysParams struct { // SoftRelax is soft-body relaxation constant. SoftRelax float32 `default:"0.9"` + // MaxForce is the maximum computed force value, which prevents + // runaway numerical overflow. + MaxForce float32 `default:"1e5"` + // MaxGeomIter is number of iterations to perform in shape-based // geometry collision computations MaxGeomIter int32 `default:"10"` @@ -93,8 +97,6 @@ type PhysParams struct { // to examine. BodyCollidePairsN int32 `edit:"-"` - pad int32 - // Gravity is the gravity acceleration function Gravity slvec.Vector3 } @@ -117,6 +119,7 @@ func (pr *PhysParams) Defaults() { pr.AngularDamping = 0 pr.SoftRelax = 0.9 + pr.MaxForce = 1.0e5 pr.MaxGeomIter = 10 } diff --git a/physics/phyxyz/scene.go b/physics/phyxyz/scene.go index 65807721..1b718907 100644 --- a/physics/phyxyz/scene.go +++ b/physics/phyxyz/scene.go @@ -71,6 +71,12 @@ func (sc *Scene) Init(ml *physics.Model) { sc.Update() } +// InitState calls InitState on the Model and then Update. +func (sc *Scene) InitState(ml *physics.Model) { + ml.InitState() + sc.Update() +} + // Reset resets any existing views, starting fresh for a new configuration. func (sc *Scene) Reset() { sc.Skins = nil diff --git a/physics/shaders/CollisionBroad.wgsl b/physics/shaders/CollisionBroad.wgsl index 2d733d3e..64aa3143 100644 --- a/physics/shaders/CollisionBroad.wgsl +++ b/physics/shaders/CollisionBroad.wgsl @@ -366,6 +366,7 @@ struct PhysParams { JointAngularComply: f32, // 0 def AngularDamping: f32, // 0 def SoftRelax: f32, + MaxForce: f32, MaxGeomIter: i32, ContactsMax: i32, Cur: i32, @@ -376,7 +377,6 @@ struct PhysParams { JointDoFsN: i32, BodyJointsMax: i32, BodyCollidePairsN: i32, - pad: i32, Gravity: vec4, } diff --git a/physics/shaders/CollisionNarrow.wgsl b/physics/shaders/CollisionNarrow.wgsl index 48f68a23..de927542 100644 --- a/physics/shaders/CollisionNarrow.wgsl +++ b/physics/shaders/CollisionNarrow.wgsl @@ -455,6 +455,7 @@ struct PhysParams { JointAngularComply: f32, // 0 def AngularDamping: f32, // 0 def SoftRelax: f32, + MaxForce: f32, MaxGeomIter: i32, ContactsMax: i32, Cur: i32, @@ -465,7 +466,6 @@ struct PhysParams { JointDoFsN: i32, BodyJointsMax: i32, BodyCollidePairsN: i32, - pad: i32, Gravity: vec4, } diff --git a/physics/shaders/DynamicsCurToNext.wgsl b/physics/shaders/DynamicsCurToNext.wgsl index d4b54ab3..de2766fa 100644 --- a/physics/shaders/DynamicsCurToNext.wgsl +++ b/physics/shaders/DynamicsCurToNext.wgsl @@ -249,6 +249,7 @@ struct PhysParams { JointAngularComply: f32, // 0 def AngularDamping: f32, // 0 def SoftRelax: f32, + MaxForce: f32, MaxGeomIter: i32, ContactsMax: i32, Cur: i32, @@ -259,7 +260,6 @@ struct PhysParams { JointDoFsN: i32, BodyJointsMax: i32, BodyCollidePairsN: i32, - pad: i32, Gravity: vec4, } diff --git a/physics/shaders/ForcesFromJoints.wgsl b/physics/shaders/ForcesFromJoints.wgsl index 75d5c234..b497aee0 100644 --- a/physics/shaders/ForcesFromJoints.wgsl +++ b/physics/shaders/ForcesFromJoints.wgsl @@ -279,6 +279,7 @@ struct PhysParams { JointAngularComply: f32, // 0 def AngularDamping: f32, // 0 def SoftRelax: f32, + MaxForce: f32, MaxGeomIter: i32, ContactsMax: i32, Cur: i32, @@ -289,7 +290,6 @@ struct PhysParams { JointDoFsN: i32, BodyJointsMax: i32, BodyCollidePairsN: i32, - pad: i32, Gravity: vec4, } diff --git a/physics/shaders/InitDynamics.wgsl b/physics/shaders/InitDynamics.wgsl index 68bd7d19..c6f99ffe 100644 --- a/physics/shaders/InitDynamics.wgsl +++ b/physics/shaders/InitDynamics.wgsl @@ -256,6 +256,7 @@ struct PhysParams { JointAngularComply: f32, // 0 def AngularDamping: f32, // 0 def SoftRelax: f32, + MaxForce: f32, MaxGeomIter: i32, ContactsMax: i32, Cur: i32, @@ -266,7 +267,6 @@ struct PhysParams { JointDoFsN: i32, BodyJointsMax: i32, BodyCollidePairsN: i32, - pad: i32, Gravity: vec4, } diff --git a/physics/shaders/StepBodyContactDeltas.wgsl b/physics/shaders/StepBodyContactDeltas.wgsl index 9adec380..1cf6dac8 100644 --- a/physics/shaders/StepBodyContactDeltas.wgsl +++ b/physics/shaders/StepBodyContactDeltas.wgsl @@ -352,6 +352,7 @@ struct PhysParams { JointAngularComply: f32, // 0 def AngularDamping: f32, // 0 def SoftRelax: f32, + MaxForce: f32, MaxGeomIter: i32, ContactsMax: i32, Cur: i32, @@ -362,7 +363,6 @@ struct PhysParams { JointDoFsN: i32, BodyJointsMax: i32, BodyCollidePairsN: i32, - pad: i32, Gravity: vec4, } diff --git a/physics/shaders/StepBodyContacts.wgsl b/physics/shaders/StepBodyContacts.wgsl index a4419564..6942beab 100644 --- a/physics/shaders/StepBodyContacts.wgsl +++ b/physics/shaders/StepBodyContacts.wgsl @@ -481,6 +481,7 @@ struct PhysParams { JointAngularComply: f32, // 0 def AngularDamping: f32, // 0 def SoftRelax: f32, + MaxForce: f32, MaxGeomIter: i32, ContactsMax: i32, Cur: i32, @@ -491,7 +492,6 @@ struct PhysParams { JointDoFsN: i32, BodyJointsMax: i32, BodyCollidePairsN: i32, - pad: i32, Gravity: vec4, } diff --git a/physics/shaders/StepBodyJointDeltas.wgsl b/physics/shaders/StepBodyJointDeltas.wgsl index 60c4a074..498c036f 100644 --- a/physics/shaders/StepBodyJointDeltas.wgsl +++ b/physics/shaders/StepBodyJointDeltas.wgsl @@ -305,6 +305,7 @@ struct PhysParams { JointAngularComply: f32, // 0 def AngularDamping: f32, // 0 def SoftRelax: f32, + MaxForce: f32, MaxGeomIter: i32, ContactsMax: i32, Cur: i32, @@ -315,7 +316,6 @@ struct PhysParams { JointDoFsN: i32, BodyJointsMax: i32, BodyCollidePairsN: i32, - pad: i32, Gravity: vec4, } diff --git a/physics/shaders/StepIntegrateBodies.wgsl b/physics/shaders/StepIntegrateBodies.wgsl index 407ece0a..c905444d 100644 --- a/physics/shaders/StepIntegrateBodies.wgsl +++ b/physics/shaders/StepIntegrateBodies.wgsl @@ -279,6 +279,7 @@ struct PhysParams { JointAngularComply: f32, // 0 def AngularDamping: f32, // 0 def SoftRelax: f32, + MaxForce: f32, MaxGeomIter: i32, ContactsMax: i32, Cur: i32, @@ -289,7 +290,6 @@ struct PhysParams { JointDoFsN: i32, BodyJointsMax: i32, BodyCollidePairsN: i32, - pad: i32, Gravity: vec4, } @@ -370,6 +370,24 @@ return nq; //////// import: "slmath-vector2.go" //////// import: "slmath-vector3.go" +fn ClampMagnitude3(v: vec3, mag: f32) -> vec3 { + var r = v; + if (r.x < -mag) { + r.x = -mag; + } else if (r.x > mag) { + r.x = mag; + } + if (r.y < -mag) { + r.y = -mag; + } else if (r.y > mag) { + r.y = mag; + } + if (r.z < -mag) { + r.z = -mag; + } else if (r.z > mag) { + r.z = mag; + }return r; +} fn Cross3(v: vec3,o: vec3) -> vec3 { return vec3(v.y*o.z-v.z*o.y, v.z*o.x-v.x*o.z, v.x*o.y-v.y*o.x); } @@ -406,6 +424,7 @@ fn StepIntegrateBodies(i: u32) { //gosl:kernel var p1 = pcom+(v1*(params.Dt)); var wb = MulQuatVectorInverse(q0, w0); var tb = MulQuatVectorInverse(q0, t0)-(Cross3(wb, inertia*(wb))); // coriolis forces + tb = ClampMagnitude3(tb, params.MaxForce); var w1 = MulQuatVector(q0, wb+(invInertia*(tb)*(params.Dt))); var q1 = QuatAdd(q0, MulQuats(vec4(w1.x, w1.y, w1.z, 0), q0)*(0.5*params.Dt)); q1 = QuatNormalize(q1); diff --git a/physics/shaders/StepJointForces.wgsl b/physics/shaders/StepJointForces.wgsl index 7f26e18c..c49cfcc7 100644 --- a/physics/shaders/StepJointForces.wgsl +++ b/physics/shaders/StepJointForces.wgsl @@ -324,6 +324,7 @@ struct PhysParams { JointAngularComply: f32, // 0 def AngularDamping: f32, // 0 def SoftRelax: f32, + MaxForce: f32, MaxGeomIter: i32, ContactsMax: i32, Cur: i32, @@ -334,7 +335,6 @@ struct PhysParams { JointDoFsN: i32, BodyJointsMax: i32, BodyCollidePairsN: i32, - pad: i32, Gravity: vec4, } diff --git a/physics/shaders/StepSolveJoints.wgsl b/physics/shaders/StepSolveJoints.wgsl index 088069de..a56ed959 100644 --- a/physics/shaders/StepSolveJoints.wgsl +++ b/physics/shaders/StepSolveJoints.wgsl @@ -344,6 +344,7 @@ struct PhysParams { JointAngularComply: f32, // 0 def AngularDamping: f32, // 0 def SoftRelax: f32, + MaxForce: f32, MaxGeomIter: i32, ContactsMax: i32, Cur: i32, @@ -354,7 +355,6 @@ struct PhysParams { JointDoFsN: i32, BodyJointsMax: i32, BodyCollidePairsN: i32, - pad: i32, Gravity: vec4, } @@ -714,6 +714,9 @@ fn StepSolveJoints(i: u32) { //gosl:kernel var qtwist = QuatNormalize(vec4(relQ.x, 0.0, 0.0, relQ.w)); var qswing = MulQuats(relQ, QuatInverse(qtwist)); var s = sqrt(relQ.x*relQ.x + relQ.w*relQ.w); + if (s == 0) { + s = f32(1); + } var invs = 1.0 / s; var invscube = invs * invs * invs; var err0 = 2.0 * asin(clamp(qtwist.x, -1.0, 1.0)); diff --git a/physics/step_body.go b/physics/step_body.go index 8919c21b..c4115e5d 100644 --- a/physics/step_body.go +++ b/physics/step_body.go @@ -124,17 +124,25 @@ func StepIntegrateBodies(i uint32) { //gosl:kernel wb := slmath.MulQuatVectorInverse(q0, w0) tb := slmath.MulQuatVectorInverse(q0, t0).Sub(slmath.Cross3(wb, inertia.MulVector3(wb))) // coriolis forces + tb = slmath.ClampMagnitude3(tb, params.MaxForce) + w1 := slmath.MulQuatVector(q0, wb.Add(invInertia.MulVector3(tb).MulScalar(params.Dt))) q1 := slmath.QuatAdd(q0, slmath.MulQuats(math32.NewQuat(w1.X, w1.Y, w1.Z, 0), q0).MulScalar(0.5*params.Dt)) q1 = slmath.QuatNormalize(q1) // angular damping w1 = w1.MulScalar(1.0 - params.AngularDamping*params.Dt) + w1 = slmath.ClampMagnitude3(w1, params.MaxForce) p1a := p1.Sub(slmath.MulQuatVector(q1, com)) // pos corrected to nominal center. // fmt.Println(params.Next, "integrate:", v0, v1) + // if p1a.IsNaN() || q1.IsNaN() { + // if di == 0 { + // fmt.Println("integ:", di, p1a, q1, "r0:", r0, "q0:", q0, "v0:", v0, "w0:", w0, "f0:", f0, "t0:", t0, "pcom:", pcom, "v1:", v1, "p1:", p1, "wb:", wb, "tb:", tb, "w1:", w1) + // } + SetDynamicPos(di, params.Next, p1a) SetDynamicQuat(di, params.Next, q1) SetDynamicDelta(di, params.Next, v1) diff --git a/physics/step_body.goal b/physics/step_body.goal index 36c3567e..09b0316f 100644 --- a/physics/step_body.goal +++ b/physics/step_body.goal @@ -121,6 +121,8 @@ func StepIntegrateBodies(i uint32) { //gosl:kernel // angular part (compute in body frame) wb := slmath.MulQuatVectorInverse(q0, w0) tb := slmath.MulQuatVectorInverse(q0, t0).Sub(slmath.Cross3(wb, inertia.MulVector3(wb))) // coriolis forces + + tb = slmath.ClampMagnitude3(tb, params.MaxForce) w1 := slmath.MulQuatVector(q0, wb.Add(invInertia.MulVector3(tb).MulScalar(params.Dt))) q1 := slmath.QuatAdd(q0, slmath.MulQuats(math32.NewQuat(w1.X, w1.Y, w1.Z, 0), q0).MulScalar(0.5 * params.Dt)) @@ -128,10 +130,16 @@ func StepIntegrateBodies(i uint32) { //gosl:kernel // angular damping w1 = w1.MulScalar(1.0 - params.AngularDamping*params.Dt) + w1 = slmath.ClampMagnitude3(w1, params.MaxForce) p1a := p1.Sub(slmath.MulQuatVector(q1, com)) // pos corrected to nominal center. // fmt.Println(params.Next, "integrate:", v0, v1) + + // if p1a.IsNaN() || q1.IsNaN() { + // if di == 0 { + // fmt.Println("integ:", di, p1a, q1, "r0:", r0, "q0:", q0, "v0:", v0, "w0:", w0, "f0:", f0, "t0:", t0, "pcom:", pcom, "v1:", v1, "p1:", p1, "wb:", wb, "tb:", tb, "w1:", w1) + // } SetDynamicPos(di, params.Next, p1a) SetDynamicQuat(di, params.Next, q1) diff --git a/physics/step_joint.go b/physics/step_joint.go index 42417e8f..4a369594 100644 --- a/physics/step_joint.go +++ b/physics/step_joint.go @@ -149,6 +149,7 @@ func StepSolveJoints(i uint32) { //gosl:kernel jPR := JointPPos(ji) jPQ := JointPQuat(ji) + xwPR := jPR // world xform, parent, pos xwPQ := jPQ // quat mInvP := float32(0.0) @@ -163,6 +164,9 @@ func StepSolveJoints(i uint32) { //gosl:kernel posePR = DynamicPos(jPi, params.Next) // now using next posePQ = DynamicQuat(jPi, params.Next) slmath.MulSpatialTransforms(posePR, posePQ, jPR, jPQ, &xwPR, &xwPQ) + // if xwPR.IsNaN() || xwPQ.IsNaN() { + // fmt.Println("jpi:", jPi, posePR, posePQ, jPR, jPQ) + // } comP = BodyCom(jPbi) mInvP = Bodies.Value(int(jPbi), int(BodyInvMass)) iInvP = BodyInvInertia(jPbi) @@ -177,7 +181,17 @@ func StepSolveJoints(i uint32) { //gosl:kernel jCQ := JointCQuat(ji) xwCR := jCR xwCQ := jCQ + // if jCQ.IsNaN() || jCR.IsNaN() { + // fmt.Println("child joint start:", jCQ, jCR) + // } + // + // if poseCQ.IsNaN() || poseCR.IsNaN() { + // fmt.Println("jCi:", jCi, poseCQ, poseCR) + // } slmath.MulSpatialTransforms(poseCR, poseCQ, jCR, jCQ, &xwCR, &xwCQ) + // if xwCQ.IsNaN() || xwCR.IsNaN() { + // fmt.Println("xwCQ:", xwCQ, xwCR, poseCR, poseCQ, jCR, jCQ) + // } comC := BodyCom(jCbi) mInvC := Bodies.Value(int(jCbi), int(BodyInvMass)) iInvC := BodyInvInertia(jCbi) @@ -323,6 +337,14 @@ func StepSolveJoints(i uint32) { //gosl:kernel angDeltaP = angDeltaP.Add(angularP.MulScalar(dLambda * params.JointAngularRelax)) linDeltaC = linDeltaC.Add(linearC.MulScalar(dLambda * params.JointLinearRelax)) angDeltaC = angDeltaC.Add(angularC.MulScalar(dLambda * params.JointAngularRelax)) + + // if angDeltaC.IsNaN() || angDeltaP.IsNaN() { + // fmt.Println("joint ang:", angDeltaC, angDeltaP) + // } + // + // if linDeltaC.IsNaN() || linDeltaP.IsNaN() { + // fmt.Println("joint lin:", linDeltaC, linDeltaP) + // } } } } @@ -337,8 +359,16 @@ func StepSolveJoints(i uint32) { //gosl:kernel qtwist := slmath.QuatNormalize(math32.NewQuat(relQ.X, 0.0, 0.0, relQ.W)) qswing := slmath.MulQuats(relQ, slmath.QuatInverse(qtwist)) + // if qP.IsNaN() || qC.IsNaN() || relQ.IsNaN() { + // fmt.Println("qp, qc:", qP, qC, relQ) + // } + // decompose to a compound rotation each axis s := math32.Sqrt(relQ.X*relQ.X + relQ.W*relQ.W) + if s == 0 { + // fmt.Println("s = 0", relQ, qP, qC) + s = 1 + } invs := 1.0 / s invscube := invs * invs * invs @@ -361,6 +391,9 @@ func StepSolveJoints(i uint32) { //gosl:kernel relQ.X*(relQ.Z*relQ.X-relQ.W*relQ.Y)*invscube) grad0 = slmath.QuatMulScalar(grad0, 2.0/math32.Abs(qtwist.W)) // # grad0 *= 2.0 / wp.sqrt(1.0-qtwist[0]*qtwist[0]) # derivative of asin(x) = 1/sqrt(1-x^2) + // if grad0.IsNaN() || grad1.IsNaN() || grad2.IsNaN() { + // fmt.Println("grads:", grad0, grad1, grad2) + // } // rescale swing swing_sq := qswing.W * qswing.W @@ -417,6 +450,9 @@ func StepSolveJoints(i uint32) { //gosl:kernel // todo: verify -- does the 0.5 go inside?? // quat_c = 0.5 * q_p * grad * wp.quat_inverse(q_c) quatC := slmath.MulQuats(slmath.MulQuats(slmath.QuatMulScalar(qP, 0.5), grad), slmath.QuatInverse(qC)) + // if ji == 0 { + // fmt.Println(quatC, qP, qC, grad) + // } angularC := math32.Vec3(quatC.X, quatC.Y, quatC.Z) angularP := slmath.Negate3(angularC) @@ -459,6 +495,13 @@ func StepSolveJoints(i uint32) { //gosl:kernel // note: no relaxation factors here: angDeltaP = angDeltaP.Add(angularP.MulScalar(dLambda)) angDeltaC = angDeltaC.Add(angularC.MulScalar(dLambda)) + // if ji == 0 && dim == 1 { + // fmt.Println(ji, dim, e, err, lambdaIn, angularP, angularC) + // } + // + // if angDeltaC.IsNaN() || angDeltaP.IsNaN() { + // fmt.Println("ang joint ang:", angDeltaC, angDeltaP, angularC, angularP, dLambda) + // } } } @@ -514,7 +557,6 @@ func AngularCorrection(err, derr float32, tfaQ, tfbQ math32.Quat, iInva, iInvb m if denom+alpha > 0.0 { deltaLambda /= (dt+gamma)*denom + alpha/dt } - return deltaLambda } diff --git a/physics/step_joint.goal b/physics/step_joint.goal index 0058771c..4bc4fb14 100644 --- a/physics/step_joint.goal +++ b/physics/step_joint.goal @@ -147,6 +147,7 @@ func StepSolveJoints(i uint32) { //gosl:kernel jPR := JointPPos(ji) jPQ := JointPQuat(ji) + xwPR := jPR // world xform, parent, pos xwPQ := jPQ // quat mInvP := float32(0.0) @@ -161,6 +162,9 @@ func StepSolveJoints(i uint32) { //gosl:kernel posePR = DynamicPos(jPi, params.Next) // now using next posePQ = DynamicQuat(jPi, params.Next) slmath.MulSpatialTransforms(posePR, posePQ, jPR, jPQ, &xwPR, &xwPQ) + // if xwPR.IsNaN() || xwPQ.IsNaN() { + // fmt.Println("jpi:", jPi, posePR, posePQ, jPR, jPQ) + // } comP = BodyCom(jPbi) mInvP = Bodies[jPbi, BodyInvMass] iInvP = BodyInvInertia(jPbi) @@ -175,7 +179,16 @@ func StepSolveJoints(i uint32) { //gosl:kernel jCQ := JointCQuat(ji) xwCR := jCR xwCQ := jCQ + // if jCQ.IsNaN() || jCR.IsNaN() { + // fmt.Println("child joint start:", jCQ, jCR) + // } + // if poseCQ.IsNaN() || poseCR.IsNaN() { + // fmt.Println("jCi:", jCi, poseCQ, poseCR) + // } slmath.MulSpatialTransforms(poseCR, poseCQ, jCR, jCQ, &xwCR, &xwCQ) + // if xwCQ.IsNaN() || xwCR.IsNaN() { + // fmt.Println("xwCQ:", xwCQ, xwCR, poseCR, poseCQ, jCR, jCQ) + // } comC := BodyCom(jCbi) mInvC := Bodies[jCbi, BodyInvMass] iInvC := BodyInvInertia(jCbi) @@ -321,6 +334,13 @@ func StepSolveJoints(i uint32) { //gosl:kernel angDeltaP = angDeltaP.Add(angularP.MulScalar(dLambda * params.JointAngularRelax)) linDeltaC = linDeltaC.Add(linearC.MulScalar(dLambda * params.JointLinearRelax)) angDeltaC = angDeltaC.Add(angularC.MulScalar(dLambda * params.JointAngularRelax)) + + // if angDeltaC.IsNaN() || angDeltaP.IsNaN() { + // fmt.Println("joint ang:", angDeltaC, angDeltaP) + // } + // if linDeltaC.IsNaN() || linDeltaP.IsNaN() { + // fmt.Println("joint lin:", linDeltaC, linDeltaP) + // } } } } @@ -335,8 +355,16 @@ func StepSolveJoints(i uint32) { //gosl:kernel qtwist := slmath.QuatNormalize(math32.NewQuat(relQ.X, 0.0, 0.0, relQ.W)) qswing := slmath.MulQuats(relQ, slmath.QuatInverse(qtwist)) + // if qP.IsNaN() || qC.IsNaN() || relQ.IsNaN() { + // fmt.Println("qp, qc:", qP, qC, relQ) + // } + // decompose to a compound rotation each axis s := math32.Sqrt(relQ.X*relQ.X + relQ.W*relQ.W) + if s == 0 { + // fmt.Println("s = 0", relQ, qP, qC) + s = 1 + } invs := 1.0 / s invscube := invs * invs * invs @@ -359,6 +387,9 @@ func StepSolveJoints(i uint32) { //gosl:kernel relQ.X*(relQ.Z*relQ.X-relQ.W*relQ.Y)*invscube) grad0 = slmath.QuatMulScalar(grad0, 2.0/math32.Abs(qtwist.W)) // # grad0 *= 2.0 / wp.sqrt(1.0-qtwist[0]*qtwist[0]) # derivative of asin(x) = 1/sqrt(1-x^2) + // if grad0.IsNaN() || grad1.IsNaN() || grad2.IsNaN() { + // fmt.Println("grads:", grad0, grad1, grad2) + // } // rescale swing swing_sq := qswing.W * qswing.W @@ -415,6 +446,9 @@ func StepSolveJoints(i uint32) { //gosl:kernel // todo: verify -- does the 0.5 go inside?? // quat_c = 0.5 * q_p * grad * wp.quat_inverse(q_c) quatC := slmath.MulQuats(slmath.MulQuats(slmath.QuatMulScalar(qP, 0.5), grad), slmath.QuatInverse(qC)) + // if ji == 0 { + // fmt.Println(quatC, qP, qC, grad) + // } angularC := math32.Vec3(quatC.X, quatC.Y, quatC.Z) angularP := slmath.Negate3(angularC) @@ -457,6 +491,12 @@ func StepSolveJoints(i uint32) { //gosl:kernel // note: no relaxation factors here: angDeltaP = angDeltaP.Add(angularP.MulScalar(dLambda)) angDeltaC = angDeltaC.Add(angularC.MulScalar(dLambda)) + // if ji == 0 && dim == 1 { + // fmt.Println(ji, dim, e, err, lambdaIn, angularP, angularC) + // } + // if angDeltaC.IsNaN() || angDeltaP.IsNaN() { + // fmt.Println("ang joint ang:", angDeltaC, angDeltaP, angularC, angularP, dLambda) + // } } } @@ -512,7 +552,6 @@ func AngularCorrection(err, derr float32, tfaQ, tfbQ math32.Quat, iInva, iInvb m if denom+alpha > 0.0 { deltaLambda /= (dt+gamma)*denom + alpha/dt } - return deltaLambda } diff --git a/physics/typegen.go b/physics/typegen.go index 9edc8c33..2d1d2e4a 100644 --- a/physics/typegen.go +++ b/physics/typegen.go @@ -24,7 +24,7 @@ var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.JointDoFVars var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.Model", IDName: "model", Doc: "Model contains and manages all of the physics elements.", Directives: []types.Directive{{Tool: "go", Directive: "generate", Args: []string{"core", "generate", "-add-types", "-gosl"}}}, Fields: []types.Field{{Name: "GPU", Doc: "GPU determines whether to use GPU (else CPU)."}, {Name: "Params", Doc: "Params are global parameters."}, {Name: "CurrentWorld", Doc: "CurrentWorld is the [BodyWorld] value to use when creating new bodies.\nSet to -1 to create global elements that interact with everything,\nwhile 0 and positive numbers only interact amongst themselves."}, {Name: "ReplicasStart", Doc: "ReplicasStart is the starting body index for replicated world bodies,\nwhich is needed for viewers to efficiently select a specific world to view.\nThis is the start of the World=0 first instance."}, {Name: "ReplicasN", Doc: "ReplicasN is the number of body elements within each set of\nreplicated world bodies, which is needed for viewers to efficiently select\na specific world to view."}, {Name: "Bodies", Doc: "Bodies are the rigid body elements (dynamic and static),\nspecifying the constant, non-dynamic properties,\nwhich is initial state for dynamics.\n[body][BodyVarsN]"}, {Name: "Joints", Doc: "Joints is a list of permanent joints connecting bodies,\nwhich do not change (no dynamic variables).\n[joint][JointVarsN]"}, {Name: "JointDoFs", Doc: "JointDoFs is a list of joint DoF parameters, allocated per joint.\n[dof][JointDoFVars]"}, {Name: "BodyJoints", Doc: "BodyJoints is a list of joint indexes for each dynamic body, for aggregating.\n[dyn body][parent, child][Params.BodyJointsMax]"}, {Name: "BodyCollidePairs", Doc: "BodyCollidePairs are pairs of Body indexes that could potentially collide\nbased on precomputed collision logic, using World, Group, and Joint indexes.\n[BodyCollidePairsN][2]"}, {Name: "Dynamics", Doc: "Dynamics are the dynamic rigid body elements: these actually move.\nThe first set of variables are for initial values, and the second current.\n[body][cur/next][DynamicVarsN]"}, {Name: "BroadContactsN", Doc: "BroadContactsN has number of points of broad contact\nbetween bodies. [1]"}, {Name: "BroadContacts", Doc: "BroadContacts are the results of broad-phase contact processing,\nestablishing possible points of contact between bodies.\n[ContactsMax][BroadContactVarsN]"}, {Name: "ContactsN", Doc: "ContactsN has number of points of narrow (final) contact\nbetween bodies. [1]"}, {Name: "Contacts", Doc: "Contacts are the results of narrow-phase contact processing,\nwhere only actual contacts with fully-specified values are present.\n[ContactsMax][ContactVarsN]"}, {Name: "JointControls", Doc: "JointControls are dynamic joint control inputs, per joint DoF\n(in correspondence with [JointDoFs]). This can be uploaded to the\nGPU at every step.\n[dof][JointControlVarsN]"}}}) -var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.PhysParams", IDName: "phys-params", Doc: "PhysParams are the physics parameters", Directives: []types.Directive{{Tool: "gosl", Directive: "start"}}, Fields: []types.Field{{Name: "Iterations", Doc: "Iterations is the number of integration iterations to perform\nwithin each solver step. Muller et al (2020) report that 1 is best."}, {Name: "Dt", Doc: "Dt is the integration stepsize.\nFor highly kinetic situations (e.g., rapidly moving bouncing balls)\n0.0001 is needed to ensure contact registration. Use SubSteps to\naccomplish a target effective read-out step size."}, {Name: "SubSteps", Doc: "SubSteps is the number of integration steps to take per Step()\nfunction call. These sub steps are taken without any sync to/from\nthe GPU and are therefore much faster."}, {Name: "ContactMargin", Doc: "Contact margin is the extra distance for broadphase collision\naround rigid bodies. This can make some joints potentially unstable if > 0"}, {Name: "ContactRelax", Doc: "ContactRelax is rigid contact relaxation constant.\nHigher values cause errros"}, {Name: "ContactWeighting", Doc: "Contact weighting: balances contact forces?"}, {Name: "Restitution", Doc: "Restitution takes into account bounciness of objects."}, {Name: "JointLinearRelax", Doc: "JointLinearRelax is joint linear relaxation constant."}, {Name: "JointAngularRelax", Doc: "JointAngularRelax is joint angular relaxation constant."}, {Name: "JointLinearComply", Doc: "JointLinearComply is joint linear compliance constant."}, {Name: "JointAngularComply", Doc: "JointAngularComply is joint angular compliance constant."}, {Name: "AngularDamping", Doc: "AngularDamping is damping of angular motion."}, {Name: "SoftRelax", Doc: "SoftRelax is soft-body relaxation constant."}, {Name: "MaxGeomIter", Doc: "MaxGeomIter is number of iterations to perform in shape-based\ngeometry collision computations"}, {Name: "ContactsMax", Doc: "Maximum number of contacts to process at any given point."}, {Name: "Cur", Doc: "Index for the current state (0 or 1, alternates with Next)."}, {Name: "Next", Doc: "Index for the next state (1 or 0, alternates with Cur)."}, {Name: "BodiesN", Doc: "BodiesN is number of rigid bodies."}, {Name: "DynamicsN", Doc: "DynamicsN is number of dynamics bodies."}, {Name: "JointsN", Doc: "JointsN is number of joints."}, {Name: "JointDoFsN", Doc: "JointDoFsN is number of joint DoFs."}, {Name: "BodyJointsMax", Doc: "BodyJointsMax is max number of joints per body + 1 for actual n."}, {Name: "BodyCollidePairsN", Doc: "BodyCollidePairsN is the total number of pre-compiled collision pairs\nto examine."}, {Name: "pad"}, {Name: "Gravity", Doc: "Gravity is the gravity acceleration function"}}}) +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.PhysParams", IDName: "phys-params", Doc: "PhysParams are the physics parameters", Directives: []types.Directive{{Tool: "gosl", Directive: "start"}}, Fields: []types.Field{{Name: "Iterations", Doc: "Iterations is the number of integration iterations to perform\nwithin each solver step. Muller et al (2020) report that 1 is best."}, {Name: "Dt", Doc: "Dt is the integration stepsize.\nFor highly kinetic situations (e.g., rapidly moving bouncing balls)\n0.0001 is needed to ensure contact registration. Use SubSteps to\naccomplish a target effective read-out step size."}, {Name: "SubSteps", Doc: "SubSteps is the number of integration steps to take per Step()\nfunction call. These sub steps are taken without any sync to/from\nthe GPU and are therefore much faster."}, {Name: "ContactMargin", Doc: "Contact margin is the extra distance for broadphase collision\naround rigid bodies. This can make some joints potentially unstable if > 0"}, {Name: "ContactRelax", Doc: "ContactRelax is rigid contact relaxation constant.\nHigher values cause errros"}, {Name: "ContactWeighting", Doc: "Contact weighting: balances contact forces?"}, {Name: "Restitution", Doc: "Restitution takes into account bounciness of objects."}, {Name: "JointLinearRelax", Doc: "JointLinearRelax is joint linear relaxation constant."}, {Name: "JointAngularRelax", Doc: "JointAngularRelax is joint angular relaxation constant."}, {Name: "JointLinearComply", Doc: "JointLinearComply is joint linear compliance constant."}, {Name: "JointAngularComply", Doc: "JointAngularComply is joint angular compliance constant."}, {Name: "AngularDamping", Doc: "AngularDamping is damping of angular motion."}, {Name: "SoftRelax", Doc: "SoftRelax is soft-body relaxation constant."}, {Name: "MaxForce", Doc: "MaxForce is the maximum computed force value, which prevents\nrunaway numerical overflow."}, {Name: "MaxGeomIter", Doc: "MaxGeomIter is number of iterations to perform in shape-based\ngeometry collision computations"}, {Name: "ContactsMax", Doc: "Maximum number of contacts to process at any given point."}, {Name: "Cur", Doc: "Index for the current state (0 or 1, alternates with Next)."}, {Name: "Next", Doc: "Index for the next state (1 or 0, alternates with Cur)."}, {Name: "BodiesN", Doc: "BodiesN is number of rigid bodies."}, {Name: "DynamicsN", Doc: "DynamicsN is number of dynamics bodies."}, {Name: "JointsN", Doc: "JointsN is number of joints."}, {Name: "JointDoFsN", Doc: "JointDoFsN is number of joint DoFs."}, {Name: "BodyJointsMax", Doc: "BodyJointsMax is max number of joints per body + 1 for actual n."}, {Name: "BodyCollidePairsN", Doc: "BodyCollidePairsN is the total number of pre-compiled collision pairs\nto examine."}, {Name: "Gravity", Doc: "Gravity is the gravity acceleration function"}}}) var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.GeomData", IDName: "geom-data", Doc: "GeomData contains all geometric data for narrow-phase collision.", Directives: []types.Directive{{Tool: "gosl", Directive: "start"}}, Fields: []types.Field{{Name: "BodyIdx"}, {Name: "Shape"}, {Name: "MinSize", Doc: "MinSize is the min of the Size dimensions."}, {Name: "Thick", Doc: "Thickness of shape."}, {Name: "Radius", Doc: "Radius is the effective radius for sphere-like elements (Sphere, Capsule, Cone)"}, {Name: "Size"}, {Name: "WbR", Doc: "World-to-Body transform\nPosition (R) (i.e., BodyPos)"}, {Name: "WbQ", Doc: "Quaternion (Q) (i.e., BodyQuat)"}, {Name: "BwR", Doc: "Body-to-World transform (inverse)\nPosition (R)"}, {Name: "BwQ", Doc: "Quaternion (Q)"}}}) From 4ca300da6d0ccb6706cb5bb5045f5d3e779517ce Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Fri, 2 Jan 2026 21:46:19 +0100 Subject: [PATCH 70/97] physics: Fixed joint handled --- physics/builder/joint.go | 2 ++ physics/examples/virtroom/virtroom.go | 8 ++++---- physics/shaders/StepIntegrateBodies.wgsl | 1 + 3 files changed, 7 insertions(+), 4 deletions(-) diff --git a/physics/builder/joint.go b/physics/builder/joint.go index e8182fad..c468eaf5 100644 --- a/physics/builder/joint.go +++ b/physics/builder/joint.go @@ -233,6 +233,8 @@ func (jd *Joint) NewPhysicsJoint(ml *physics.Model, ob *Object) int32 { ji = ml.NewJointRevolute(pdi, cdi, jd.PPose.Pos, jd.CPose.Pos, jd.DoFs[0].Axis) case physics.Ball: ji = ml.NewJointBall(pdi, cdi, jd.PPose.Pos, jd.CPose.Pos) + case physics.Fixed: + ji = ml.NewJointFixed(pdi, cdi, jd.PPose.Pos, jd.CPose.Pos) case physics.Distance: ji = ml.NewJointBall(pdi, cdi, jd.PPose.Pos, jd.CPose.Pos) case physics.Free: diff --git a/physics/examples/virtroom/virtroom.go b/physics/examples/virtroom/virtroom.go index 13c6412f..ab2f6b26 100644 --- a/physics/examples/virtroom/virtroom.go +++ b/physics/examples/virtroom/virtroom.go @@ -117,7 +117,7 @@ func (ev *Env) Defaults() { ev.EmerHt = 1 ev.MoveStep = ev.EmerHt * .2 ev.RotStep = 15 - ev.ModelSteps = 10 + ev.ModelSteps = 100 ev.DepthMap = core.ColorMapName("ColdHot") ev.Camera.Defaults() ev.Camera.FOV = 90 @@ -260,15 +260,15 @@ func (ev *Env) RotBodyRight() { //types:add // RotHeadLeft rotates emer left and takes GrabEyeImg func (ev *Env) RotHeadLeft() { //types:add - // ev.Emer.PoseFromPhysics() - ev.NeckJoint.AddTargetPos(1, math32.DegToRad(ev.RotStep), 1000) + ev.Emer.PoseFromPhysics() + ev.NeckJoint.AddTargetAngle(1, ev.RotStep, 1000) ev.ModelStep() } // RotHeadRight rotates emer right and takes GrabEyeImg func (ev *Env) RotHeadRight() { //types:add ev.Emer.PoseFromPhysics() - ev.NeckJoint.AddTargetPos(1, -math32.DegToRad(ev.RotStep), 1000) + ev.NeckJoint.AddTargetAngle(1, -ev.RotStep, 1000) ev.ModelStep() } diff --git a/physics/shaders/StepIntegrateBodies.wgsl b/physics/shaders/StepIntegrateBodies.wgsl index c905444d..d2b0e0d9 100644 --- a/physics/shaders/StepIntegrateBodies.wgsl +++ b/physics/shaders/StepIntegrateBodies.wgsl @@ -429,6 +429,7 @@ fn StepIntegrateBodies(i: u32) { //gosl:kernel var q1 = QuatAdd(q0, MulQuats(vec4(w1.x, w1.y, w1.z, 0), q0)*(0.5*params.Dt)); q1 = QuatNormalize(q1); w1 = w1*(1.0 - params.AngularDamping*params.Dt); + w1 = ClampMagnitude3(w1, params.MaxForce); var p1a = p1-(MulQuatVector(q1, com)); // pos corrected to nominal center. SetDynamicPos(di, params.Next, p1a); SetDynamicQuat(di, params.Next, q1); From 86dcc9dee5d19c4bedf076b48d1686651de29db9 Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Sat, 3 Jan 2026 17:59:26 -0800 Subject: [PATCH 71/97] physics: major progress in fixing joint behavior: need to update body positions in sequential order, not in parallel. XY is now working sensibly finally. --- docs/content/physics.md | 3 + physics/builder/builder.go | 4 + physics/builder/joint.go | 17 +- physics/enumgen.go | 22 +- physics/examples/pendula/pendula.go | 1 + physics/examples/virtroom/typegen.go | 2 +- physics/examples/virtroom/virtroom.go | 125 ++-- physics/gosl.go | 148 ++--- physics/joint.go | 94 +-- physics/joint.goal | 94 +-- physics/model.go | 43 +- physics/model.goal | 43 +- physics/params.go | 20 + physics/shaders/CollisionBroad.wgsl | 109 ++-- physics/shaders/CollisionNarrow.wgsl | 143 +++-- physics/shaders/DynamicsCurToNext.wgsl | 95 ++- physics/shaders/ForcesFromJoints.wgsl | 135 ++--- physics/shaders/InitDynamics.wgsl | 111 ++-- physics/shaders/StepBodyContactDeltas.wgsl | 135 ++--- physics/shaders/StepBodyContacts.wgsl | 131 ++--- physics/shaders/StepBodyJointDeltas.wgsl | 477 --------------- physics/shaders/StepIntegrateBodies.wgsl | 115 ++-- physics/shaders/StepJointForces.wgsl | 150 +++-- physics/shaders/StepSolveJoints.wgsl | 548 +++++++++++------- physics/step.go | 3 +- physics/step.goal | 3 +- physics/step_body.go | 36 -- physics/step_body.goal | 36 -- physics/step_joint.go | 432 ++++++++------ physics/step_joint.goal | 429 ++++++++------ physics/typegen.go | 4 +- physics/vars.go | 18 +- .../cogentcore_org-lab-physics-builder.go | 17 +- .../labsymbols/cogentcore_org-lab-physics.go | 33 +- 34 files changed, 1686 insertions(+), 2090 deletions(-) delete mode 100644 physics/shaders/StepBodyJointDeltas.wgsl diff --git a/docs/content/physics.md b/docs/content/physics.md index ce1964fa..9ab18e90 100644 --- a/docs/content/physics.md +++ b/docs/content/physics.md @@ -40,6 +40,7 @@ ed.SetConfigFunc(func() { rleft := math32.NewQuatAxisAngle(math32.Vec3(0, 0, 1), -math32.Pi/2) pb := sc.NewDynamic(ml, "top", physics.Capsule, "blue", mass, hsz, math32.Vec3(x, stY, 0), rleft) pb.SetBodyGroup(1) // no collide across groups + ml.NewObject() ji := sc.NewJointRevolute(ml, nil, pb, math32.Vec3(0, stY, 0), math32.Vec3(0, hsz.Y, 0), math32.Vec3(0, 0, 1)) physics.SetJointTargetPos(ji, 0, 0, 0) physics.SetJointTargetVel(ji, 0, 0, 0) @@ -103,6 +104,7 @@ ed.SetConfigFunc(func() { mass := float32(0.1) obj := sc.NewDynamic(ml, "body", physics.Box, "blue", mass, hsz, math32.Vec3(0, hsz.Y, 0), math32.NewQuatIdentity()) + ml.NewObject() ji := sc.NewJointPrismatic(ml, nil, obj, math32.Vec3(-5, 0, 0), math32.Vec3(0, hsz.Y, 0), math32.Vec3(1, 0, 0)) }) @@ -172,6 +174,7 @@ ed.SetConfigFunc(func() { mass := float32(0.1) obj := sc.NewDynamic(ml, "body", physics.Box, "blue", mass, hsz, math32.Vec3(0, hsz.Y, 0), math32.NewQuatIdentity()) + ml.NewObject() ji := sc.NewJointBall(ml, nil, obj, math32.Vec3(0, 0, 0), math32.Vec3(0, -hsz.Y, 0)) }) diff --git a/physics/builder/builder.go b/physics/builder/builder.go index 37b527d5..6de77281 100644 --- a/physics/builder/builder.go +++ b/physics/builder/builder.go @@ -81,6 +81,10 @@ func (bl *Builder) Build(ml *physics.Model, sc *phyxyz.Scene) { } } } + if len(ob.Joints) == 0 { + continue + } + ml.NewObject() for bji := range ob.Joints { jd := ob.Joint(bji) jd.NewPhysicsJoint(ml, ob) diff --git a/physics/builder/joint.go b/physics/builder/joint.go index c468eaf5..748f0971 100644 --- a/physics/builder/joint.go +++ b/physics/builder/joint.go @@ -30,6 +30,9 @@ type Joint struct { // in the parent's body-centered coordinates. CPose Pose + // ParentFixed does not update the parent side of the joint. + ParentFixed bool + // LinearDoFN is the number of linear degrees of freedom (3 max). LinearDoFN int @@ -113,7 +116,9 @@ func (ob *Object) newJoint(typ physics.JointTypes, parent, child *Body, ppos, cp ob.Joints = append(ob.Joints, Joint{Parent: pidx, Child: child.ObjectIndex, Type: typ, LinearDoFN: linDoF, AngularDoFN: angDoF}) jd := ob.Joint(idx) jd.PPose.Pos = ppos + jd.PPose.Quat = math32.NewQuatIdentity() jd.CPose.Pos = cpos + jd.CPose.Quat = math32.NewQuatIdentity() ndof := linDoF + angDoF if ndof > 0 { jd.DoFs = make([]DoF, linDoF+angDoF) @@ -203,9 +208,9 @@ func (ob *Object) NewJointFree(parent, child *Body, ppos, cpos math32.Vector3) * } // NewJointPlaneXZ adds a new 3 DoF Planar motion joint suitable for -// controlling the motion of a body on the standard X-Z play (Y = up). -// The first two linear DoF control position in X, Z, and the third -// angular DoF controls rotation in the plane (along the Y axis). +// controlling the motion of a body on the standard X-Z plane (Y = up). +// The two linear DoF control position in X, Z, and 3rd angular +// controls rotation in Y axis. // Use -1 for parent to add a world-anchored joint (typical). // ppos, cpos are the relative positions from the parent, child. // Sets relative rotation matricies to identity by default. @@ -242,6 +247,7 @@ func (jd *Joint) NewPhysicsJoint(ml *physics.Model, ob *Object) int32 { case physics.PlaneXZ: ji = ml.NewJointPlaneXZ(pdi, cdi, jd.PPose.Pos, jd.CPose.Pos) } + physics.SetJointParentFixed(ji, jd.ParentFixed) for i := range jd.LinearDoFN { d := jd.DoF(i) di := int32(i) @@ -361,11 +367,10 @@ func (jd *Joint) AddTargetAngle(dof int32, angDeg, stiff float32) { } // AddPlaneXZPos adds to the Current target X and Z axis positions for -// a PlaneXZ joint, using the current Y axis rotation angle to project +// a PlaneXZ joint, using the given Y axis rotation angle to project // along the current angle direction. angOff provides an angle offset to // add to the Y axis angle. -func (jd *Joint) AddPlaneXZPos(angOff, delta, stiff float32) { - ang := angOff - jd.DoF(2).Current.Pos +func (jd *Joint) AddPlaneXZPos(ang, delta, stiff float32) { dx := delta * math32.Cos(ang) dz := delta * math32.Sin(ang) jd.AddTargetPos(0, dx, stiff) diff --git a/physics/enumgen.go b/physics/enumgen.go index 22d5b164..abc1132f 100644 --- a/physics/enumgen.go +++ b/physics/enumgen.go @@ -192,20 +192,20 @@ func (i *DynamicVars) UnmarshalText(text []byte) error { return enums.UnmarshalText(i, text, "DynamicVars") } -var _GPUVarsValues = []GPUVars{0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11} +var _GPUVarsValues = []GPUVars{0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12} // GPUVarsN is the highest valid value for type GPUVars, plus one. // //gosl:start -const GPUVarsN GPUVars = 12 +const GPUVarsN GPUVars = 13 //gosl:end -var _GPUVarsValueMap = map[string]GPUVars{`ParamsVar`: 0, `BodiesVar`: 1, `JointsVar`: 2, `JointDoFsVar`: 3, `BodyJointsVar`: 4, `BodyCollidePairsVar`: 5, `DynamicsVar`: 6, `BroadContactsNVar`: 7, `BroadContactsVar`: 8, `ContactsNVar`: 9, `ContactsVar`: 10, `JointControlsVar`: 11} +var _GPUVarsValueMap = map[string]GPUVars{`ParamsVar`: 0, `BodiesVar`: 1, `ObjectsVar`: 2, `BodyJointsVar`: 3, `JointsVar`: 4, `JointDoFsVar`: 5, `BodyCollidePairsVar`: 6, `DynamicsVar`: 7, `BroadContactsNVar`: 8, `BroadContactsVar`: 9, `ContactsNVar`: 10, `ContactsVar`: 11, `JointControlsVar`: 12} -var _GPUVarsDescMap = map[GPUVars]string{0: ``, 1: ``, 2: ``, 3: ``, 4: ``, 5: ``, 6: ``, 7: ``, 8: ``, 9: ``, 10: ``, 11: ``} +var _GPUVarsDescMap = map[GPUVars]string{0: ``, 1: ``, 2: ``, 3: ``, 4: ``, 5: ``, 6: ``, 7: ``, 8: ``, 9: ``, 10: ``, 11: ``, 12: ``} -var _GPUVarsMap = map[GPUVars]string{0: `ParamsVar`, 1: `BodiesVar`, 2: `JointsVar`, 3: `JointDoFsVar`, 4: `BodyJointsVar`, 5: `BodyCollidePairsVar`, 6: `DynamicsVar`, 7: `BroadContactsNVar`, 8: `BroadContactsVar`, 9: `ContactsNVar`, 10: `ContactsVar`, 11: `JointControlsVar`} +var _GPUVarsMap = map[GPUVars]string{0: `ParamsVar`, 1: `BodiesVar`, 2: `ObjectsVar`, 3: `BodyJointsVar`, 4: `JointsVar`, 5: `JointDoFsVar`, 6: `BodyCollidePairsVar`, 7: `DynamicsVar`, 8: `BroadContactsNVar`, 9: `BroadContactsVar`, 10: `ContactsNVar`, 11: `ContactsVar`, 12: `JointControlsVar`} // String returns the string representation of this GPUVars value. func (i GPUVars) String() string { return enums.String(i, _GPUVarsMap) } @@ -248,7 +248,7 @@ const JointTypesN JointTypes = 8 var _JointTypesValueMap = map[string]JointTypes{`Prismatic`: 0, `Revolute`: 1, `Ball`: 2, `Fixed`: 3, `Free`: 4, `Distance`: 5, `D6`: 6, `PlaneXZ`: 7} -var _JointTypesDescMap = map[JointTypes]string{0: `Prismatic allows translation along a single axis (slider): 1 DoF.`, 1: `Revolute allows rotation about a single axis (axel): 1 DoF.`, 2: `Ball allows rotation about all three axes (3 DoF, quaternion).`, 3: `Fixed locks all relative motion: 0 DoF.`, 4: `Free allows full 6-DoF motion (translation and rotation).`, 5: `Distance keeps two bodies a distance within joint limits: 6 DoF.`, 6: `D6 is a generic 6-DoF joint.`, 7: `PlaneXZ is a version of D6 for navigation in the X-Z plane, which creates 2 linear DoF (X, Z) and 1 angular DoF (around Y axis).`} +var _JointTypesDescMap = map[JointTypes]string{0: `Prismatic allows translation along a single axis (slider): 1 DoF.`, 1: `Revolute allows rotation about a single axis (axel): 1 DoF.`, 2: `Ball allows rotation about all three axes (3 DoF, quaternion).`, 3: `Fixed locks all relative motion: 0 DoF.`, 4: `Free allows full 6-DoF motion (translation and rotation).`, 5: `Distance keeps two bodies a distance within joint limits: 6 DoF.`, 6: `D6 is a generic 6-DoF joint.`, 7: `PlaneXZ is a version of D6 for navigation in the X-Z plane, which creates 2 linear DoF (X, Z) for movement.`} var _JointTypesMap = map[JointTypes]string{0: `Prismatic`, 1: `Revolute`, 2: `Ball`, 3: `Fixed`, 4: `Free`, 5: `Distance`, 6: `D6`, 7: `PlaneXZ`} @@ -284,20 +284,20 @@ func (i *JointTypes) UnmarshalText(text []byte) error { return enums.UnmarshalText(i, text, "JointTypes") } -var _JointVarsValues = []JointVars{0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48, 49} +var _JointVarsValues = []JointVars{0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38} // JointVarsN is the highest valid value for type JointVars, plus one. // //gosl:start -const JointVarsN JointVars = 50 +const JointVarsN JointVars = 39 //gosl:end -var _JointVarsValueMap = map[string]JointVars{`JointType`: 0, `JointEnabled`: 1, `JointParent`: 2, `JointChild`: 3, `JointPPosX`: 4, `JointPPosY`: 5, `JointPPosZ`: 6, `JointPQuatX`: 7, `JointPQuatY`: 8, `JointPQuatZ`: 9, `JointPQuatW`: 10, `JointCPosX`: 11, `JointCPosY`: 12, `JointCPosZ`: 13, `JointCQuatX`: 14, `JointCQuatY`: 15, `JointCQuatZ`: 16, `JointCQuatW`: 17, `JointLinearDoFN`: 18, `JointAngularDoFN`: 19, `JointDoF1`: 20, `JointDoF2`: 21, `JointDoF3`: 22, `JointDoF4`: 23, `JointDoF5`: 24, `JointDoF6`: 25, `JointPForceX`: 26, `JointPForceY`: 27, `JointPForceZ`: 28, `JointPTorqueX`: 29, `JointPTorqueY`: 30, `JointPTorqueZ`: 31, `JointCForceX`: 32, `JointCForceY`: 33, `JointCForceZ`: 34, `JointCTorqueX`: 35, `JointCTorqueY`: 36, `JointCTorqueZ`: 37, `JointPDeltaX`: 38, `JointPDeltaY`: 39, `JointPDeltaZ`: 40, `JointPAngDeltaX`: 41, `JointPAngDeltaY`: 42, `JointPAngDeltaZ`: 43, `JointCDeltaX`: 44, `JointCDeltaY`: 45, `JointCDeltaZ`: 46, `JointCAngDeltaX`: 47, `JointCAngDeltaY`: 48, `JointCAngDeltaZ`: 49} +var _JointVarsValueMap = map[string]JointVars{`JointType`: 0, `JointEnabled`: 1, `JointParentFixed`: 2, `JointParent`: 3, `JointChild`: 4, `JointPPosX`: 5, `JointPPosY`: 6, `JointPPosZ`: 7, `JointPQuatX`: 8, `JointPQuatY`: 9, `JointPQuatZ`: 10, `JointPQuatW`: 11, `JointCPosX`: 12, `JointCPosY`: 13, `JointCPosZ`: 14, `JointCQuatX`: 15, `JointCQuatY`: 16, `JointCQuatZ`: 17, `JointCQuatW`: 18, `JointLinearDoFN`: 19, `JointAngularDoFN`: 20, `JointDoF1`: 21, `JointDoF2`: 22, `JointDoF3`: 23, `JointDoF4`: 24, `JointDoF5`: 25, `JointDoF6`: 26, `JointPForceX`: 27, `JointPForceY`: 28, `JointPForceZ`: 29, `JointPTorqueX`: 30, `JointPTorqueY`: 31, `JointPTorqueZ`: 32, `JointCForceX`: 33, `JointCForceY`: 34, `JointCForceZ`: 35, `JointCTorqueX`: 36, `JointCTorqueY`: 37, `JointCTorqueZ`: 38} -var _JointVarsDescMap = map[JointVars]string{0: `JointType (as an int32 from bits).`, 1: `JointEnabled allows joints to be dynamically enabled.`, 2: `JointParent is the dynamic body index for parent body. Can be -1 for a fixed parent for absolute anchor.`, 3: `JointChild is the dynamic body index for child body.`, 4: `relative position of joint, in parent frame. This is prior to parent body rotation.`, 5: ``, 6: ``, 7: `relative orientation of joint, in parent frame. This is prior to parent body rotation.`, 8: ``, 9: ``, 10: ``, 11: `relative position of joint, in child frame. This is prior to child body rotation.`, 12: ``, 13: ``, 14: `relative orientation of joint, in child frame. This is prior to parent body rotation.`, 15: ``, 16: ``, 17: ``, 18: `JointLinearDoFN is the number of linear degrees-of-freedom for the joint.`, 19: `JointAngularDoFN is the number of angular degrees-of-freedom for the joint.`, 20: `indexes in JointDoFs for each DoF`, 21: ``, 22: ``, 23: `angular starts here for Free, Distance, D6`, 24: ``, 25: ``, 26: `Computed parent joint force value.`, 27: ``, 28: ``, 29: `Computed parent joint torque value.`, 30: ``, 31: ``, 32: `Computed child joint force value.`, 33: ``, 34: ``, 35: `Computed child joint torque value.`, 36: ``, 37: ``, 38: `Computed parent joint delta value.`, 39: ``, 40: ``, 41: `Computed parent joint angdelta value.`, 42: ``, 43: ``, 44: `Computed child joint delta value.`, 45: ``, 46: ``, 47: `Computed child joint angdelta value.`, 48: ``, 49: ``} +var _JointVarsDescMap = map[JointVars]string{0: `JointType (as an int32 from bits).`, 1: `JointEnabled allows joints to be dynamically enabled.`, 2: `JointParentFixed means that the parent is NOT updated based on the forces and positions for this joint. This can make dynamics cleaner when full accuracy is not necessary.`, 3: `JointParent is the dynamic body index for parent body. Can be -1 for a fixed parent for absolute anchor.`, 4: `JointChild is the dynamic body index for child body.`, 5: `relative position of joint, in parent frame. This is prior to parent body rotation.`, 6: ``, 7: ``, 8: `relative orientation of joint, in parent frame. This is prior to parent body rotation.`, 9: ``, 10: ``, 11: ``, 12: `relative position of joint, in child frame. This is prior to child body rotation.`, 13: ``, 14: ``, 15: `relative orientation of joint, in child frame. This is prior to parent body rotation.`, 16: ``, 17: ``, 18: ``, 19: `JointLinearDoFN is the number of linear degrees-of-freedom for the joint.`, 20: `JointAngularDoFN is the number of angular degrees-of-freedom for the joint.`, 21: `indexes in JointDoFs for each DoF`, 22: ``, 23: ``, 24: `angular starts here for Free, Distance, D6`, 25: ``, 26: ``, 27: `Computed parent joint force value.`, 28: ``, 29: ``, 30: `Computed parent joint torque value.`, 31: ``, 32: ``, 33: `Computed child joint force value.`, 34: ``, 35: ``, 36: `Computed child joint torque value.`, 37: ``, 38: ``} -var _JointVarsMap = map[JointVars]string{0: `JointType`, 1: `JointEnabled`, 2: `JointParent`, 3: `JointChild`, 4: `JointPPosX`, 5: `JointPPosY`, 6: `JointPPosZ`, 7: `JointPQuatX`, 8: `JointPQuatY`, 9: `JointPQuatZ`, 10: `JointPQuatW`, 11: `JointCPosX`, 12: `JointCPosY`, 13: `JointCPosZ`, 14: `JointCQuatX`, 15: `JointCQuatY`, 16: `JointCQuatZ`, 17: `JointCQuatW`, 18: `JointLinearDoFN`, 19: `JointAngularDoFN`, 20: `JointDoF1`, 21: `JointDoF2`, 22: `JointDoF3`, 23: `JointDoF4`, 24: `JointDoF5`, 25: `JointDoF6`, 26: `JointPForceX`, 27: `JointPForceY`, 28: `JointPForceZ`, 29: `JointPTorqueX`, 30: `JointPTorqueY`, 31: `JointPTorqueZ`, 32: `JointCForceX`, 33: `JointCForceY`, 34: `JointCForceZ`, 35: `JointCTorqueX`, 36: `JointCTorqueY`, 37: `JointCTorqueZ`, 38: `JointPDeltaX`, 39: `JointPDeltaY`, 40: `JointPDeltaZ`, 41: `JointPAngDeltaX`, 42: `JointPAngDeltaY`, 43: `JointPAngDeltaZ`, 44: `JointCDeltaX`, 45: `JointCDeltaY`, 46: `JointCDeltaZ`, 47: `JointCAngDeltaX`, 48: `JointCAngDeltaY`, 49: `JointCAngDeltaZ`} +var _JointVarsMap = map[JointVars]string{0: `JointType`, 1: `JointEnabled`, 2: `JointParentFixed`, 3: `JointParent`, 4: `JointChild`, 5: `JointPPosX`, 6: `JointPPosY`, 7: `JointPPosZ`, 8: `JointPQuatX`, 9: `JointPQuatY`, 10: `JointPQuatZ`, 11: `JointPQuatW`, 12: `JointCPosX`, 13: `JointCPosY`, 14: `JointCPosZ`, 15: `JointCQuatX`, 16: `JointCQuatY`, 17: `JointCQuatZ`, 18: `JointCQuatW`, 19: `JointLinearDoFN`, 20: `JointAngularDoFN`, 21: `JointDoF1`, 22: `JointDoF2`, 23: `JointDoF3`, 24: `JointDoF4`, 25: `JointDoF5`, 26: `JointDoF6`, 27: `JointPForceX`, 28: `JointPForceY`, 29: `JointPForceZ`, 30: `JointPTorqueX`, 31: `JointPTorqueY`, 32: `JointPTorqueZ`, 33: `JointCForceX`, 34: `JointCForceY`, 35: `JointCForceZ`, 36: `JointCTorqueX`, 37: `JointCTorqueY`, 38: `JointCTorqueZ`} // String returns the string representation of this JointVars value. func (i JointVars) String() string { return enums.String(i, _JointVarsMap) } diff --git a/physics/examples/pendula/pendula.go b/physics/examples/pendula/pendula.go index e5f2ccc9..c55d7dfe 100644 --- a/physics/examples/pendula/pendula.go +++ b/physics/examples/pendula/pendula.go @@ -92,6 +92,7 @@ func main() { obj := wld.NewObject() ml := ed.Model + // ml.GPU = false sc := ed.Scene rot := math32.NewQuatIdentity() rleft := math32.NewQuatAxisAngle(math32.Vec3(0, 0, 1), -math32.Pi/2) diff --git a/physics/examples/virtroom/typegen.go b/physics/examples/virtroom/typegen.go index 712d128c..fbfbb40d 100644 --- a/physics/examples/virtroom/typegen.go +++ b/physics/examples/virtroom/typegen.go @@ -6,4 +6,4 @@ import ( "cogentcore.org/core/types" ) -var _ = types.AddType(&types.Type{Name: "main.Env", IDName: "env", Doc: "Env encapsulates the virtual environment", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Methods: []types.Method{{Name: "InitState", Doc: "Initstate reinitializes the physics model state.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "GrabEyeImg", Doc: "GrabEyeImg takes a snapshot from the perspective of Emer's right eye", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "ModelStep", Doc: "ModelStep does one step of the physics model.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "StepForward", Doc: "StepForward moves Emer forward in current facing direction one step,\nand takes GrabEyeImg", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "StepBackward", Doc: "StepBackward moves Emer backward in current facing direction one step, and takes GrabEyeImg", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "RotBodyLeft", Doc: "RotBodyLeft rotates emer left and takes GrabEyeImg", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "RotBodyRight", Doc: "RotBodyRight rotates emer right and takes GrabEyeImg", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "RotHeadLeft", Doc: "RotHeadLeft rotates emer left and takes GrabEyeImg", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "RotHeadRight", Doc: "RotHeadRight rotates emer right and takes GrabEyeImg", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}}, Fields: []types.Field{{Name: "EmerAngry", Doc: "if true, emer is angry: changes face color"}, {Name: "EmerHt", Doc: "height of emer"}, {Name: "MoveStep", Doc: "how far to move every step"}, {Name: "RotStep", Doc: "how far to rotate every step"}, {Name: "ModelSteps", Doc: "number of model steps to take"}, {Name: "Width", Doc: "width of room"}, {Name: "Depth", Doc: "depth of room"}, {Name: "Height", Doc: "height of room"}, {Name: "Thick", Doc: "thickness of walls of room"}, {Name: "DepthVals", Doc: "current depth map"}, {Name: "Camera", Doc: "offscreen render camera settings"}, {Name: "DepthMap", Doc: "color map to use for rendering depth map"}, {Name: "Physics", Doc: "The core physics elements: Model, Builder, Scene"}, {Name: "SceneEditor", Doc: "3D visualization of the Scene"}, {Name: "Emer", Doc: "emer object"}, {Name: "EmerJoint", Doc: "emer PlaneXZ joint for controlling motion"}, {Name: "EyeR", Doc: "Right eye of emer"}, {Name: "EyeRImg", Doc: "snapshot image"}, {Name: "NeckJoint", Doc: "ball joint for the neck."}, {Name: "DepthImage", Doc: "depth map image"}}}) +var _ = types.AddType(&types.Type{Name: "main.Env", IDName: "env", Doc: "Env encapsulates the virtual environment", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Methods: []types.Method{{Name: "InitState", Doc: "Initstate reinitializes the physics model state.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "GrabEyeImg", Doc: "GrabEyeImg takes a snapshot from the perspective of Emer's right eye", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "ModelStep", Doc: "ModelStep does one step of the physics model.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "StepForward", Doc: "StepForward moves Emer forward in current facing direction one step,\nand takes GrabEyeImg", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "StepBackward", Doc: "StepBackward moves Emer backward in current facing direction one step, and takes GrabEyeImg", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "RotBodyLeft", Doc: "RotBodyLeft rotates emer left and takes GrabEyeImg", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "RotBodyRight", Doc: "RotBodyRight rotates emer right and takes GrabEyeImg", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "RotHeadLeft", Doc: "RotHeadLeft rotates emer left and takes GrabEyeImg", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "RotHeadRight", Doc: "RotHeadRight rotates emer right and takes GrabEyeImg", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}}, Fields: []types.Field{{Name: "Emer", Doc: "Emer state"}, {Name: "Stiff", Doc: "Stiffness for actions"}, {Name: "MoveStep", Doc: "how far to move every step"}, {Name: "RotStep", Doc: "how far to rotate every step"}, {Name: "ModelSteps", Doc: "number of model steps to take"}, {Name: "Width", Doc: "width of room"}, {Name: "Depth", Doc: "depth of room"}, {Name: "Height", Doc: "height of room"}, {Name: "Thick", Doc: "thickness of walls of room"}, {Name: "DepthVals", Doc: "current depth map"}, {Name: "Camera", Doc: "offscreen render camera settings"}, {Name: "DepthMap", Doc: "color map to use for rendering depth map"}, {Name: "Physics", Doc: "The core physics elements: Model, Builder, Scene"}, {Name: "SceneEditor", Doc: "3D visualization of the Scene"}, {Name: "EyeRImg", Doc: "snapshot image"}, {Name: "DepthImage", Doc: "depth map image"}}}) diff --git a/physics/examples/virtroom/virtroom.go b/physics/examples/virtroom/virtroom.go index ab2f6b26..10eaa7ff 100644 --- a/physics/examples/virtroom/virtroom.go +++ b/physics/examples/virtroom/virtroom.go @@ -45,14 +45,41 @@ func main() { b.RunMainWindow() } +type Emer struct { + // if true, emer is angry: changes face color + Angry bool + + // height of emer + Height float32 + + // emer object + Obj *builder.Object `display:"-"` + + // Revolute joint controlling orientation. + Orient *builder.Joint + + // PlaneXZ joint for controlling 2D position. + XZ *builder.Joint + + // ball joint for the neck. + Neck *builder.Joint + + // Right eye of emer + EyeR *builder.Body `display:"-"` +} + +func (em *Emer) Defaults() { + em.Height = 1 +} + // Env encapsulates the virtual environment type Env struct { //types:add - // if true, emer is angry: changes face color - EmerAngry bool + // Emer state + Emer Emer `new-window:"+"` - // height of emer - EmerHt float32 + // Stiffness for actions + Stiff float32 // how far to move every step MoveStep float32 @@ -90,32 +117,21 @@ type Env struct { //types:add // 3D visualization of the Scene SceneEditor *xyzcore.SceneEditor - // emer object - Emer *builder.Object `display:"-"` - - // emer PlaneXZ joint for controlling motion - EmerJoint *builder.Joint - - // Right eye of emer - EyeR *builder.Body `display:"-"` - // snapshot image EyeRImg *core.Image `display:"-"` - // ball joint for the neck. - NeckJoint *builder.Joint - // depth map image DepthImage *core.Image `display:"-"` } func (ev *Env) Defaults() { + ev.Emer.Defaults() ev.Width = 10 ev.Depth = 15 ev.Height = 2 ev.Thick = 0.2 - ev.EmerHt = 1 - ev.MoveStep = ev.EmerHt * .2 + ev.Stiff = 1000 + ev.MoveStep = ev.Emer.Height * .2 ev.RotStep = 15 ev.ModelSteps = 100 ev.DepthMap = core.ColorMapName("ColdHot") @@ -137,13 +153,13 @@ func (ev *Env) MakeWorld(sc *xyz.Scene) { wl := ev.Physics.Builder.NewGlobalWorld() ev.MakeRoom(wl, "room1", ev.Width, ev.Depth, ev.Height, ev.Thick) ew := ev.Physics.Builder.NewWorld() - ev.MakeEmer(ew, "emer", ev.EmerHt) + ev.MakeEmer(ew, &ev.Emer, "emer") // ev.Physics.Builder.ReplicateWorld(1, 8, 2) ev.Physics.Build() - params := physics.GetParams(0) - params.Gravity.Y = 0 - params.MaxForce = 1.0e3 - params.AngularDamping = 0.5 + // params := physics.GetParams(0) + // params.Gravity.Y = 0 + // params.MaxForce = 1.0e3 + // params.AngularDamping = 0.5 // params.SubSteps = 1 } @@ -160,7 +176,10 @@ func (ev *Env) ConfigView3D(sc *xyz.Scene) { // RenderEyeImg returns a snapshot from the perspective of Emer's right eye func (ev *Env) RenderEyeImg() image.Image { - return ev.Physics.Scene.RenderFrom(ev.EyeR.Skin, &ev.Camera, 0) + if ev.Emer.EyeR == nil { + return nil + } + return ev.Physics.Scene.RenderFrom(ev.Emer.EyeR.Skin, &ev.Camera, 0) } // GrabEyeImg takes a snapshot from the perspective of Emer's right eye @@ -209,7 +228,7 @@ func (ev *Env) ModelStep() { //types:add // } // } // } - ev.EmerAngry = false + ev.Emer.Angry = false // if len(ev.Contacts) > 1 { // turn around // ev.EmerAngry = true // fmt.Printf("hit wall: turn around!\n") @@ -223,52 +242,44 @@ func (ev *Env) ModelStep() { //types:add // StepForward moves Emer forward in current facing direction one step, // and takes GrabEyeImg func (ev *Env) StepForward() { //types:add - ev.Emer.PoseFromPhysics() // doesn't integrate well with joints.. // ev.Emer.MoveOnAxisBody(0, 0, 0, 1, -ev.MoveStep) // ev.Emer.PoseToPhysics() - ev.EmerJoint.AddPlaneXZPos(math32.Pi*.5, -ev.MoveStep, 1000) + ang := math32.Pi*.5 - ev.Emer.XZ.DoF(2).Current.Pos + // ang := float32(math32.Pi * .5) + ev.Emer.XZ.AddPlaneXZPos(ang, -ev.MoveStep, ev.Stiff) ev.ModelStep() } // StepBackward moves Emer backward in current facing direction one step, and takes GrabEyeImg func (ev *Env) StepBackward() { //types:add - ev.Emer.PoseFromPhysics() - // ev.Emer.MoveOnAxisBody(0, 0, 0, 1, ev.MoveStep) - // ev.Emer.PoseToPhysics() - ev.EmerJoint.AddPlaneXZPos(math32.Pi*.5, ev.MoveStep, 1000) + ang := math32.Pi*.5 - ev.Emer.XZ.DoF(2).Current.Pos + // ang := float32(math32.Pi * .5) + ev.Emer.XZ.AddPlaneXZPos(ang, ev.MoveStep, ev.Stiff) ev.ModelStep() } // RotBodyLeft rotates emer left and takes GrabEyeImg func (ev *Env) RotBodyLeft() { //types:add - ev.Emer.PoseFromPhysics() - // ev.Emer.RotateOnAxisBody(0, 0, 1, 0, ev.RotStep) - // ev.Emer.PoseToPhysics() - ev.EmerJoint.AddTargetPos(2, math32.DegToRad(ev.RotStep), 1000) + ev.Emer.XZ.AddTargetAngle(2, ev.RotStep, ev.Stiff) ev.ModelStep() } // RotBodyRight rotates emer right and takes GrabEyeImg func (ev *Env) RotBodyRight() { //types:add - ev.Emer.PoseFromPhysics() - // ev.Emer.RotateOnAxisBody(0, 0, 1, 0, -ev.RotStep) - // ev.Emer.PoseToPhysics() - ev.EmerJoint.AddTargetPos(2, math32.DegToRad(-ev.RotStep), 1000) + ev.Emer.XZ.AddTargetAngle(2, -ev.RotStep, ev.Stiff) ev.ModelStep() } // RotHeadLeft rotates emer left and takes GrabEyeImg func (ev *Env) RotHeadLeft() { //types:add - ev.Emer.PoseFromPhysics() - ev.NeckJoint.AddTargetAngle(1, ev.RotStep, 1000) + ev.Emer.Neck.AddTargetAngle(0, ev.RotStep, ev.Stiff) ev.ModelStep() } // RotHeadRight rotates emer right and takes GrabEyeImg func (ev *Env) RotHeadRight() { //types:add - ev.Emer.PoseFromPhysics() - ev.NeckJoint.AddTargetAngle(1, -ev.RotStep, 1000) + ev.Emer.Neck.AddTargetAngle(0, -ev.RotStep, ev.Stiff) ev.ModelStep() } @@ -294,8 +305,8 @@ func (ev *Env) MakeRoom(wl *builder.World, name string, width, depth, height, th } // MakeEmer constructs a new Emer virtual robot of given height (e.g., 1). -func (ev *Env) MakeEmer(wl *builder.World, name string, height float32) { - hh := height / 2 +func (ev *Env) MakeEmer(wl *builder.World, em *Emer, name string) { + hh := em.Height / 2 hw := hh * .4 hd := hh * .15 headsz := hd * 1.5 @@ -303,13 +314,13 @@ func (ev *Env) MakeEmer(wl *builder.World, name string, height float32) { mass := float32(1) // kg rot := math32.NewQuatIdentity() obj := wl.NewObject() - ev.Emer = obj + em.Obj = obj sc := ev.Physics.Scene emr := obj.NewDynamicSkin(sc, name+"_body", physics.Box, "purple", mass, math32.Vec3(hw, hh, hd), math32.Vec3(0, hh, 0), rot) // body := physics.NewCapsule(emr, "body", math32.Vec3(0, hh, 0), hh, hw) // body := physics.NewCylinder(emr, "body", math32.Vec3(0, hh, 0), hh, hw) - ev.EmerJoint = obj.NewJointPlaneXZ(nil, emr, math32.Vec3(0, 0, 0), math32.Vec3(0, -hh, 0)) - emr.Group = 0 + em.XZ = obj.NewJointPlaneXZ(nil, emr, math32.Vec3(0, 0, 0), math32.Vec3(0, -hh, 0)) + emr.Group = 0 // no collide (temporary) headPos := math32.Vec3(0, 2*hh+headsz, 0) head := obj.NewDynamicSkin(sc, name+"_head", physics.Box, "tan", mass*.1, math32.Vec3(headsz, headsz, headsz), headPos, rot) @@ -319,23 +330,27 @@ func (ev *Env) MakeEmer(wl *builder.World, name string, height float32) { hdsk.BoxInit(sld) sld.Updater(func() { clr := hdsk.Color - if ev.EmerAngry { + if ev.Emer.Angry { clr = "pink" } hdsk.UpdateColor(clr, sld) }) } - ev.NeckJoint = obj.NewJointBall(emr, head, math32.Vec3(0, hh, 0), math32.Vec3(0, -headsz, 0)) + em.Neck = obj.NewJointBall(emr, head, math32.Vec3(0, hh, 0), math32.Vec3(0, -headsz, 0)) + em.Neck.ParentFixed = true + // obj.NewJointFixed(emr, head, math32.Vec3(0, hh, 0), math32.Vec3(0, -headsz, 0)) eyeoff := math32.Vec3(-headsz*.6, headsz*.1, -(headsz + eyesz*.3)) - bd := obj.NewDynamicSkin(sc, name+"_eye-l", physics.Box, "green", mass*.01, math32.Vec3(eyesz, eyesz*.5, eyesz*.2), headPos.Add(eyeoff), rot) + bd := obj.NewDynamicSkin(sc, name+"_eye-l", physics.Box, "green", mass*.001, math32.Vec3(eyesz, eyesz*.5, eyesz*.2), headPos.Add(eyeoff), rot) bd.Group = 0 - obj.NewJointFixed(head, bd, eyeoff, math32.Vec3(0, 0, -eyesz*.3)) + ej := obj.NewJointFixed(head, bd, eyeoff, math32.Vec3(0, 0, -eyesz*.3)) + ej.ParentFixed = true eyeoff = math32.Vec3(headsz*.6, headsz*.1, -(headsz + eyesz*.3)) - ev.EyeR = obj.NewDynamicSkin(sc, name+"_eye-r", physics.Box, "green", mass*.01, math32.Vec3(eyesz, eyesz*.5, eyesz*.2), headPos.Add(eyeoff), rot) - ev.EyeR.Group = 0 - obj.NewJointFixed(head, ev.EyeR, eyeoff, math32.Vec3(0, 0, -eyesz*.3)) + em.EyeR = obj.NewDynamicSkin(sc, name+"_eye-r", physics.Box, "green", mass*.001, math32.Vec3(eyesz, eyesz*.5, eyesz*.2), headPos.Add(eyeoff), rot) + em.EyeR.Group = 0 + ej = obj.NewJointFixed(head, em.EyeR, eyeoff, math32.Vec3(0, 0, -eyesz*.3)) + ej.ParentFixed = true } func (ev *Env) ConfigGUI() *core.Body { diff --git a/physics/gosl.go b/physics/gosl.go index afde8063..2c30106d 100644 --- a/physics/gosl.go +++ b/physics/gosl.go @@ -41,16 +41,17 @@ type GPUVars int32 //enums:enum const ( ParamsVar GPUVars = 0 BodiesVar GPUVars = 1 - JointsVar GPUVars = 2 - JointDoFsVar GPUVars = 3 - BodyJointsVar GPUVars = 4 - BodyCollidePairsVar GPUVars = 5 - DynamicsVar GPUVars = 6 - BroadContactsNVar GPUVars = 7 - BroadContactsVar GPUVars = 8 - ContactsNVar GPUVars = 9 - ContactsVar GPUVars = 10 - JointControlsVar GPUVars = 11 + ObjectsVar GPUVars = 2 + BodyJointsVar GPUVars = 3 + JointsVar GPUVars = 4 + JointDoFsVar GPUVars = 5 + BodyCollidePairsVar GPUVars = 6 + DynamicsVar GPUVars = 7 + BroadContactsNVar GPUVars = 8 + BroadContactsVar GPUVars = 9 + ContactsNVar GPUVars = 10 + ContactsVar GPUVars = 11 + JointControlsVar GPUVars = 12 ) // Tensor stride variables @@ -90,9 +91,10 @@ func GPUInit() { var vr *gpu.Var _ = vr vr = sgp.Add("Bodies", gpu.Float32, 1, gpu.ComputeShader) + vr = sgp.Add("Objects", gpu.Int32, 1, gpu.ComputeShader) + vr = sgp.Add("BodyJoints", gpu.Int32, 1, gpu.ComputeShader) vr = sgp.Add("Joints", gpu.Float32, 1, gpu.ComputeShader) vr = sgp.Add("JointDoFs", gpu.Float32, 1, gpu.ComputeShader) - vr = sgp.Add("BodyJoints", gpu.Int32, 1, gpu.ComputeShader) vr = sgp.Add("BodyCollidePairs", gpu.Int32, 1, gpu.ComputeShader) sgp.SetNValues(1) } @@ -161,13 +163,6 @@ func GPUInit() { pl.AddVarUsed(2, "ContactsN") pl.AddVarUsed(2, "Dynamics") pl.AddVarUsed(0, "Params") - pl = gpu.NewComputePipelineShaderFS(shaders, "shaders/StepBodyJointDeltas.wgsl", sy) - pl.AddVarUsed(0, "TensorStrides") - pl.AddVarUsed(1, "Bodies") - pl.AddVarUsed(1, "BodyJoints") - pl.AddVarUsed(2, "Dynamics") - pl.AddVarUsed(1, "Joints") - pl.AddVarUsed(0, "Params") pl = gpu.NewComputePipelineShaderFS(shaders, "shaders/StepInit.wgsl", sy) pl.AddVarUsed(0, "TensorStrides") pl.AddVarUsed(2, "BroadContactsN") @@ -193,6 +188,7 @@ func GPUInit() { pl.AddVarUsed(3, "JointControls") pl.AddVarUsed(1, "JointDoFs") pl.AddVarUsed(1, "Joints") + pl.AddVarUsed(1, "Objects") pl.AddVarUsed(0, "Params") sy.Config() } @@ -507,48 +503,6 @@ func RunOneStepBodyContacts(n int, syncVars ...GPUVars) { RunStepBodyContactsCPU(n) } } -// RunStepBodyJointDeltas runs the StepBodyJointDeltas kernel with given number of elements, -// on either the CPU or GPU depending on the UseGPU variable. -// Can call multiple Run* kernels in a row, which are then all launched -// in the same command submission on the GPU, which is by far the most efficient. -// MUST call RunDone (with optional vars to sync) after all Run calls. -// Alternatively, a single-shot RunOneStepBodyJointDeltas call does Run and Done for a -// single run-and-sync case. -func RunStepBodyJointDeltas(n int) { - if UseGPU { - RunStepBodyJointDeltasGPU(n) - } else { - RunStepBodyJointDeltasCPU(n) - } -} - -// RunStepBodyJointDeltasGPU runs the StepBodyJointDeltas kernel on the GPU. See [RunStepBodyJointDeltas] for more info. -func RunStepBodyJointDeltasGPU(n int) { - sy := GPUSystem - pl := sy.ComputePipelines["StepBodyJointDeltas"] - ce, _ := sy.BeginComputePass() - pl.Dispatch1D(ce, n, 64) -} - -// RunStepBodyJointDeltasCPU runs the StepBodyJointDeltas kernel on the CPU. -func RunStepBodyJointDeltasCPU(n int) { - gpu.VectorizeFunc(0, n, StepBodyJointDeltas) -} - -// RunOneStepBodyJointDeltas runs the StepBodyJointDeltas kernel with given number of elements, -// on either the CPU or GPU depending on the UseGPU variable. -// This version then calls RunDone with the given variables to sync -// after the Run, for a single-shot Run-and-Done call. If multiple kernels -// can be run in sequence, it is much more efficient to do multiple Run* -// calls followed by a RunDone call. -func RunOneStepBodyJointDeltas(n int, syncVars ...GPUVars) { - if UseGPU { - RunStepBodyJointDeltasGPU(n) - RunDone(syncVars...) - } else { - RunStepBodyJointDeltasCPU(n) - } -} // RunStepInit runs the StepInit kernel with given number of elements, // on either the CPU or GPU depending on the UseGPU variable. // Can call multiple Run* kernels in a row, which are then all launched @@ -748,15 +702,18 @@ func ToGPU(vars ...GPUVars) { case BodiesVar: v, _ := syVars.ValueByIndex(1, "Bodies", 0) gpu.SetValueFrom(v, Bodies.Values) + case ObjectsVar: + v, _ := syVars.ValueByIndex(1, "Objects", 0) + gpu.SetValueFrom(v, Objects.Values) + case BodyJointsVar: + v, _ := syVars.ValueByIndex(1, "BodyJoints", 0) + gpu.SetValueFrom(v, BodyJoints.Values) case JointsVar: v, _ := syVars.ValueByIndex(1, "Joints", 0) gpu.SetValueFrom(v, Joints.Values) case JointDoFsVar: v, _ := syVars.ValueByIndex(1, "JointDoFs", 0) gpu.SetValueFrom(v, JointDoFs.Values) - case BodyJointsVar: - v, _ := syVars.ValueByIndex(1, "BodyJoints", 0) - gpu.SetValueFrom(v, BodyJoints.Values) case BodyCollidePairsVar: v, _ := syVars.ValueByIndex(1, "BodyCollidePairs", 0) gpu.SetValueFrom(v, BodyCollidePairs.Values) @@ -799,29 +756,31 @@ func ToGPUTensorStrides() { } sy := GPUSystem syVars := sy.Vars() - TensorStrides.SetShapeSizes(110) + TensorStrides.SetShapeSizes(120) TensorStrides.SetInt1D(Bodies.Shape().Strides[0], 0) TensorStrides.SetInt1D(Bodies.Shape().Strides[1], 1) - TensorStrides.SetInt1D(Joints.Shape().Strides[0], 10) - TensorStrides.SetInt1D(Joints.Shape().Strides[1], 11) - TensorStrides.SetInt1D(JointDoFs.Shape().Strides[0], 20) - TensorStrides.SetInt1D(JointDoFs.Shape().Strides[1], 21) - TensorStrides.SetInt1D(BodyJoints.Shape().Strides[0], 30) - TensorStrides.SetInt1D(BodyJoints.Shape().Strides[1], 31) - TensorStrides.SetInt1D(BodyJoints.Shape().Strides[2], 32) - TensorStrides.SetInt1D(BodyCollidePairs.Shape().Strides[0], 40) - TensorStrides.SetInt1D(BodyCollidePairs.Shape().Strides[1], 41) - TensorStrides.SetInt1D(Dynamics.Shape().Strides[0], 50) - TensorStrides.SetInt1D(Dynamics.Shape().Strides[1], 51) - TensorStrides.SetInt1D(Dynamics.Shape().Strides[2], 52) - TensorStrides.SetInt1D(BroadContactsN.Shape().Strides[0], 60) - TensorStrides.SetInt1D(BroadContacts.Shape().Strides[0], 70) - TensorStrides.SetInt1D(BroadContacts.Shape().Strides[1], 71) - TensorStrides.SetInt1D(ContactsN.Shape().Strides[0], 80) - TensorStrides.SetInt1D(Contacts.Shape().Strides[0], 90) - TensorStrides.SetInt1D(Contacts.Shape().Strides[1], 91) - TensorStrides.SetInt1D(JointControls.Shape().Strides[0], 100) - TensorStrides.SetInt1D(JointControls.Shape().Strides[1], 101) + TensorStrides.SetInt1D(Objects.Shape().Strides[0], 10) + TensorStrides.SetInt1D(Objects.Shape().Strides[1], 11) + TensorStrides.SetInt1D(BodyJoints.Shape().Strides[0], 20) + TensorStrides.SetInt1D(BodyJoints.Shape().Strides[1], 21) + TensorStrides.SetInt1D(BodyJoints.Shape().Strides[2], 22) + TensorStrides.SetInt1D(Joints.Shape().Strides[0], 30) + TensorStrides.SetInt1D(Joints.Shape().Strides[1], 31) + TensorStrides.SetInt1D(JointDoFs.Shape().Strides[0], 40) + TensorStrides.SetInt1D(JointDoFs.Shape().Strides[1], 41) + TensorStrides.SetInt1D(BodyCollidePairs.Shape().Strides[0], 50) + TensorStrides.SetInt1D(BodyCollidePairs.Shape().Strides[1], 51) + TensorStrides.SetInt1D(Dynamics.Shape().Strides[0], 60) + TensorStrides.SetInt1D(Dynamics.Shape().Strides[1], 61) + TensorStrides.SetInt1D(Dynamics.Shape().Strides[2], 62) + TensorStrides.SetInt1D(BroadContactsN.Shape().Strides[0], 70) + TensorStrides.SetInt1D(BroadContacts.Shape().Strides[0], 80) + TensorStrides.SetInt1D(BroadContacts.Shape().Strides[1], 81) + TensorStrides.SetInt1D(ContactsN.Shape().Strides[0], 90) + TensorStrides.SetInt1D(Contacts.Shape().Strides[0], 100) + TensorStrides.SetInt1D(Contacts.Shape().Strides[1], 101) + TensorStrides.SetInt1D(JointControls.Shape().Strides[0], 110) + TensorStrides.SetInt1D(JointControls.Shape().Strides[1], 111) v, _ := syVars.ValueByIndex(0, "TensorStrides", 0) gpu.SetValueFrom(v, TensorStrides.Values) } @@ -838,15 +797,18 @@ func ReadFromGPU(vars ...GPUVars) { case BodiesVar: v, _ := syVars.ValueByIndex(1, "Bodies", 0) v.GPUToRead(sy.CommandEncoder) + case ObjectsVar: + v, _ := syVars.ValueByIndex(1, "Objects", 0) + v.GPUToRead(sy.CommandEncoder) + case BodyJointsVar: + v, _ := syVars.ValueByIndex(1, "BodyJoints", 0) + v.GPUToRead(sy.CommandEncoder) case JointsVar: v, _ := syVars.ValueByIndex(1, "Joints", 0) v.GPUToRead(sy.CommandEncoder) case JointDoFsVar: v, _ := syVars.ValueByIndex(1, "JointDoFs", 0) v.GPUToRead(sy.CommandEncoder) - case BodyJointsVar: - v, _ := syVars.ValueByIndex(1, "BodyJoints", 0) - v.GPUToRead(sy.CommandEncoder) case BodyCollidePairsVar: v, _ := syVars.ValueByIndex(1, "BodyCollidePairs", 0) v.GPUToRead(sy.CommandEncoder) @@ -886,6 +848,14 @@ func SyncFromGPU(vars ...GPUVars) { v, _ := syVars.ValueByIndex(1, "Bodies", 0) v.ReadSync() gpu.ReadToBytes(v, Bodies.Values) + case ObjectsVar: + v, _ := syVars.ValueByIndex(1, "Objects", 0) + v.ReadSync() + gpu.ReadToBytes(v, Objects.Values) + case BodyJointsVar: + v, _ := syVars.ValueByIndex(1, "BodyJoints", 0) + v.ReadSync() + gpu.ReadToBytes(v, BodyJoints.Values) case JointsVar: v, _ := syVars.ValueByIndex(1, "Joints", 0) v.ReadSync() @@ -894,10 +864,6 @@ func SyncFromGPU(vars ...GPUVars) { v, _ := syVars.ValueByIndex(1, "JointDoFs", 0) v.ReadSync() gpu.ReadToBytes(v, JointDoFs.Values) - case BodyJointsVar: - v, _ := syVars.ValueByIndex(1, "BodyJoints", 0) - v.ReadSync() - gpu.ReadToBytes(v, BodyJoints.Values) case BodyCollidePairsVar: v, _ := syVars.ValueByIndex(1, "BodyCollidePairs", 0) v.ReadSync() diff --git a/physics/joint.go b/physics/joint.go index 965b3abd..e79bdde1 100644 --- a/physics/joint.go +++ b/physics/joint.go @@ -44,7 +44,7 @@ const ( D6 // PlaneXZ is a version of D6 for navigation in the X-Z plane, - // which creates 2 linear DoF (X, Z) and 1 angular DoF (around Y axis). + // which creates 2 linear DoF (X, Z) for movement. PlaneXZ ) @@ -60,6 +60,11 @@ const ( // JointEnabled allows joints to be dynamically enabled. JointEnabled + // JointParentFixed means that the parent is NOT updated based on + // the forces and positions for this joint. This can make dynamics + // cleaner when full accuracy is not necessary. + JointParentFixed + // JointParent is the dynamic body index for parent body. // Can be -1 for a fixed parent for absolute anchor. JointParent @@ -128,26 +133,6 @@ const ( JointCTorqueX JointCTorqueY JointCTorqueZ - - // Computed parent joint delta value. - JointPDeltaX - JointPDeltaY - JointPDeltaZ - - // Computed parent joint angdelta value. - JointPAngDeltaX - JointPAngDeltaY - JointPAngDeltaZ - - // Computed child joint delta value. - JointCDeltaX - JointCDeltaY - JointCDeltaZ - - // Computed child joint angdelta value. - JointCAngDeltaX - JointCAngDeltaY - JointCAngDeltaZ ) func GetJointType(idx int32) JointTypes { @@ -171,6 +156,19 @@ func SetJointEnabled(idx int32, enabled bool) { Joints.Set(math.Float32frombits(je), int(idx), int(JointEnabled)) } +func GetJointParentFixed(idx int32) bool { + je := math.Float32bits(Joints.Value(int(idx), int(JointParentFixed))) + return je != 0 +} + +func SetJointParentFixed(idx int32, enabled bool) { + je := uint32(0) + if enabled { + je = 1 + } + Joints.Set(math.Float32frombits(je), int(idx), int(JointParentFixed)) +} + func SetJointParent(idx, bodyIdx int32) { Joints.Set(math.Float32frombits(uint32(bodyIdx)), int(idx), int(JointParent)) } @@ -293,46 +291,6 @@ func SetJointCTorque(idx int32, t math32.Vector3) { Joints.Set(t.Z, int(idx), int(JointCTorqueZ)) } -func JointPDelta(idx int32) math32.Vector3 { - return math32.Vec3(Joints.Value(int(idx), int(JointPDeltaX)), Joints.Value(int(idx), int(JointPDeltaY)), Joints.Value(int(idx), int(JointPDeltaZ))) -} - -func SetJointPDelta(idx int32, f math32.Vector3) { - Joints.Set(f.X, int(idx), int(JointPDeltaX)) - Joints.Set(f.Y, int(idx), int(JointPDeltaY)) - Joints.Set(f.Z, int(idx), int(JointPDeltaZ)) -} - -func JointPAngDelta(idx int32) math32.Vector3 { - return math32.Vec3(Joints.Value(int(idx), int(JointPAngDeltaX)), Joints.Value(int(idx), int(JointPAngDeltaY)), Joints.Value(int(idx), int(JointPAngDeltaZ))) -} - -func SetJointPAngDelta(idx int32, t math32.Vector3) { - Joints.Set(t.X, int(idx), int(JointPAngDeltaX)) - Joints.Set(t.Y, int(idx), int(JointPAngDeltaY)) - Joints.Set(t.Z, int(idx), int(JointPAngDeltaZ)) -} - -func JointCDelta(idx int32) math32.Vector3 { - return math32.Vec3(Joints.Value(int(idx), int(JointCDeltaX)), Joints.Value(int(idx), int(JointCDeltaY)), Joints.Value(int(idx), int(JointCDeltaZ))) -} - -func SetJointCDelta(idx int32, f math32.Vector3) { - Joints.Set(f.X, int(idx), int(JointCDeltaX)) - Joints.Set(f.Y, int(idx), int(JointCDeltaY)) - Joints.Set(f.Z, int(idx), int(JointCDeltaZ)) -} - -func JointCAngDelta(idx int32) math32.Vector3 { - return math32.Vec3(Joints.Value(int(idx), int(JointCAngDeltaX)), Joints.Value(int(idx), int(JointCAngDeltaY)), Joints.Value(int(idx), int(JointCAngDeltaZ))) -} - -func SetJointCAngDelta(idx int32, t math32.Vector3) { - Joints.Set(t.X, int(idx), int(JointCAngDeltaX)) - Joints.Set(t.Y, int(idx), int(JointCAngDeltaY)) - Joints.Set(t.Z, int(idx), int(JointCAngDeltaZ)) -} - // JointDoFVars are joint DoF state variables stored in tensor.Float32, // one for each DoF. type JointDoFVars int32 //enums:enum @@ -445,8 +403,8 @@ func (ml *Model) NewJointBall(parent, child int32, ppos, cpos math32.Vector3) in // NewJointPlaneXZ adds a new 3 DoF Planar motion joint suitable for // controlling the motion of a body on the standard X-Z plane (Y = up). -// The first two linear DoF control position in X, Z, and the third -// angular DoF controls rotation in the plane (along the Y axis). +// The two linear DoF control position in X, Z, and 3rd angular +// controls rotation in Y axis. // Use -1 for parent to add a world-anchored joint (typical). // ppos, cpos are the relative positions from the parent, child. // Sets relative rotation matricies to identity by default. @@ -519,7 +477,8 @@ func (ml *Model) NewJointFree(parent, child int32, ppos, cpos math32.Vector3) in func (ml *Model) newJoint(joint JointTypes, parent, child int32, ppos, cpos math32.Vector3) int32 { sizes := ml.Joints.ShapeSizes() idx := int32(sizes[0]) - ml.Params[0].JointsN = idx + 1 + params := &ml.Params[0] + params.JointsN = idx + 1 ml.Joints.SetShapeSizes(int(idx+1), int(JointVarsN)) ml.JointDefaults(idx) SetJointType(idx, joint) @@ -528,6 +487,13 @@ func (ml *Model) newJoint(joint JointTypes, parent, child int32, ppos, cpos math SetJointChild(idx, child) SetJointPPos(idx, ppos) SetJointCPos(idx, cpos) + if ml.CurrentObjectJoint >= int(params.MaxObjectJoints)-1 { + params.MaxObjectJoints = int32(ml.CurrentObjectJoint + 1) + ml.Objects.SetShapeSizes(ml.CurrentObject+1, int(params.MaxObjectJoints+1)) + } + ml.Objects.Set(idx, int(ml.CurrentObject), int(1+ml.CurrentObjectJoint)) + ml.CurrentObjectJoint++ + ml.Objects.Set(int32(ml.CurrentObjectJoint), int(ml.CurrentObject), int(0)) return idx } diff --git a/physics/joint.goal b/physics/joint.goal index d931421d..7fd7ebe3 100644 --- a/physics/joint.goal +++ b/physics/joint.goal @@ -42,7 +42,7 @@ const ( D6 // PlaneXZ is a version of D6 for navigation in the X-Z plane, - // which creates 2 linear DoF (X, Z) and 1 angular DoF (around Y axis). + // which creates 2 linear DoF (X, Z) for movement. PlaneXZ ) @@ -58,6 +58,11 @@ const ( // JointEnabled allows joints to be dynamically enabled. JointEnabled + // JointParentFixed means that the parent is NOT updated based on + // the forces and positions for this joint. This can make dynamics + // cleaner when full accuracy is not necessary. + JointParentFixed + // JointParent is the dynamic body index for parent body. // Can be -1 for a fixed parent for absolute anchor. JointParent @@ -126,26 +131,6 @@ const ( JointCTorqueX JointCTorqueY JointCTorqueZ - - // Computed parent joint delta value. - JointPDeltaX - JointPDeltaY - JointPDeltaZ - - // Computed parent joint angdelta value. - JointPAngDeltaX - JointPAngDeltaY - JointPAngDeltaZ - - // Computed child joint delta value. - JointCDeltaX - JointCDeltaY - JointCDeltaZ - - // Computed child joint angdelta value. - JointCAngDeltaX - JointCAngDeltaY - JointCAngDeltaZ ) func GetJointType(idx int32) JointTypes { @@ -169,6 +154,19 @@ func SetJointEnabled(idx int32, enabled bool) { Joints[idx, JointEnabled] = math.Float32frombits(je) } +func GetJointParentFixed(idx int32) bool { + je := math.Float32bits(Joints[idx, JointParentFixed]) + return je != 0 +} + +func SetJointParentFixed(idx int32, enabled bool) { + je := uint32(0) + if enabled { + je = 1 + } + Joints[idx, JointParentFixed] = math.Float32frombits(je) +} + func SetJointParent(idx, bodyIdx int32) { Joints[idx, JointParent] = math.Float32frombits(uint32(bodyIdx)) } @@ -291,46 +289,6 @@ func SetJointCTorque(idx int32, t math32.Vector3) { Joints[idx, JointCTorqueZ] = t.Z } -func JointPDelta(idx int32) math32.Vector3 { - return math32.Vec3(Joints[idx, JointPDeltaX], Joints[idx, JointPDeltaY], Joints[idx, JointPDeltaZ]) -} - -func SetJointPDelta(idx int32, f math32.Vector3) { - Joints[idx, JointPDeltaX] = f.X - Joints[idx, JointPDeltaY] = f.Y - Joints[idx, JointPDeltaZ] = f.Z -} - -func JointPAngDelta(idx int32) math32.Vector3 { - return math32.Vec3(Joints[idx, JointPAngDeltaX], Joints[idx, JointPAngDeltaY], Joints[idx, JointPAngDeltaZ]) -} - -func SetJointPAngDelta(idx int32, t math32.Vector3) { - Joints[idx, JointPAngDeltaX] = t.X - Joints[idx, JointPAngDeltaY] = t.Y - Joints[idx, JointPAngDeltaZ] = t.Z -} - -func JointCDelta(idx int32) math32.Vector3 { - return math32.Vec3(Joints[idx, JointCDeltaX], Joints[idx, JointCDeltaY], Joints[idx, JointCDeltaZ]) -} - -func SetJointCDelta(idx int32, f math32.Vector3) { - Joints[idx, JointCDeltaX] = f.X - Joints[idx, JointCDeltaY] = f.Y - Joints[idx, JointCDeltaZ] = f.Z -} - -func JointCAngDelta(idx int32) math32.Vector3 { - return math32.Vec3(Joints[idx, JointCAngDeltaX], Joints[idx, JointCAngDeltaY], Joints[idx, JointCAngDeltaZ]) -} - -func SetJointCAngDelta(idx int32, t math32.Vector3) { - Joints[idx, JointCAngDeltaX] = t.X - Joints[idx, JointCAngDeltaY] = t.Y - Joints[idx, JointCAngDeltaZ] = t.Z -} - // JointDoFVars are joint DoF state variables stored in tensor.Float32, // one for each DoF. type JointDoFVars int32 //enums:enum @@ -445,8 +403,8 @@ func (ml *Model) NewJointBall(parent, child int32, ppos, cpos math32.Vector3) in // NewJointPlaneXZ adds a new 3 DoF Planar motion joint suitable for // controlling the motion of a body on the standard X-Z plane (Y = up). -// The first two linear DoF control position in X, Z, and the third -// angular DoF controls rotation in the plane (along the Y axis). +// The two linear DoF control position in X, Z, and 3rd angular +// controls rotation in Y axis. // Use -1 for parent to add a world-anchored joint (typical). // ppos, cpos are the relative positions from the parent, child. // Sets relative rotation matricies to identity by default. @@ -519,7 +477,8 @@ func (ml *Model) NewJointFree(parent, child int32, ppos, cpos math32.Vector3) in func (ml *Model) newJoint(joint JointTypes, parent, child int32, ppos, cpos math32.Vector3) int32 { sizes := ml.Joints.ShapeSizes() idx := int32(sizes[0]) - ml.Params[0].JointsN = idx + 1 + params := &ml.Params[0] + params.JointsN = idx + 1 ml.Joints.SetShapeSizes(int(idx+1), int(JointVarsN)) ml.JointDefaults(idx) SetJointType(idx, joint) @@ -528,6 +487,13 @@ func (ml *Model) newJoint(joint JointTypes, parent, child int32, ppos, cpos math SetJointChild(idx, child) SetJointPPos(idx, ppos) SetJointCPos(idx, cpos) + if ml.CurrentObjectJoint >= int(params.MaxObjectJoints)-1 { + params.MaxObjectJoints = int32(ml.CurrentObjectJoint+1) + ml.Objects.SetShapeSizes(ml.CurrentObject+1, int(params.MaxObjectJoints+1)) + } + ml.Objects[ml.CurrentObject, 1 + ml.CurrentObjectJoint] = idx + ml.CurrentObjectJoint++ + ml.Objects[ml.CurrentObject, 0] = int32(ml.CurrentObjectJoint) return idx } diff --git a/physics/model.go b/physics/model.go index aef4ff66..cfa1fff0 100644 --- a/physics/model.go +++ b/physics/model.go @@ -26,6 +26,14 @@ type Model struct { // while 0 and positive numbers only interact amongst themselves. CurrentWorld int + // CurrentObject is the Object to use when creating new joints. + // Call NewObject to increment. + CurrentObject int + + // CurrentObjectJoint is the Joint index in CurrentObject + // to use when creating new joints. + CurrentObjectJoint int + // ReplicasStart is the starting body index for replicated world bodies, // which is needed for viewers to efficiently select a specific world to view. // This is the start of the World=0 first instance. @@ -42,6 +50,17 @@ type Model struct { // [body][BodyVarsN] Bodies *tensor.Float32 `display:"no-inline"` + // Objects is a list of joint indexes for each object, where each object + // contains all the joints interconnecting an overlapping set of bodies. + // Joints must be added in parent -> child order within objects, as joints + // are updated in sequential order within object. + // [object][maxjointsperobj] + Objects *tensor.Int32 `display:"no-inline"` + + // BodyJoints is a list of joint indexes for each dynamic body, for aggregating. + // [dyn body][parent, child][Params.BodyJointsMax] + BodyJoints *tensor.Int32 `display:"no-inline"` + // Joints is a list of permanent joints connecting bodies, // which do not change (no dynamic variables). // [joint][JointVarsN] @@ -51,10 +70,6 @@ type Model struct { // [dof][JointDoFVars] JointDoFs *tensor.Float32 `display:"no-inline"` - // BodyJoints is a list of joint indexes for each dynamic body, for aggregating. - // [dyn body][parent, child][Params.BodyJointsMax] - BodyJoints *tensor.Int32 `display:"no-inline"` - // BodyCollidePairs are pairs of Body indexes that could potentially collide // based on precomputed collision logic, using World, Group, and Joint indexes. // [BodyCollidePairsN][2] @@ -107,7 +122,12 @@ func (ml *Model) Init() { // Reset resets all data to empty: starting over. func (ml *Model) Reset() { + ml.CurrentWorld = 0 + ml.CurrentObject = 0 + ml.CurrentObjectJoint = 0 + ml.Params[0].Reset() ml.Bodies = tensor.NewFloat32(0, int(BodyVarsN)) + ml.Objects = tensor.NewInt32(0, 1) ml.Joints = tensor.NewFloat32(0, int(JointVarsN)) ml.JointDoFs = tensor.NewFloat32(0, int(JointDoFVarsN)) ml.BodyJoints = tensor.NewInt32(0, 2, 2) @@ -121,6 +141,18 @@ func (ml *Model) Reset() { ml.SetAsCurrentVars() } +// NewObject adds a new object. Returns the CurrentObject. +func (ml *Model) NewObject() int32 { + params := &ml.Params[0] + sizes := ml.Objects.ShapeSizes() + idx := int32(sizes[0]) + ml.Objects.SetShapeSizes(int(idx+1), int(params.MaxObjectJoints+1)) + params.ObjectsN = idx + 1 + ml.CurrentObject = int(idx) + ml.CurrentObjectJoint = 0 + return idx +} + // NewBody adds a new body with given parameters. Returns the index. // Use this for Static elements; NewDynamic for dynamic elements. func (ml *Model) NewBody(shape Shapes, hsize, pos math32.Vector3, rot math32.Quat) int32 { @@ -175,6 +207,7 @@ func (ml *Model) SetAsCurrent() { func (ml *Model) SetAsCurrentVars() { Params = ml.Params Bodies = ml.Bodies + Objects = ml.Objects Joints = ml.Joints JointDoFs = ml.JointDoFs BodyJoints = ml.BodyJoints @@ -199,7 +232,7 @@ func (ml *Model) GPUInit() { // the GPU. This is done in GPUInit, and if current switched. func (ml *Model) ToGPUInfra() { ToGPUTensorStrides() - ToGPU(ParamsVar, BodiesVar, JointsVar, JointDoFsVar, BodyJointsVar, BodyCollidePairsVar, DynamicsVar, BroadContactsNVar, BroadContactsVar, ContactsNVar, ContactsVar, JointControlsVar) + ToGPU(ParamsVar, BodiesVar, ObjectsVar, JointsVar, JointDoFsVar, BodyJointsVar, BodyCollidePairsVar, DynamicsVar, BroadContactsNVar, BroadContactsVar, ContactsNVar, ContactsVar, JointControlsVar) } // ReplicaWorldsN returns the number of replicated worlds. 0 if none. diff --git a/physics/model.goal b/physics/model.goal index 0c26a850..6a7f4348 100644 --- a/physics/model.goal +++ b/physics/model.goal @@ -24,6 +24,14 @@ type Model struct { // while 0 and positive numbers only interact amongst themselves. CurrentWorld int + // CurrentObject is the Object to use when creating new joints. + // Call NewObject to increment. + CurrentObject int + + // CurrentObjectJoint is the Joint index in CurrentObject + // to use when creating new joints. + CurrentObjectJoint int + // ReplicasStart is the starting body index for replicated world bodies, // which is needed for viewers to efficiently select a specific world to view. // This is the start of the World=0 first instance. @@ -40,6 +48,17 @@ type Model struct { // [body][BodyVarsN] Bodies *tensor.Float32 `display:"no-inline"` + // Objects is a list of joint indexes for each object, where each object + // contains all the joints interconnecting an overlapping set of bodies. + // Joints must be added in parent -> child order within objects, as joints + // are updated in sequential order within object. + // [object][maxjointsperobj] + Objects *tensor.Int32 `display:"no-inline"` + + // BodyJoints is a list of joint indexes for each dynamic body, for aggregating. + // [dyn body][parent, child][Params.BodyJointsMax] + BodyJoints *tensor.Int32 `display:"no-inline"` + // Joints is a list of permanent joints connecting bodies, // which do not change (no dynamic variables). // [joint][JointVarsN] @@ -49,10 +68,6 @@ type Model struct { // [dof][JointDoFVars] JointDoFs *tensor.Float32 `display:"no-inline"` - // BodyJoints is a list of joint indexes for each dynamic body, for aggregating. - // [dyn body][parent, child][Params.BodyJointsMax] - BodyJoints *tensor.Int32 `display:"no-inline"` - // BodyCollidePairs are pairs of Body indexes that could potentially collide // based on precomputed collision logic, using World, Group, and Joint indexes. // [BodyCollidePairsN][2] @@ -105,7 +120,12 @@ func (ml *Model) Init() { // Reset resets all data to empty: starting over. func (ml *Model) Reset() { + ml.CurrentWorld = 0 + ml.CurrentObject = 0 + ml.CurrentObjectJoint = 0 + ml.Params[0].Reset() ml.Bodies = tensor.NewFloat32(0, int(BodyVarsN)) + ml.Objects = tensor.NewInt32(0, 1) ml.Joints = tensor.NewFloat32(0, int(JointVarsN)) ml.JointDoFs = tensor.NewFloat32(0, int(JointDoFVarsN)) ml.BodyJoints = tensor.NewInt32(0, 2, 2) @@ -119,6 +139,18 @@ func (ml *Model) Reset() { ml.SetAsCurrentVars() } +// NewObject adds a new object. Returns the CurrentObject. +func (ml *Model) NewObject() int32 { + params := &ml.Params[0] + sizes := ml.Objects.ShapeSizes() + idx := int32(sizes[0]) + ml.Objects.SetShapeSizes(int(idx+1), int(params.MaxObjectJoints+1)) + params.ObjectsN = idx + 1 + ml.CurrentObject = int(idx) + ml.CurrentObjectJoint = 0 + return idx +} + // NewBody adds a new body with given parameters. Returns the index. // Use this for Static elements; NewDynamic for dynamic elements. func (ml *Model) NewBody(shape Shapes, hsize, pos math32.Vector3, rot math32.Quat) int32 { @@ -173,6 +205,7 @@ func (ml *Model) SetAsCurrent() { func (ml *Model) SetAsCurrentVars() { Params = ml.Params Bodies = ml.Bodies + Objects = ml.Objects Joints = ml.Joints JointDoFs = ml.JointDoFs BodyJoints = ml.BodyJoints @@ -197,7 +230,7 @@ func (ml *Model) GPUInit() { // the GPU. This is done in GPUInit, and if current switched. func (ml *Model) ToGPUInfra() { ToGPUTensorStrides() - ToGPU(ParamsVar, BodiesVar, JointsVar, JointDoFsVar, BodyJointsVar, BodyCollidePairsVar, DynamicsVar, BroadContactsNVar, BroadContactsVar, ContactsNVar, ContactsVar, JointControlsVar) + ToGPU(ParamsVar, BodiesVar, ObjectsVar, JointsVar, JointDoFsVar, BodyJointsVar, BodyCollidePairsVar, DynamicsVar, BroadContactsNVar, BroadContactsVar, ContactsNVar, ContactsVar, JointControlsVar) } // ReplicaWorldsN returns the number of replicated worlds. 0 if none. diff --git a/physics/params.go b/physics/params.go index 8c5f81e7..fc722069 100644 --- a/physics/params.go +++ b/physics/params.go @@ -84,6 +84,12 @@ type PhysParams struct { // DynamicsN is number of dynamics bodies. DynamicsN int32 `edit:"-"` + // ObjectsN is number of objects. + ObjectsN int32 `edit:"-"` + + // MaxObjectJoints is max number of joints per object. + MaxObjectJoints int32 `edit:"-"` + // JointsN is number of joints. JointsN int32 `edit:"-"` @@ -97,6 +103,8 @@ type PhysParams struct { // to examine. BodyCollidePairsN int32 `edit:"-"` + pad, pad1 int32 + // Gravity is the gravity acceleration function Gravity slvec.Vector3 } @@ -123,6 +131,18 @@ func (pr *PhysParams) Defaults() { pr.MaxGeomIter = 10 } +// Reset resets the N's +func (pr *PhysParams) Reset() { + pr.BodiesN = 0 + pr.DynamicsN = 0 + pr.ObjectsN = 0 + pr.MaxObjectJoints = 0 + pr.JointsN = 0 + pr.JointDoFsN = 0 + pr.BodyJointsMax = 0 + pr.BodyCollidePairsN = 0 +} + // StepsToMsec returns the given number of individual Step calls // converted into milliseconds, suitable for driving controls. func (pr *PhysParams) StepsToMsec(steps int) int { diff --git a/physics/shaders/CollisionBroad.wgsl b/physics/shaders/CollisionBroad.wgsl index 64aa3143..e85bd1ba 100644 --- a/physics/shaders/CollisionBroad.wgsl +++ b/physics/shaders/CollisionBroad.wgsl @@ -9,7 +9,7 @@ var Params: array; // // Bodies are the rigid body elements (dynamic and static), // specifying the constant, non-dynamic properties, // which is initial state for dynamics. // [body][BodyVarsN] @group(1) @binding(0) var Bodies: array; -@group(1) @binding(4) +@group(1) @binding(5) var BodyCollidePairs: array; // // Dynamics are the dynamic rigid body elements: these actually move. // Two alternating states are used: Params.Cur and Params.Next. // [dyn body][cur/next][DynamicVarsN] @group(2) @binding(0) @@ -154,13 +154,13 @@ const ContactBAngDeltaY: ContactVars = 31; const ContactBAngDeltaZ: ContactVars = 32; const BroadContactVarsN = ContactAPointX; fn SetBroadContactA(idx: i32,bodIdx: i32) { - BroadContacts[Index2D(TensorStrides[70], TensorStrides[71], u32(idx), u32(ContactA))] = bitcast(u32(bodIdx)); + BroadContacts[Index2D(TensorStrides[80], TensorStrides[81], u32(idx), u32(ContactA))] = bitcast(u32(bodIdx)); } fn SetBroadContactB(idx: i32,bodIdx: i32) { - BroadContacts[Index2D(TensorStrides[70], TensorStrides[71], u32(idx), u32(ContactB))] = bitcast(u32(bodIdx)); + BroadContacts[Index2D(TensorStrides[80], TensorStrides[81], u32(idx), u32(ContactB))] = bitcast(u32(bodIdx)); } fn SetBroadContactPointIdx(idx: i32,ptIdx: i32) { - BroadContacts[Index2D(TensorStrides[70], TensorStrides[71], u32(idx), u32(ContactPointIdx))] = bitcast(u32(ptIdx)); + BroadContacts[Index2D(TensorStrides[80], TensorStrides[81], u32(idx), u32(ContactPointIdx))] = bitcast(u32(ptIdx)); } fn CollisionBroad(i: u32) { //gosl:kernel var params = Params[0]; @@ -168,8 +168,8 @@ fn CollisionBroad(i: u32) { //gosl:kernel if (ci >= params.BodyCollidePairsN) { return; } - var biA = BodyCollidePairs[Index2D(TensorStrides[40], TensorStrides[41], u32(ci), u32(0))]; - var biB = BodyCollidePairs[Index2D(TensorStrides[40], TensorStrides[41], u32(ci), u32(1))]; + var biA = BodyCollidePairs[Index2D(TensorStrides[50], TensorStrides[51], u32(ci), u32(0))]; + var biB = BodyCollidePairs[Index2D(TensorStrides[50], TensorStrides[51], u32(ci), u32(1))]; var xwAR = BodyDynamicPos(biA, params.Cur); var xwAQ = BodyDynamicQuat(biA, params.Cur); var xwBR = BodyDynamicPos(biB, params.Cur); @@ -265,10 +265,10 @@ const DynAngDeltaY: DynamicVars = 30; const DynAngDeltaZ: DynamicVars = 31; const DynContactWeight: DynamicVars = 32; fn DynamicPos(idx: i32,cni: i32) -> vec3 { - return vec3(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosZ))]); + return vec3(Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynPosX))], Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynPosY))], Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynPosZ))]); } fn DynamicQuat(idx: i32,cni: i32) -> vec4 { - return vec4(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatZ))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatW))]); + return vec4(Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynQuatX))], Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynQuatY))], Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynQuatZ))], Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynQuatW))]); } //////// import: "enumgen.go" @@ -276,9 +276,9 @@ const BodyVarsN: BodyVars = 43; const ContactVarsN: ContactVars = 33; const JointControlVarsN: JointControlVars = 5; const DynamicVarsN: DynamicVars = 33; -const GPUVarsN: GPUVars = 12; +const GPUVarsN: GPUVars = 13; const JointTypesN: JointTypes = 8; -const JointVarsN: JointVars = 50; +const JointVarsN: JointVars = 39; const JointDoFVarsN: JointDoFVars = 5; const ShapesN: Shapes = 6; @@ -296,54 +296,43 @@ const PlaneXZ: JointTypes = 7; alias JointVars = i32; //enums:enum const JointType: JointVars = 0; const JointEnabled: JointVars = 1; -const JointParent: JointVars = 2; -const JointChild: JointVars = 3; -const JointPPosX: JointVars = 4; -const JointPPosY: JointVars = 5; -const JointPPosZ: JointVars = 6; -const JointPQuatX: JointVars = 7; -const JointPQuatY: JointVars = 8; -const JointPQuatZ: JointVars = 9; -const JointPQuatW: JointVars = 10; -const JointCPosX: JointVars = 11; -const JointCPosY: JointVars = 12; -const JointCPosZ: JointVars = 13; -const JointCQuatX: JointVars = 14; -const JointCQuatY: JointVars = 15; -const JointCQuatZ: JointVars = 16; -const JointCQuatW: JointVars = 17; -const JointLinearDoFN: JointVars = 18; -const JointAngularDoFN: JointVars = 19; -const JointDoF1: JointVars = 20; -const JointDoF2: JointVars = 21; -const JointDoF3: JointVars = 22; -const JointDoF4: JointVars = 23; -const JointDoF5: JointVars = 24; -const JointDoF6: JointVars = 25; -const JointPForceX: JointVars = 26; -const JointPForceY: JointVars = 27; -const JointPForceZ: JointVars = 28; -const JointPTorqueX: JointVars = 29; -const JointPTorqueY: JointVars = 30; -const JointPTorqueZ: JointVars = 31; -const JointCForceX: JointVars = 32; -const JointCForceY: JointVars = 33; -const JointCForceZ: JointVars = 34; -const JointCTorqueX: JointVars = 35; -const JointCTorqueY: JointVars = 36; -const JointCTorqueZ: JointVars = 37; -const JointPDeltaX: JointVars = 38; -const JointPDeltaY: JointVars = 39; -const JointPDeltaZ: JointVars = 40; -const JointPAngDeltaX: JointVars = 41; -const JointPAngDeltaY: JointVars = 42; -const JointPAngDeltaZ: JointVars = 43; -const JointCDeltaX: JointVars = 44; -const JointCDeltaY: JointVars = 45; -const JointCDeltaZ: JointVars = 46; -const JointCAngDeltaX: JointVars = 47; -const JointCAngDeltaY: JointVars = 48; -const JointCAngDeltaZ: JointVars = 49; +const JointParentFixed: JointVars = 2; +const JointParent: JointVars = 3; +const JointChild: JointVars = 4; +const JointPPosX: JointVars = 5; +const JointPPosY: JointVars = 6; +const JointPPosZ: JointVars = 7; +const JointPQuatX: JointVars = 8; +const JointPQuatY: JointVars = 9; +const JointPQuatZ: JointVars = 10; +const JointPQuatW: JointVars = 11; +const JointCPosX: JointVars = 12; +const JointCPosY: JointVars = 13; +const JointCPosZ: JointVars = 14; +const JointCQuatX: JointVars = 15; +const JointCQuatY: JointVars = 16; +const JointCQuatZ: JointVars = 17; +const JointCQuatW: JointVars = 18; +const JointLinearDoFN: JointVars = 19; +const JointAngularDoFN: JointVars = 20; +const JointDoF1: JointVars = 21; +const JointDoF2: JointVars = 22; +const JointDoF3: JointVars = 23; +const JointDoF4: JointVars = 24; +const JointDoF5: JointVars = 25; +const JointDoF6: JointVars = 26; +const JointPForceX: JointVars = 27; +const JointPForceY: JointVars = 28; +const JointPForceZ: JointVars = 29; +const JointPTorqueX: JointVars = 30; +const JointPTorqueY: JointVars = 31; +const JointPTorqueZ: JointVars = 32; +const JointCForceX: JointVars = 33; +const JointCForceY: JointVars = 34; +const JointCForceZ: JointVars = 35; +const JointCTorqueX: JointVars = 36; +const JointCTorqueY: JointVars = 37; +const JointCTorqueZ: JointVars = 38; alias JointDoFVars = i32; //enums:enum const JointAxisX: JointDoFVars = 0; const JointAxisY: JointDoFVars = 1; @@ -373,10 +362,14 @@ struct PhysParams { Next: i32, BodiesN: i32, DynamicsN: i32, + ObjectsN: i32, + MaxObjectJoints: i32, JointsN: i32, JointDoFsN: i32, BodyJointsMax: i32, BodyCollidePairsN: i32, + pad: i32, + pad1: i32, Gravity: vec4, } diff --git a/physics/shaders/CollisionNarrow.wgsl b/physics/shaders/CollisionNarrow.wgsl index de927542..1015c525 100644 --- a/physics/shaders/CollisionNarrow.wgsl +++ b/physics/shaders/CollisionNarrow.wgsl @@ -156,48 +156,48 @@ const ContactBAngDeltaY: ContactVars = 31; const ContactBAngDeltaZ: ContactVars = 32; const BroadContactVarsN = ContactAPointX; fn GetBroadContactA(idx: i32) -> i32 { - return i32(bitcast(BroadContacts[Index2D(TensorStrides[70], TensorStrides[71], u32(idx), u32(ContactA))])); + return i32(bitcast(BroadContacts[Index2D(TensorStrides[80], TensorStrides[81], u32(idx), u32(ContactA))])); } fn GetBroadContactB(idx: i32) -> i32 { - return i32(bitcast(BroadContacts[Index2D(TensorStrides[70], TensorStrides[71], u32(idx), u32(ContactB))])); + return i32(bitcast(BroadContacts[Index2D(TensorStrides[80], TensorStrides[81], u32(idx), u32(ContactB))])); } fn GetBroadContactPointIdx(idx: i32) -> i32 { - return i32(bitcast(BroadContacts[Index2D(TensorStrides[70], TensorStrides[71], + return i32(bitcast(BroadContacts[Index2D(TensorStrides[80], TensorStrides[81], u32(idx), u32(ContactPointIdx))])); } fn SetContactA(idx: i32,bodIdx: i32) { - Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactA))] = bitcast(u32(bodIdx)); + Contacts[Index2D(TensorStrides[100], TensorStrides[101], u32(idx), u32(ContactA))] = bitcast(u32(bodIdx)); } fn SetContactB(idx: i32,bodIdx: i32) { - Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactB))] = bitcast(u32(bodIdx)); + Contacts[Index2D(TensorStrides[100], TensorStrides[101], u32(idx), u32(ContactB))] = bitcast(u32(bodIdx)); } fn SetContactPointIdx(idx: i32,ptIdx: i32) { - Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactPointIdx))] = bitcast(u32(ptIdx)); + Contacts[Index2D(TensorStrides[100], TensorStrides[101], u32(idx), u32(ContactPointIdx))] = bitcast(u32(ptIdx)); } fn SetContactAPoint(idx: i32, pos: vec3) { - Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactAPointX))] = pos.x; - Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactAPointY))] = pos.y; - Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactAPointZ))] = pos.z; + Contacts[Index2D(TensorStrides[100], TensorStrides[101], u32(idx), u32(ContactAPointX))] = pos.x; + Contacts[Index2D(TensorStrides[100], TensorStrides[101], u32(idx), u32(ContactAPointY))] = pos.y; + Contacts[Index2D(TensorStrides[100], TensorStrides[101], u32(idx), u32(ContactAPointZ))] = pos.z; } fn SetContactBPoint(idx: i32, pos: vec3) { - Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactBPointX))] = pos.x; - Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactBPointY))] = pos.y; - Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactBPointZ))] = pos.z; + Contacts[Index2D(TensorStrides[100], TensorStrides[101], u32(idx), u32(ContactBPointX))] = pos.x; + Contacts[Index2D(TensorStrides[100], TensorStrides[101], u32(idx), u32(ContactBPointY))] = pos.y; + Contacts[Index2D(TensorStrides[100], TensorStrides[101], u32(idx), u32(ContactBPointZ))] = pos.z; } fn SetContactAOff(idx: i32, pos: vec3) { - Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactAOffX))] = pos.x; - Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactAOffY))] = pos.y; - Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactAOffZ))] = pos.z; + Contacts[Index2D(TensorStrides[100], TensorStrides[101], u32(idx), u32(ContactAOffX))] = pos.x; + Contacts[Index2D(TensorStrides[100], TensorStrides[101], u32(idx), u32(ContactAOffY))] = pos.y; + Contacts[Index2D(TensorStrides[100], TensorStrides[101], u32(idx), u32(ContactAOffZ))] = pos.z; } fn SetContactBOff(idx: i32, pos: vec3) { - Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactBOffX))] = pos.x; - Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactBOffY))] = pos.y; - Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactBOffZ))] = pos.z; + Contacts[Index2D(TensorStrides[100], TensorStrides[101], u32(idx), u32(ContactBOffX))] = pos.x; + Contacts[Index2D(TensorStrides[100], TensorStrides[101], u32(idx), u32(ContactBOffY))] = pos.y; + Contacts[Index2D(TensorStrides[100], TensorStrides[101], u32(idx), u32(ContactBOffZ))] = pos.z; } fn SetContactNorm(idx: i32, pos: vec3) { - Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactNormX))] = pos.x; - Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactNormY))] = pos.y; - Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactNormZ))] = pos.z; + Contacts[Index2D(TensorStrides[100], TensorStrides[101], u32(idx), u32(ContactNormX))] = pos.x; + Contacts[Index2D(TensorStrides[100], TensorStrides[101], u32(idx), u32(ContactNormY))] = pos.y; + Contacts[Index2D(TensorStrides[100], TensorStrides[101], u32(idx), u32(ContactNormZ))] = pos.z; } fn CollisionNarrow(i: u32) { //gosl:kernel var params = Params[0]; @@ -305,8 +305,8 @@ fn CollisionNarrow(i: u32) { //gosl:kernel SetContactAOff(nci, offA); SetContactBOff(nci, offB); SetContactNorm(nci, norm); - Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(nci), u32(ContactAThick))] = offMagA; - Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(nci), u32(ContactBThick))] = offMagB; + Contacts[Index2D(TensorStrides[100], TensorStrides[101], u32(nci), u32(ContactAThick))] = offMagA; + Contacts[Index2D(TensorStrides[100], TensorStrides[101], u32(nci), u32(ContactBThick))] = offMagB; Params[0] = params; } @@ -354,10 +354,10 @@ const DynAngDeltaY: DynamicVars = 30; const DynAngDeltaZ: DynamicVars = 31; const DynContactWeight: DynamicVars = 32; fn DynamicPos(idx: i32,cni: i32) -> vec3 { - return vec3(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosZ))]); + return vec3(Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynPosX))], Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynPosY))], Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynPosZ))]); } fn DynamicQuat(idx: i32,cni: i32) -> vec4 { - return vec4(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatZ))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatW))]); + return vec4(Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynQuatX))], Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynQuatY))], Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynQuatZ))], Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynQuatW))]); } //////// import: "enumgen.go" @@ -365,9 +365,9 @@ const BodyVarsN: BodyVars = 43; const ContactVarsN: ContactVars = 33; const JointControlVarsN: JointControlVars = 5; const DynamicVarsN: DynamicVars = 33; -const GPUVarsN: GPUVars = 12; +const GPUVarsN: GPUVars = 13; const JointTypesN: JointTypes = 8; -const JointVarsN: JointVars = 50; +const JointVarsN: JointVars = 39; const JointDoFVarsN: JointDoFVars = 5; const ShapesN: Shapes = 6; @@ -385,54 +385,43 @@ const PlaneXZ: JointTypes = 7; alias JointVars = i32; //enums:enum const JointType: JointVars = 0; const JointEnabled: JointVars = 1; -const JointParent: JointVars = 2; -const JointChild: JointVars = 3; -const JointPPosX: JointVars = 4; -const JointPPosY: JointVars = 5; -const JointPPosZ: JointVars = 6; -const JointPQuatX: JointVars = 7; -const JointPQuatY: JointVars = 8; -const JointPQuatZ: JointVars = 9; -const JointPQuatW: JointVars = 10; -const JointCPosX: JointVars = 11; -const JointCPosY: JointVars = 12; -const JointCPosZ: JointVars = 13; -const JointCQuatX: JointVars = 14; -const JointCQuatY: JointVars = 15; -const JointCQuatZ: JointVars = 16; -const JointCQuatW: JointVars = 17; -const JointLinearDoFN: JointVars = 18; -const JointAngularDoFN: JointVars = 19; -const JointDoF1: JointVars = 20; -const JointDoF2: JointVars = 21; -const JointDoF3: JointVars = 22; -const JointDoF4: JointVars = 23; -const JointDoF5: JointVars = 24; -const JointDoF6: JointVars = 25; -const JointPForceX: JointVars = 26; -const JointPForceY: JointVars = 27; -const JointPForceZ: JointVars = 28; -const JointPTorqueX: JointVars = 29; -const JointPTorqueY: JointVars = 30; -const JointPTorqueZ: JointVars = 31; -const JointCForceX: JointVars = 32; -const JointCForceY: JointVars = 33; -const JointCForceZ: JointVars = 34; -const JointCTorqueX: JointVars = 35; -const JointCTorqueY: JointVars = 36; -const JointCTorqueZ: JointVars = 37; -const JointPDeltaX: JointVars = 38; -const JointPDeltaY: JointVars = 39; -const JointPDeltaZ: JointVars = 40; -const JointPAngDeltaX: JointVars = 41; -const JointPAngDeltaY: JointVars = 42; -const JointPAngDeltaZ: JointVars = 43; -const JointCDeltaX: JointVars = 44; -const JointCDeltaY: JointVars = 45; -const JointCDeltaZ: JointVars = 46; -const JointCAngDeltaX: JointVars = 47; -const JointCAngDeltaY: JointVars = 48; -const JointCAngDeltaZ: JointVars = 49; +const JointParentFixed: JointVars = 2; +const JointParent: JointVars = 3; +const JointChild: JointVars = 4; +const JointPPosX: JointVars = 5; +const JointPPosY: JointVars = 6; +const JointPPosZ: JointVars = 7; +const JointPQuatX: JointVars = 8; +const JointPQuatY: JointVars = 9; +const JointPQuatZ: JointVars = 10; +const JointPQuatW: JointVars = 11; +const JointCPosX: JointVars = 12; +const JointCPosY: JointVars = 13; +const JointCPosZ: JointVars = 14; +const JointCQuatX: JointVars = 15; +const JointCQuatY: JointVars = 16; +const JointCQuatZ: JointVars = 17; +const JointCQuatW: JointVars = 18; +const JointLinearDoFN: JointVars = 19; +const JointAngularDoFN: JointVars = 20; +const JointDoF1: JointVars = 21; +const JointDoF2: JointVars = 22; +const JointDoF3: JointVars = 23; +const JointDoF4: JointVars = 24; +const JointDoF5: JointVars = 25; +const JointDoF6: JointVars = 26; +const JointPForceX: JointVars = 27; +const JointPForceY: JointVars = 28; +const JointPForceZ: JointVars = 29; +const JointPTorqueX: JointVars = 30; +const JointPTorqueY: JointVars = 31; +const JointPTorqueZ: JointVars = 32; +const JointCForceX: JointVars = 33; +const JointCForceY: JointVars = 34; +const JointCForceZ: JointVars = 35; +const JointCTorqueX: JointVars = 36; +const JointCTorqueY: JointVars = 37; +const JointCTorqueZ: JointVars = 38; alias JointDoFVars = i32; //enums:enum const JointAxisX: JointDoFVars = 0; const JointAxisY: JointDoFVars = 1; @@ -462,10 +451,14 @@ struct PhysParams { Next: i32, BodiesN: i32, DynamicsN: i32, + ObjectsN: i32, + MaxObjectJoints: i32, JointsN: i32, JointDoFsN: i32, BodyJointsMax: i32, BodyCollidePairsN: i32, + pad: i32, + pad1: i32, Gravity: vec4, } diff --git a/physics/shaders/DynamicsCurToNext.wgsl b/physics/shaders/DynamicsCurToNext.wgsl index de2766fa..f2371c1c 100644 --- a/physics/shaders/DynamicsCurToNext.wgsl +++ b/physics/shaders/DynamicsCurToNext.wgsl @@ -159,9 +159,9 @@ const BodyVarsN: BodyVars = 43; const ContactVarsN: ContactVars = 33; const JointControlVarsN: JointControlVars = 5; const DynamicVarsN: DynamicVars = 33; -const GPUVarsN: GPUVars = 12; +const GPUVarsN: GPUVars = 13; const JointTypesN: JointTypes = 8; -const JointVarsN: JointVars = 50; +const JointVarsN: JointVars = 39; const JointDoFVarsN: JointDoFVars = 5; const ShapesN: Shapes = 6; @@ -179,54 +179,43 @@ const PlaneXZ: JointTypes = 7; alias JointVars = i32; //enums:enum const JointType: JointVars = 0; const JointEnabled: JointVars = 1; -const JointParent: JointVars = 2; -const JointChild: JointVars = 3; -const JointPPosX: JointVars = 4; -const JointPPosY: JointVars = 5; -const JointPPosZ: JointVars = 6; -const JointPQuatX: JointVars = 7; -const JointPQuatY: JointVars = 8; -const JointPQuatZ: JointVars = 9; -const JointPQuatW: JointVars = 10; -const JointCPosX: JointVars = 11; -const JointCPosY: JointVars = 12; -const JointCPosZ: JointVars = 13; -const JointCQuatX: JointVars = 14; -const JointCQuatY: JointVars = 15; -const JointCQuatZ: JointVars = 16; -const JointCQuatW: JointVars = 17; -const JointLinearDoFN: JointVars = 18; -const JointAngularDoFN: JointVars = 19; -const JointDoF1: JointVars = 20; -const JointDoF2: JointVars = 21; -const JointDoF3: JointVars = 22; -const JointDoF4: JointVars = 23; -const JointDoF5: JointVars = 24; -const JointDoF6: JointVars = 25; -const JointPForceX: JointVars = 26; -const JointPForceY: JointVars = 27; -const JointPForceZ: JointVars = 28; -const JointPTorqueX: JointVars = 29; -const JointPTorqueY: JointVars = 30; -const JointPTorqueZ: JointVars = 31; -const JointCForceX: JointVars = 32; -const JointCForceY: JointVars = 33; -const JointCForceZ: JointVars = 34; -const JointCTorqueX: JointVars = 35; -const JointCTorqueY: JointVars = 36; -const JointCTorqueZ: JointVars = 37; -const JointPDeltaX: JointVars = 38; -const JointPDeltaY: JointVars = 39; -const JointPDeltaZ: JointVars = 40; -const JointPAngDeltaX: JointVars = 41; -const JointPAngDeltaY: JointVars = 42; -const JointPAngDeltaZ: JointVars = 43; -const JointCDeltaX: JointVars = 44; -const JointCDeltaY: JointVars = 45; -const JointCDeltaZ: JointVars = 46; -const JointCAngDeltaX: JointVars = 47; -const JointCAngDeltaY: JointVars = 48; -const JointCAngDeltaZ: JointVars = 49; +const JointParentFixed: JointVars = 2; +const JointParent: JointVars = 3; +const JointChild: JointVars = 4; +const JointPPosX: JointVars = 5; +const JointPPosY: JointVars = 6; +const JointPPosZ: JointVars = 7; +const JointPQuatX: JointVars = 8; +const JointPQuatY: JointVars = 9; +const JointPQuatZ: JointVars = 10; +const JointPQuatW: JointVars = 11; +const JointCPosX: JointVars = 12; +const JointCPosY: JointVars = 13; +const JointCPosZ: JointVars = 14; +const JointCQuatX: JointVars = 15; +const JointCQuatY: JointVars = 16; +const JointCQuatZ: JointVars = 17; +const JointCQuatW: JointVars = 18; +const JointLinearDoFN: JointVars = 19; +const JointAngularDoFN: JointVars = 20; +const JointDoF1: JointVars = 21; +const JointDoF2: JointVars = 22; +const JointDoF3: JointVars = 23; +const JointDoF4: JointVars = 24; +const JointDoF5: JointVars = 25; +const JointDoF6: JointVars = 26; +const JointPForceX: JointVars = 27; +const JointPForceY: JointVars = 28; +const JointPForceZ: JointVars = 29; +const JointPTorqueX: JointVars = 30; +const JointPTorqueY: JointVars = 31; +const JointPTorqueZ: JointVars = 32; +const JointCForceX: JointVars = 33; +const JointCForceY: JointVars = 34; +const JointCForceZ: JointVars = 35; +const JointCTorqueX: JointVars = 36; +const JointCTorqueY: JointVars = 37; +const JointCTorqueZ: JointVars = 38; alias JointDoFVars = i32; //enums:enum const JointAxisX: JointDoFVars = 0; const JointAxisY: JointDoFVars = 1; @@ -256,10 +245,14 @@ struct PhysParams { Next: i32, BodiesN: i32, DynamicsN: i32, + ObjectsN: i32, + MaxObjectJoints: i32, JointsN: i32, JointDoFsN: i32, BodyJointsMax: i32, BodyCollidePairsN: i32, + pad: i32, + pad1: i32, Gravity: vec4, } @@ -307,7 +300,7 @@ fn DynamicsCurToNext(i: u32) { //gosl:kernel } for (var di = DynBody; di < DynamicVarsN; di++) { - Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(ii), u32(params.Next), u32(di))] = Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(ii), u32(params.Cur), u32(di))]; + Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(ii), u32(params.Next), u32(di))] = Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(ii), u32(params.Cur), u32(di))]; } Params[0] = params; } diff --git a/physics/shaders/ForcesFromJoints.wgsl b/physics/shaders/ForcesFromJoints.wgsl index b497aee0..d51ca38f 100644 --- a/physics/shaders/ForcesFromJoints.wgsl +++ b/physics/shaders/ForcesFromJoints.wgsl @@ -7,10 +7,10 @@ var TensorStrides: array; @group(0) @binding(1) var Params: array; // // Bodies are the rigid body elements (dynamic and static), // specifying the constant, non-dynamic properties, // which is initial state for dynamics. // [body][BodyVarsN] -@group(1) @binding(1) -var Joints: array; -@group(1) @binding(3) +@group(1) @binding(2) var BodyJoints: array; +@group(1) @binding(3) +var Joints: array; // // Dynamics are the dynamic rigid body elements: these actually move. // Two alternating states are used: Params.Cur and Params.Next. // [dyn body][cur/next][DynamicVarsN] @group(2) @binding(0) var Dynamics: array; @@ -24,14 +24,14 @@ fn main(@builtin(workgroup_id) wgid: vec3, @builtin(num_workgroups) nwg: ve ForcesFromJoints(idx); } -fn Index2D(s0: u32, s1: u32, i0: u32, i1: u32) -> u32 { - return s0 * i0 + s1 * i1; -} - fn Index3D(s0: u32, s1: u32, s2: u32, i0: u32, i1: u32, i2: u32) -> u32 { return s0 * i0 + s1 * i1 + s2 * i2; } +fn Index2D(s0: u32, s1: u32, i0: u32, i1: u32) -> u32 { + return s0 * i0 + s1 * i1; +} + //////// import: "vars.go" @@ -162,14 +162,14 @@ const DynAngDeltaY: DynamicVars = 30; const DynAngDeltaZ: DynamicVars = 31; const DynContactWeight: DynamicVars = 32; fn SetDynamicForce(idx: i32,cni: i32, force: vec3) { - Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynForceX))] = force.x; - Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynForceY))] = force.y; - Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynForceZ))] = force.z; + Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynForceX))] = force.x; + Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynForceY))] = force.y; + Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynForceZ))] = force.z; } fn SetDynamicTorque(idx: i32,cni: i32, torque: vec3) { - Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynTorqueX))] = torque.x; - Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynTorqueY))] = torque.y; - Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynTorqueZ))] = torque.z; + Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynTorqueX))] = torque.x; + Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynTorqueY))] = torque.y; + Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynTorqueZ))] = torque.z; } //////// import: "enumgen.go" @@ -177,9 +177,9 @@ const BodyVarsN: BodyVars = 43; const ContactVarsN: ContactVars = 33; const JointControlVarsN: JointControlVars = 5; const DynamicVarsN: DynamicVars = 33; -const GPUVarsN: GPUVars = 12; +const GPUVarsN: GPUVars = 13; const JointTypesN: JointTypes = 8; -const JointVarsN: JointVars = 50; +const JointVarsN: JointVars = 39; const JointDoFVarsN: JointDoFVars = 5; const ShapesN: Shapes = 6; @@ -197,65 +197,54 @@ const PlaneXZ: JointTypes = 7; alias JointVars = i32; //enums:enum const JointType: JointVars = 0; const JointEnabled: JointVars = 1; -const JointParent: JointVars = 2; -const JointChild: JointVars = 3; -const JointPPosX: JointVars = 4; -const JointPPosY: JointVars = 5; -const JointPPosZ: JointVars = 6; -const JointPQuatX: JointVars = 7; -const JointPQuatY: JointVars = 8; -const JointPQuatZ: JointVars = 9; -const JointPQuatW: JointVars = 10; -const JointCPosX: JointVars = 11; -const JointCPosY: JointVars = 12; -const JointCPosZ: JointVars = 13; -const JointCQuatX: JointVars = 14; -const JointCQuatY: JointVars = 15; -const JointCQuatZ: JointVars = 16; -const JointCQuatW: JointVars = 17; -const JointLinearDoFN: JointVars = 18; -const JointAngularDoFN: JointVars = 19; -const JointDoF1: JointVars = 20; -const JointDoF2: JointVars = 21; -const JointDoF3: JointVars = 22; -const JointDoF4: JointVars = 23; -const JointDoF5: JointVars = 24; -const JointDoF6: JointVars = 25; -const JointPForceX: JointVars = 26; -const JointPForceY: JointVars = 27; -const JointPForceZ: JointVars = 28; -const JointPTorqueX: JointVars = 29; -const JointPTorqueY: JointVars = 30; -const JointPTorqueZ: JointVars = 31; -const JointCForceX: JointVars = 32; -const JointCForceY: JointVars = 33; -const JointCForceZ: JointVars = 34; -const JointCTorqueX: JointVars = 35; -const JointCTorqueY: JointVars = 36; -const JointCTorqueZ: JointVars = 37; -const JointPDeltaX: JointVars = 38; -const JointPDeltaY: JointVars = 39; -const JointPDeltaZ: JointVars = 40; -const JointPAngDeltaX: JointVars = 41; -const JointPAngDeltaY: JointVars = 42; -const JointPAngDeltaZ: JointVars = 43; -const JointCDeltaX: JointVars = 44; -const JointCDeltaY: JointVars = 45; -const JointCDeltaZ: JointVars = 46; -const JointCAngDeltaX: JointVars = 47; -const JointCAngDeltaY: JointVars = 48; -const JointCAngDeltaZ: JointVars = 49; +const JointParentFixed: JointVars = 2; +const JointParent: JointVars = 3; +const JointChild: JointVars = 4; +const JointPPosX: JointVars = 5; +const JointPPosY: JointVars = 6; +const JointPPosZ: JointVars = 7; +const JointPQuatX: JointVars = 8; +const JointPQuatY: JointVars = 9; +const JointPQuatZ: JointVars = 10; +const JointPQuatW: JointVars = 11; +const JointCPosX: JointVars = 12; +const JointCPosY: JointVars = 13; +const JointCPosZ: JointVars = 14; +const JointCQuatX: JointVars = 15; +const JointCQuatY: JointVars = 16; +const JointCQuatZ: JointVars = 17; +const JointCQuatW: JointVars = 18; +const JointLinearDoFN: JointVars = 19; +const JointAngularDoFN: JointVars = 20; +const JointDoF1: JointVars = 21; +const JointDoF2: JointVars = 22; +const JointDoF3: JointVars = 23; +const JointDoF4: JointVars = 24; +const JointDoF5: JointVars = 25; +const JointDoF6: JointVars = 26; +const JointPForceX: JointVars = 27; +const JointPForceY: JointVars = 28; +const JointPForceZ: JointVars = 29; +const JointPTorqueX: JointVars = 30; +const JointPTorqueY: JointVars = 31; +const JointPTorqueZ: JointVars = 32; +const JointCForceX: JointVars = 33; +const JointCForceY: JointVars = 34; +const JointCForceZ: JointVars = 35; +const JointCTorqueX: JointVars = 36; +const JointCTorqueY: JointVars = 37; +const JointCTorqueZ: JointVars = 38; fn JointPForce(idx: i32) -> vec3 { - return vec3(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPForceX))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPForceY))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPForceZ))]); + return vec3(Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointPForceX))], Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointPForceY))], Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointPForceZ))]); } fn JointPTorque(idx: i32) -> vec3 { - return vec3(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPTorqueX))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPTorqueY))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPTorqueZ))]); + return vec3(Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointPTorqueX))], Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointPTorqueY))], Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointPTorqueZ))]); } fn JointCForce(idx: i32) -> vec3 { - return vec3(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCForceX))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCForceY))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCForceZ))]); + return vec3(Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointCForceX))], Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointCForceY))], Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointCForceZ))]); } fn JointCTorque(idx: i32) -> vec3 { - return vec3(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCTorqueX))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCTorqueY))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCTorqueZ))]); + return vec3(Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointCTorqueX))], Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointCTorqueY))], Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointCTorqueZ))]); } alias JointDoFVars = i32; //enums:enum const JointAxisX: JointDoFVars = 0; @@ -286,10 +275,14 @@ struct PhysParams { Next: i32, BodiesN: i32, DynamicsN: i32, + ObjectsN: i32, + MaxObjectJoints: i32, JointsN: i32, JointDoFsN: i32, BodyJointsMax: i32, BodyCollidePairsN: i32, + pad: i32, + pad1: i32, Gravity: vec4, } @@ -335,13 +328,13 @@ fn ForcesFromJoints(i: u32) { //gosl:kernel if (di >= params.DynamicsN) { return; } - var np = BodyJoints[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(di), u32(0), u32(0))]; - var nc = BodyJoints[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(di), u32(1), u32(0))]; + var np = BodyJoints[Index3D(TensorStrides[20], TensorStrides[21], TensorStrides[22], u32(di), u32(0), u32(0))]; + var nc = BodyJoints[Index3D(TensorStrides[20], TensorStrides[21], TensorStrides[22], u32(di), u32(1), u32(0))]; var tf = vec3(0, 0, 0); var tt = vec3(0, 0, 0); for (var i = i32(1); i <= np; i++) { - var ji = BodyJoints[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(di), u32(0), u32(i))]; + var ji = BodyJoints[Index3D(TensorStrides[20], TensorStrides[21], TensorStrides[22], u32(di), u32(0), u32(i))]; var f = JointPForce(ji); tf = tf+(f); var t = JointPTorque(ji); @@ -349,7 +342,7 @@ fn ForcesFromJoints(i: u32) { //gosl:kernel } for (var i = i32(1); i <= nc; i++) { - var ji = BodyJoints[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(di), u32(1), u32(i))]; + var ji = BodyJoints[Index3D(TensorStrides[20], TensorStrides[21], TensorStrides[22], u32(di), u32(1), u32(i))]; var f = JointCForce(ji); tf = tf+(f); var t = JointCTorque(ji); diff --git a/physics/shaders/InitDynamics.wgsl b/physics/shaders/InitDynamics.wgsl index c6f99ffe..7e032147 100644 --- a/physics/shaders/InitDynamics.wgsl +++ b/physics/shaders/InitDynamics.wgsl @@ -159,16 +159,16 @@ const DynAngDeltaX: DynamicVars = 29; const DynAngDeltaY: DynamicVars = 30; const DynAngDeltaZ: DynamicVars = 31; const DynContactWeight: DynamicVars = 32; -fn DynamicBody(idx: i32) -> i32 { return i32(bitcast(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(0), u32(DynBody))])); } +fn DynamicBody(idx: i32) -> i32 { return i32(bitcast(Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(0), u32(DynBody))])); } //////// import: "enumgen.go" const BodyVarsN: BodyVars = 43; const ContactVarsN: ContactVars = 33; const JointControlVarsN: JointControlVars = 5; const DynamicVarsN: DynamicVars = 33; -const GPUVarsN: GPUVars = 12; +const GPUVarsN: GPUVars = 13; const JointTypesN: JointTypes = 8; -const JointVarsN: JointVars = 50; +const JointVarsN: JointVars = 39; const JointDoFVarsN: JointDoFVars = 5; const ShapesN: Shapes = 6; @@ -186,54 +186,43 @@ const PlaneXZ: JointTypes = 7; alias JointVars = i32; //enums:enum const JointType: JointVars = 0; const JointEnabled: JointVars = 1; -const JointParent: JointVars = 2; -const JointChild: JointVars = 3; -const JointPPosX: JointVars = 4; -const JointPPosY: JointVars = 5; -const JointPPosZ: JointVars = 6; -const JointPQuatX: JointVars = 7; -const JointPQuatY: JointVars = 8; -const JointPQuatZ: JointVars = 9; -const JointPQuatW: JointVars = 10; -const JointCPosX: JointVars = 11; -const JointCPosY: JointVars = 12; -const JointCPosZ: JointVars = 13; -const JointCQuatX: JointVars = 14; -const JointCQuatY: JointVars = 15; -const JointCQuatZ: JointVars = 16; -const JointCQuatW: JointVars = 17; -const JointLinearDoFN: JointVars = 18; -const JointAngularDoFN: JointVars = 19; -const JointDoF1: JointVars = 20; -const JointDoF2: JointVars = 21; -const JointDoF3: JointVars = 22; -const JointDoF4: JointVars = 23; -const JointDoF5: JointVars = 24; -const JointDoF6: JointVars = 25; -const JointPForceX: JointVars = 26; -const JointPForceY: JointVars = 27; -const JointPForceZ: JointVars = 28; -const JointPTorqueX: JointVars = 29; -const JointPTorqueY: JointVars = 30; -const JointPTorqueZ: JointVars = 31; -const JointCForceX: JointVars = 32; -const JointCForceY: JointVars = 33; -const JointCForceZ: JointVars = 34; -const JointCTorqueX: JointVars = 35; -const JointCTorqueY: JointVars = 36; -const JointCTorqueZ: JointVars = 37; -const JointPDeltaX: JointVars = 38; -const JointPDeltaY: JointVars = 39; -const JointPDeltaZ: JointVars = 40; -const JointPAngDeltaX: JointVars = 41; -const JointPAngDeltaY: JointVars = 42; -const JointPAngDeltaZ: JointVars = 43; -const JointCDeltaX: JointVars = 44; -const JointCDeltaY: JointVars = 45; -const JointCDeltaZ: JointVars = 46; -const JointCAngDeltaX: JointVars = 47; -const JointCAngDeltaY: JointVars = 48; -const JointCAngDeltaZ: JointVars = 49; +const JointParentFixed: JointVars = 2; +const JointParent: JointVars = 3; +const JointChild: JointVars = 4; +const JointPPosX: JointVars = 5; +const JointPPosY: JointVars = 6; +const JointPPosZ: JointVars = 7; +const JointPQuatX: JointVars = 8; +const JointPQuatY: JointVars = 9; +const JointPQuatZ: JointVars = 10; +const JointPQuatW: JointVars = 11; +const JointCPosX: JointVars = 12; +const JointCPosY: JointVars = 13; +const JointCPosZ: JointVars = 14; +const JointCQuatX: JointVars = 15; +const JointCQuatY: JointVars = 16; +const JointCQuatZ: JointVars = 17; +const JointCQuatW: JointVars = 18; +const JointLinearDoFN: JointVars = 19; +const JointAngularDoFN: JointVars = 20; +const JointDoF1: JointVars = 21; +const JointDoF2: JointVars = 22; +const JointDoF3: JointVars = 23; +const JointDoF4: JointVars = 24; +const JointDoF5: JointVars = 25; +const JointDoF6: JointVars = 26; +const JointPForceX: JointVars = 27; +const JointPForceY: JointVars = 28; +const JointPForceZ: JointVars = 29; +const JointPTorqueX: JointVars = 30; +const JointPTorqueY: JointVars = 31; +const JointPTorqueZ: JointVars = 32; +const JointCForceX: JointVars = 33; +const JointCForceY: JointVars = 34; +const JointCForceZ: JointVars = 35; +const JointCTorqueX: JointVars = 36; +const JointCTorqueY: JointVars = 37; +const JointCTorqueZ: JointVars = 38; alias JointDoFVars = i32; //enums:enum const JointAxisX: JointDoFVars = 0; const JointAxisY: JointDoFVars = 1; @@ -263,10 +252,14 @@ struct PhysParams { Next: i32, BodiesN: i32, DynamicsN: i32, + ObjectsN: i32, + MaxObjectJoints: i32, JointsN: i32, JointDoFsN: i32, BodyJointsMax: i32, BodyCollidePairsN: i32, + pad: i32, + pad1: i32, Gravity: vec4, } @@ -314,16 +307,16 @@ fn InitDynamics(i: u32) { //gosl:kernel } for (var cni=0; cni<2; cni++) { var bi = DynamicBody(ii); - Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(ii), u32(cni), u32(DynPosX))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyPosX))]; - Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(ii), u32(cni), u32(DynPosY))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyPosY))]; - Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(ii), u32(cni), u32(DynPosZ))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyPosZ))]; - Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(ii), u32(cni), u32(DynQuatX))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyQuatX))]; - Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(ii), u32(cni), u32(DynQuatY))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyQuatY))]; - Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(ii), u32(cni), u32(DynQuatZ))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyQuatZ))]; - Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(ii), u32(cni), u32(DynQuatW))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyQuatW))]; + Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(ii), u32(cni), u32(DynPosX))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyPosX))]; + Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(ii), u32(cni), u32(DynPosY))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyPosY))]; + Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(ii), u32(cni), u32(DynPosZ))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyPosZ))]; + Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(ii), u32(cni), u32(DynQuatX))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyQuatX))]; + Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(ii), u32(cni), u32(DynQuatY))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyQuatY))]; + Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(ii), u32(cni), u32(DynQuatZ))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyQuatZ))]; + Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(ii), u32(cni), u32(DynQuatW))] = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyQuatW))]; for (var v = DynVelX; v < DynamicVarsN; v++) { - Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], + Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(ii), u32(cni), u32(v))] = 0.0; } } diff --git a/physics/shaders/StepBodyContactDeltas.wgsl b/physics/shaders/StepBodyContactDeltas.wgsl index 1cf6dac8..6d16324b 100644 --- a/physics/shaders/StepBodyContactDeltas.wgsl +++ b/physics/shaders/StepBodyContactDeltas.wgsl @@ -136,19 +136,19 @@ const ContactBAngDeltaX: ContactVars = 30; const ContactBAngDeltaY: ContactVars = 31; const ContactBAngDeltaZ: ContactVars = 32; const BroadContactVarsN = ContactAPointX; -fn GetContactA(idx: i32) -> i32 { return i32(bitcast(Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactA))])); } -fn GetContactB(idx: i32) -> i32 { return i32(bitcast(Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactB))])); } +fn GetContactA(idx: i32) -> i32 { return i32(bitcast(Contacts[Index2D(TensorStrides[100], TensorStrides[101], u32(idx), u32(ContactA))])); } +fn GetContactB(idx: i32) -> i32 { return i32(bitcast(Contacts[Index2D(TensorStrides[100], TensorStrides[101], u32(idx), u32(ContactB))])); } fn ContactADelta(idx: i32) -> vec3 { - return vec3(Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactADeltaX))], Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactADeltaY))], Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactADeltaZ))]); + return vec3(Contacts[Index2D(TensorStrides[100], TensorStrides[101], u32(idx), u32(ContactADeltaX))], Contacts[Index2D(TensorStrides[100], TensorStrides[101], u32(idx), u32(ContactADeltaY))], Contacts[Index2D(TensorStrides[100], TensorStrides[101], u32(idx), u32(ContactADeltaZ))]); } fn ContactAAngDelta(idx: i32) -> vec3 { - return vec3(Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactAAngDeltaX))], Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactAAngDeltaY))], Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactAAngDeltaZ))]); + return vec3(Contacts[Index2D(TensorStrides[100], TensorStrides[101], u32(idx), u32(ContactAAngDeltaX))], Contacts[Index2D(TensorStrides[100], TensorStrides[101], u32(idx), u32(ContactAAngDeltaY))], Contacts[Index2D(TensorStrides[100], TensorStrides[101], u32(idx), u32(ContactAAngDeltaZ))]); } fn ContactBDelta(idx: i32) -> vec3 { - return vec3(Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactBDeltaX))], Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactBDeltaY))], Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactBDeltaZ))]); + return vec3(Contacts[Index2D(TensorStrides[100], TensorStrides[101], u32(idx), u32(ContactBDeltaX))], Contacts[Index2D(TensorStrides[100], TensorStrides[101], u32(idx), u32(ContactBDeltaY))], Contacts[Index2D(TensorStrides[100], TensorStrides[101], u32(idx), u32(ContactBDeltaZ))]); } fn ContactBAngDelta(idx: i32) -> vec3 { - return vec3(Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactBAngDeltaX))], Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactBAngDeltaY))], Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactBAngDeltaZ))]); + return vec3(Contacts[Index2D(TensorStrides[100], TensorStrides[101], u32(idx), u32(ContactBAngDeltaX))], Contacts[Index2D(TensorStrides[100], TensorStrides[101], u32(idx), u32(ContactBAngDeltaY))], Contacts[Index2D(TensorStrides[100], TensorStrides[101], u32(idx), u32(ContactBAngDeltaZ))]); } fn StepBodyContactDeltas(i: u32) { //gosl:kernel var params = Params[0]; @@ -166,7 +166,7 @@ fn StepBodyContactDeltas(i: u32) { //gosl:kernel var angDel = vec3(0, 0, 0); var tw = f32(0); for (var ci=0; ci i32 { return i32(bitcast(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(0), u32(DynBody))])); } -fn DynamicPos(idx: i32,cni: i32) -> vec3 { return vec3(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosZ))]); } -fn SetDynamicPos(idx: i32,cni: i32, pos: vec3) { Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosX))] = pos.x;; Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosY))] = pos.y;; Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosZ))] = pos.z; } -fn DynamicQuat(idx: i32,cni: i32) -> vec4 { return vec4(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatZ))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatW))]); } -fn SetDynamicQuat(idx: i32,cni: i32, rot: vec4) { Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatX))] = rot.x;; Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatY))] = rot.y;; Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatZ))] = rot.z;; Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatW))] = rot.w; } +fn DynamicBody(idx: i32) -> i32 { return i32(bitcast(Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(0), u32(DynBody))])); } +fn DynamicPos(idx: i32,cni: i32) -> vec3 { return vec3(Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynPosX))], Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynPosY))], Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynPosZ))]); } +fn SetDynamicPos(idx: i32,cni: i32, pos: vec3) { Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynPosX))] = pos.x;; Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynPosY))] = pos.y;; Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynPosZ))] = pos.z; } +fn DynamicQuat(idx: i32,cni: i32) -> vec4 { return vec4(Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynQuatX))], Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynQuatY))], Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynQuatZ))], Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynQuatW))]); } +fn SetDynamicQuat(idx: i32,cni: i32, rot: vec4) { Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynQuatX))] = rot.x;; Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynQuatY))] = rot.y;; Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynQuatZ))] = rot.z;; Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynQuatW))] = rot.w; } fn DynamicDelta(idx: i32,cni: i32) -> vec3 { - return vec3(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaZ))]); + return vec3(Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynDeltaX))], Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynDeltaY))], Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynDeltaZ))]); } fn SetDynamicDelta(idx: i32,cni: i32, delta: vec3) { - Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaX))] = delta.x; - Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaY))] = delta.y; - Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaZ))] = delta.z; + Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynDeltaX))] = delta.x; + Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynDeltaY))] = delta.y; + Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynDeltaZ))] = delta.z; } fn DynamicAngDelta(idx: i32,cni: i32) -> vec3 { - return vec3(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaZ))]); + return vec3(Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynAngDeltaX))], Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynAngDeltaY))], Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynAngDeltaZ))]); } fn SetDynamicAngDelta(idx: i32,cni: i32, angDelta: vec3) { - Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaX))] = angDelta.x; - Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaY))] = angDelta.y; - Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaZ))] = angDelta.z; + Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynAngDeltaX))] = angDelta.x; + Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynAngDeltaY))] = angDelta.y; + Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynAngDeltaZ))] = angDelta.z; } //////// import: "enumgen.go" @@ -262,9 +262,9 @@ const BodyVarsN: BodyVars = 43; const ContactVarsN: ContactVars = 33; const JointControlVarsN: JointControlVars = 5; const DynamicVarsN: DynamicVars = 33; -const GPUVarsN: GPUVars = 12; +const GPUVarsN: GPUVars = 13; const JointTypesN: JointTypes = 8; -const JointVarsN: JointVars = 50; +const JointVarsN: JointVars = 39; const JointDoFVarsN: JointDoFVars = 5; const ShapesN: Shapes = 6; @@ -282,54 +282,43 @@ const PlaneXZ: JointTypes = 7; alias JointVars = i32; //enums:enum const JointType: JointVars = 0; const JointEnabled: JointVars = 1; -const JointParent: JointVars = 2; -const JointChild: JointVars = 3; -const JointPPosX: JointVars = 4; -const JointPPosY: JointVars = 5; -const JointPPosZ: JointVars = 6; -const JointPQuatX: JointVars = 7; -const JointPQuatY: JointVars = 8; -const JointPQuatZ: JointVars = 9; -const JointPQuatW: JointVars = 10; -const JointCPosX: JointVars = 11; -const JointCPosY: JointVars = 12; -const JointCPosZ: JointVars = 13; -const JointCQuatX: JointVars = 14; -const JointCQuatY: JointVars = 15; -const JointCQuatZ: JointVars = 16; -const JointCQuatW: JointVars = 17; -const JointLinearDoFN: JointVars = 18; -const JointAngularDoFN: JointVars = 19; -const JointDoF1: JointVars = 20; -const JointDoF2: JointVars = 21; -const JointDoF3: JointVars = 22; -const JointDoF4: JointVars = 23; -const JointDoF5: JointVars = 24; -const JointDoF6: JointVars = 25; -const JointPForceX: JointVars = 26; -const JointPForceY: JointVars = 27; -const JointPForceZ: JointVars = 28; -const JointPTorqueX: JointVars = 29; -const JointPTorqueY: JointVars = 30; -const JointPTorqueZ: JointVars = 31; -const JointCForceX: JointVars = 32; -const JointCForceY: JointVars = 33; -const JointCForceZ: JointVars = 34; -const JointCTorqueX: JointVars = 35; -const JointCTorqueY: JointVars = 36; -const JointCTorqueZ: JointVars = 37; -const JointPDeltaX: JointVars = 38; -const JointPDeltaY: JointVars = 39; -const JointPDeltaZ: JointVars = 40; -const JointPAngDeltaX: JointVars = 41; -const JointPAngDeltaY: JointVars = 42; -const JointPAngDeltaZ: JointVars = 43; -const JointCDeltaX: JointVars = 44; -const JointCDeltaY: JointVars = 45; -const JointCDeltaZ: JointVars = 46; -const JointCAngDeltaX: JointVars = 47; -const JointCAngDeltaY: JointVars = 48; -const JointCAngDeltaZ: JointVars = 49; +const JointParentFixed: JointVars = 2; +const JointParent: JointVars = 3; +const JointChild: JointVars = 4; +const JointPPosX: JointVars = 5; +const JointPPosY: JointVars = 6; +const JointPPosZ: JointVars = 7; +const JointPQuatX: JointVars = 8; +const JointPQuatY: JointVars = 9; +const JointPQuatZ: JointVars = 10; +const JointPQuatW: JointVars = 11; +const JointCPosX: JointVars = 12; +const JointCPosY: JointVars = 13; +const JointCPosZ: JointVars = 14; +const JointCQuatX: JointVars = 15; +const JointCQuatY: JointVars = 16; +const JointCQuatZ: JointVars = 17; +const JointCQuatW: JointVars = 18; +const JointLinearDoFN: JointVars = 19; +const JointAngularDoFN: JointVars = 20; +const JointDoF1: JointVars = 21; +const JointDoF2: JointVars = 22; +const JointDoF3: JointVars = 23; +const JointDoF4: JointVars = 24; +const JointDoF5: JointVars = 25; +const JointDoF6: JointVars = 26; +const JointPForceX: JointVars = 27; +const JointPForceY: JointVars = 28; +const JointPForceZ: JointVars = 29; +const JointPTorqueX: JointVars = 30; +const JointPTorqueY: JointVars = 31; +const JointPTorqueZ: JointVars = 32; +const JointCForceX: JointVars = 33; +const JointCForceY: JointVars = 34; +const JointCForceZ: JointVars = 35; +const JointCTorqueX: JointVars = 36; +const JointCTorqueY: JointVars = 37; +const JointCTorqueZ: JointVars = 38; alias JointDoFVars = i32; //enums:enum const JointAxisX: JointDoFVars = 0; const JointAxisY: JointDoFVars = 1; @@ -359,10 +348,14 @@ struct PhysParams { Next: i32, BodiesN: i32, DynamicsN: i32, + ObjectsN: i32, + MaxObjectJoints: i32, JointsN: i32, JointDoFsN: i32, BodyJointsMax: i32, BodyCollidePairsN: i32, + pad: i32, + pad1: i32, Gravity: vec4, } diff --git a/physics/shaders/StepBodyContacts.wgsl b/physics/shaders/StepBodyContacts.wgsl index 6942beab..ad14c325 100644 --- a/physics/shaders/StepBodyContacts.wgsl +++ b/physics/shaders/StepBodyContacts.wgsl @@ -153,17 +153,17 @@ const ContactBAngDeltaX: ContactVars = 30; const ContactBAngDeltaY: ContactVars = 31; const ContactBAngDeltaZ: ContactVars = 32; const BroadContactVarsN = ContactAPointX; -fn GetContactA(idx: i32) -> i32 { return i32(bitcast(Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactA))])); } -fn GetContactB(idx: i32) -> i32 { return i32(bitcast(Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactB))])); } -fn ContactAPoint(idx: i32) -> vec3 { return vec3(Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactAPointX))], Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactAPointY))], Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactAPointZ))]); } -fn ContactBPoint(idx: i32) -> vec3 { return vec3(Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactBPointX))], Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactBPointY))], Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactBPointZ))]); } -fn ContactAOff(idx: i32) -> vec3 { return vec3(Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactAOffX))], Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactAOffY))], Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactAOffZ))]); } -fn ContactBOff(idx: i32) -> vec3 { return vec3(Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactBOffX))], Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactBOffY))], Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactBOffZ))]); } -fn ContactNorm(idx: i32) -> vec3 { return vec3(Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactNormX))], Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactNormY))], Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactNormZ))]); } -fn SetContactADelta(idx: i32, pos: vec3) { Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactADeltaX))] = pos.x;; Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactADeltaY))] = pos.y;; Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactADeltaZ))] = pos.z; } -fn SetContactAAngDelta(idx: i32, pos: vec3) { Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactAAngDeltaX))] = pos.x;; Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactAAngDeltaY))] = pos.y;; Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactAAngDeltaZ))] = pos.z; } -fn SetContactBDelta(idx: i32, pos: vec3) { Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactBDeltaX))] = pos.x;; Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactBDeltaY))] = pos.y;; Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactBDeltaZ))] = pos.z; } -fn SetContactBAngDelta(idx: i32, pos: vec3) { Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactBAngDeltaX))] = pos.x;; Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactBAngDeltaY))] = pos.y;; Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(idx), u32(ContactBAngDeltaZ))] = pos.z; } +fn GetContactA(idx: i32) -> i32 { return i32(bitcast(Contacts[Index2D(TensorStrides[100], TensorStrides[101], u32(idx), u32(ContactA))])); } +fn GetContactB(idx: i32) -> i32 { return i32(bitcast(Contacts[Index2D(TensorStrides[100], TensorStrides[101], u32(idx), u32(ContactB))])); } +fn ContactAPoint(idx: i32) -> vec3 { return vec3(Contacts[Index2D(TensorStrides[100], TensorStrides[101], u32(idx), u32(ContactAPointX))], Contacts[Index2D(TensorStrides[100], TensorStrides[101], u32(idx), u32(ContactAPointY))], Contacts[Index2D(TensorStrides[100], TensorStrides[101], u32(idx), u32(ContactAPointZ))]); } +fn ContactBPoint(idx: i32) -> vec3 { return vec3(Contacts[Index2D(TensorStrides[100], TensorStrides[101], u32(idx), u32(ContactBPointX))], Contacts[Index2D(TensorStrides[100], TensorStrides[101], u32(idx), u32(ContactBPointY))], Contacts[Index2D(TensorStrides[100], TensorStrides[101], u32(idx), u32(ContactBPointZ))]); } +fn ContactAOff(idx: i32) -> vec3 { return vec3(Contacts[Index2D(TensorStrides[100], TensorStrides[101], u32(idx), u32(ContactAOffX))], Contacts[Index2D(TensorStrides[100], TensorStrides[101], u32(idx), u32(ContactAOffY))], Contacts[Index2D(TensorStrides[100], TensorStrides[101], u32(idx), u32(ContactAOffZ))]); } +fn ContactBOff(idx: i32) -> vec3 { return vec3(Contacts[Index2D(TensorStrides[100], TensorStrides[101], u32(idx), u32(ContactBOffX))], Contacts[Index2D(TensorStrides[100], TensorStrides[101], u32(idx), u32(ContactBOffY))], Contacts[Index2D(TensorStrides[100], TensorStrides[101], u32(idx), u32(ContactBOffZ))]); } +fn ContactNorm(idx: i32) -> vec3 { return vec3(Contacts[Index2D(TensorStrides[100], TensorStrides[101], u32(idx), u32(ContactNormX))], Contacts[Index2D(TensorStrides[100], TensorStrides[101], u32(idx), u32(ContactNormY))], Contacts[Index2D(TensorStrides[100], TensorStrides[101], u32(idx), u32(ContactNormZ))]); } +fn SetContactADelta(idx: i32, pos: vec3) { Contacts[Index2D(TensorStrides[100], TensorStrides[101], u32(idx), u32(ContactADeltaX))] = pos.x;; Contacts[Index2D(TensorStrides[100], TensorStrides[101], u32(idx), u32(ContactADeltaY))] = pos.y;; Contacts[Index2D(TensorStrides[100], TensorStrides[101], u32(idx), u32(ContactADeltaZ))] = pos.z; } +fn SetContactAAngDelta(idx: i32, pos: vec3) { Contacts[Index2D(TensorStrides[100], TensorStrides[101], u32(idx), u32(ContactAAngDeltaX))] = pos.x;; Contacts[Index2D(TensorStrides[100], TensorStrides[101], u32(idx), u32(ContactAAngDeltaY))] = pos.y;; Contacts[Index2D(TensorStrides[100], TensorStrides[101], u32(idx), u32(ContactAAngDeltaZ))] = pos.z; } +fn SetContactBDelta(idx: i32, pos: vec3) { Contacts[Index2D(TensorStrides[100], TensorStrides[101], u32(idx), u32(ContactBDeltaX))] = pos.x;; Contacts[Index2D(TensorStrides[100], TensorStrides[101], u32(idx), u32(ContactBDeltaY))] = pos.y;; Contacts[Index2D(TensorStrides[100], TensorStrides[101], u32(idx), u32(ContactBDeltaZ))] = pos.z; } +fn SetContactBAngDelta(idx: i32, pos: vec3) { Contacts[Index2D(TensorStrides[100], TensorStrides[101], u32(idx), u32(ContactBAngDeltaX))] = pos.x;; Contacts[Index2D(TensorStrides[100], TensorStrides[101], u32(idx), u32(ContactBAngDeltaY))] = pos.y;; Contacts[Index2D(TensorStrides[100], TensorStrides[101], u32(idx), u32(ContactBAngDeltaZ))] = pos.z; } fn StepBodyContacts(i: u32) { //gosl:kernel var params = Params[0];; var ci = i32(i); ; var cmax = ContactsN[0]; @@ -183,15 +183,15 @@ var params = Params[0];; var ci = i32(i); ; var offB = ContactBOff(ci); ; var ctAw = MulSpatialPoint(r1A, q1A, ctA); ; var ctBw = MulSpatialPoint(r1B, q1B, ctB); -; var thickA = Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(ci), u32(ContactAThick))]; -; var thickB = Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(ci), u32(ContactBThick))]; +; var thickA = Contacts[Index2D(TensorStrides[100], TensorStrides[101], u32(ci), u32(ContactAThick))]; +; var thickB = Contacts[Index2D(TensorStrides[100], TensorStrides[101], u32(ci), u32(ContactBThick))]; ; var thick = thickA + thickB; ; var nnorm = ContactNorm(ci); ; var norm = Negate3(nnorm); ; var d = Dot3(norm, ctBw-(ctAw)) - thick; ; if (d >= 0.0) { // todo: should this be margin or not? - Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(ci), u32(ContactWeight))] = 0.0; + Contacts[Index2D(TensorStrides[100], TensorStrides[101], u32(ci), u32(ContactWeight))] = 0.0; var z = vec3(0, 0, 0); SetContactADelta(ci, z); SetContactBDelta(ci, z); @@ -315,7 +315,7 @@ if (params.Restitution == 1 && bounce > 0 && (mInvA > 0 || mInvB > 0)) { angDeltaB = angDeltaB+(dwB); } } -}; Contacts[Index2D(TensorStrides[90], TensorStrides[91], u32(ci), u32(ContactWeight))] = 1.0;; SetContactADelta(ci, linDeltaA);; SetContactBDelta(ci, linDeltaB);; SetContactAAngDelta(ci, angDeltaA);; SetContactBAngDelta(ci, angDeltaB); } +}; Contacts[Index2D(TensorStrides[100], TensorStrides[101], u32(ci), u32(ContactWeight))] = 1.0;; SetContactADelta(ci, linDeltaA);; SetContactBDelta(ci, linDeltaB);; SetContactAAngDelta(ci, angDeltaA);; SetContactBAngDelta(ci, angDeltaB); } fn ContactConstraint(err: f32, q0A: vec4,q0B: vec4, mInvA: f32,mInvB: f32, iInvA: mat3x3f,iInvB: mat3x3f, linA: vec3,linB: vec3,angA: vec3,angB: vec3, relaxation: f32,dt: f32) -> f32 { var denom = f32(0.0); denom += LengthSquared3(linA) * mInvA; @@ -374,16 +374,16 @@ const DynAngDeltaY: DynamicVars = 30; const DynAngDeltaZ: DynamicVars = 31; const DynContactWeight: DynamicVars = 32; fn DynamicPos(idx: i32,cni: i32) -> vec3 { - return vec3(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosZ))]); + return vec3(Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynPosX))], Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynPosY))], Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynPosZ))]); } fn DynamicQuat(idx: i32,cni: i32) -> vec4 { - return vec4(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatZ))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatW))]); + return vec4(Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynQuatX))], Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynQuatY))], Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynQuatZ))], Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynQuatW))]); } fn DynamicDelta(idx: i32,cni: i32) -> vec3 { - return vec3(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaZ))]); + return vec3(Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynDeltaX))], Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynDeltaY))], Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynDeltaZ))]); } fn DynamicAngDelta(idx: i32,cni: i32) -> vec3 { - return vec3(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaZ))]); + return vec3(Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynAngDeltaX))], Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynAngDeltaY))], Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynAngDeltaZ))]); } //////// import: "enumgen.go" @@ -391,9 +391,9 @@ const BodyVarsN: BodyVars = 43; const ContactVarsN: ContactVars = 33; const JointControlVarsN: JointControlVars = 5; const DynamicVarsN: DynamicVars = 33; -const GPUVarsN: GPUVars = 12; +const GPUVarsN: GPUVars = 13; const JointTypesN: JointTypes = 8; -const JointVarsN: JointVars = 50; +const JointVarsN: JointVars = 39; const JointDoFVarsN: JointDoFVars = 5; const ShapesN: Shapes = 6; @@ -411,54 +411,43 @@ const PlaneXZ: JointTypes = 7; alias JointVars = i32; //enums:enum const JointType: JointVars = 0; const JointEnabled: JointVars = 1; -const JointParent: JointVars = 2; -const JointChild: JointVars = 3; -const JointPPosX: JointVars = 4; -const JointPPosY: JointVars = 5; -const JointPPosZ: JointVars = 6; -const JointPQuatX: JointVars = 7; -const JointPQuatY: JointVars = 8; -const JointPQuatZ: JointVars = 9; -const JointPQuatW: JointVars = 10; -const JointCPosX: JointVars = 11; -const JointCPosY: JointVars = 12; -const JointCPosZ: JointVars = 13; -const JointCQuatX: JointVars = 14; -const JointCQuatY: JointVars = 15; -const JointCQuatZ: JointVars = 16; -const JointCQuatW: JointVars = 17; -const JointLinearDoFN: JointVars = 18; -const JointAngularDoFN: JointVars = 19; -const JointDoF1: JointVars = 20; -const JointDoF2: JointVars = 21; -const JointDoF3: JointVars = 22; -const JointDoF4: JointVars = 23; -const JointDoF5: JointVars = 24; -const JointDoF6: JointVars = 25; -const JointPForceX: JointVars = 26; -const JointPForceY: JointVars = 27; -const JointPForceZ: JointVars = 28; -const JointPTorqueX: JointVars = 29; -const JointPTorqueY: JointVars = 30; -const JointPTorqueZ: JointVars = 31; -const JointCForceX: JointVars = 32; -const JointCForceY: JointVars = 33; -const JointCForceZ: JointVars = 34; -const JointCTorqueX: JointVars = 35; -const JointCTorqueY: JointVars = 36; -const JointCTorqueZ: JointVars = 37; -const JointPDeltaX: JointVars = 38; -const JointPDeltaY: JointVars = 39; -const JointPDeltaZ: JointVars = 40; -const JointPAngDeltaX: JointVars = 41; -const JointPAngDeltaY: JointVars = 42; -const JointPAngDeltaZ: JointVars = 43; -const JointCDeltaX: JointVars = 44; -const JointCDeltaY: JointVars = 45; -const JointCDeltaZ: JointVars = 46; -const JointCAngDeltaX: JointVars = 47; -const JointCAngDeltaY: JointVars = 48; -const JointCAngDeltaZ: JointVars = 49; +const JointParentFixed: JointVars = 2; +const JointParent: JointVars = 3; +const JointChild: JointVars = 4; +const JointPPosX: JointVars = 5; +const JointPPosY: JointVars = 6; +const JointPPosZ: JointVars = 7; +const JointPQuatX: JointVars = 8; +const JointPQuatY: JointVars = 9; +const JointPQuatZ: JointVars = 10; +const JointPQuatW: JointVars = 11; +const JointCPosX: JointVars = 12; +const JointCPosY: JointVars = 13; +const JointCPosZ: JointVars = 14; +const JointCQuatX: JointVars = 15; +const JointCQuatY: JointVars = 16; +const JointCQuatZ: JointVars = 17; +const JointCQuatW: JointVars = 18; +const JointLinearDoFN: JointVars = 19; +const JointAngularDoFN: JointVars = 20; +const JointDoF1: JointVars = 21; +const JointDoF2: JointVars = 22; +const JointDoF3: JointVars = 23; +const JointDoF4: JointVars = 24; +const JointDoF5: JointVars = 25; +const JointDoF6: JointVars = 26; +const JointPForceX: JointVars = 27; +const JointPForceY: JointVars = 28; +const JointPForceZ: JointVars = 29; +const JointPTorqueX: JointVars = 30; +const JointPTorqueY: JointVars = 31; +const JointPTorqueZ: JointVars = 32; +const JointCForceX: JointVars = 33; +const JointCForceY: JointVars = 34; +const JointCForceZ: JointVars = 35; +const JointCTorqueX: JointVars = 36; +const JointCTorqueY: JointVars = 37; +const JointCTorqueZ: JointVars = 38; alias JointDoFVars = i32; //enums:enum const JointAxisX: JointDoFVars = 0; const JointAxisY: JointDoFVars = 1; @@ -488,10 +477,14 @@ struct PhysParams { Next: i32, BodiesN: i32, DynamicsN: i32, + ObjectsN: i32, + MaxObjectJoints: i32, JointsN: i32, JointDoFsN: i32, BodyJointsMax: i32, BodyCollidePairsN: i32, + pad: i32, + pad1: i32, Gravity: vec4, } diff --git a/physics/shaders/StepBodyJointDeltas.wgsl b/physics/shaders/StepBodyJointDeltas.wgsl deleted file mode 100644 index 498c036f..00000000 --- a/physics/shaders/StepBodyJointDeltas.wgsl +++ /dev/null @@ -1,477 +0,0 @@ -// Code generated by "gosl"; DO NOT EDIT -// kernel: StepBodyJointDeltas - -// // Params are global parameters. -@group(0) @binding(0) -var TensorStrides: array; -@group(0) @binding(1) -var Params: array; -// // Bodies are the rigid body elements (dynamic and static), // specifying the constant, non-dynamic properties, // which is initial state for dynamics. // [body][BodyVarsN] -@group(1) @binding(0) -var Bodies: array; -@group(1) @binding(1) -var Joints: array; -@group(1) @binding(3) -var BodyJoints: array; -// // Dynamics are the dynamic rigid body elements: these actually move. // Two alternating states are used: Params.Cur and Params.Next. // [dyn body][cur/next][DynamicVarsN] -@group(2) @binding(0) -var Dynamics: array; -// // JointControls are dynamic joint control inputs, per joint DoF // (in correspondence with [JointDoFs]). This can be uploaded to the // GPU at every step. // [dof][JointControlVarsN] - -alias GPUVars = i32; - -@compute @workgroup_size(64, 1, 1) -fn main(@builtin(workgroup_id) wgid: vec3, @builtin(num_workgroups) nwg: vec3, @builtin(local_invocation_index) loci: u32) { - let idx = loci + (wgid.x + wgid.y * nwg.x + wgid.z * nwg.x * nwg.y) * 64; - StepBodyJointDeltas(idx); -} - -fn Index2D(s0: u32, s1: u32, i0: u32, i1: u32) -> u32 { - return s0 * i0 + s1 * i1; -} - -fn Index3D(s0: u32, s1: u32, s2: u32, i0: u32, i1: u32, i2: u32) -> u32 { - return s0 * i0 + s1 * i1 + s2 * i2; -} - - -//////// import: "vars.go" - -//////// import: "body.go" -alias BodyVars = i32; //enums:enum -const BodyShape: BodyVars = 0; -const BodyDynamic: BodyVars = 1; -const BodyWorld: BodyVars = 2; -const BodyGroup: BodyVars = 3; -const BodyHSizeX: BodyVars = 4; -const BodyHSizeY: BodyVars = 5; -const BodyHSizeZ: BodyVars = 6; -const BodyThick: BodyVars = 7; -const BodyMass: BodyVars = 8; -const BodyInvMass: BodyVars = 9; -const BodyBounce: BodyVars = 10; -const BodyFriction: BodyVars = 11; -const BodyFrictionTortion: BodyVars = 12; -const BodyFrictionRolling: BodyVars = 13; -const BodyPosX: BodyVars = 14; -const BodyPosY: BodyVars = 15; -const BodyPosZ: BodyVars = 16; -const BodyQuatX: BodyVars = 17; -const BodyQuatY: BodyVars = 18; -const BodyQuatZ: BodyVars = 19; -const BodyQuatW: BodyVars = 20; -const BodyComX: BodyVars = 21; -const BodyComY: BodyVars = 22; -const BodyComZ: BodyVars = 23; -const BodyInertiaXX: BodyVars = 24; -const BodyInertiaYX: BodyVars = 25; -const BodyInertiaZX: BodyVars = 26; -const BodyInertiaXY: BodyVars = 27; -const BodyInertiaYY: BodyVars = 28; -const BodyInertiaZY: BodyVars = 29; -const BodyInertiaXZ: BodyVars = 30; -const BodyInertiaYZ: BodyVars = 31; -const BodyInertiaZZ: BodyVars = 32; -const BodyInvInertiaXX: BodyVars = 33; -const BodyInvInertiaYX: BodyVars = 34; -const BodyInvInertiaZX: BodyVars = 35; -const BodyInvInertiaXY: BodyVars = 36; -const BodyInvInertiaYY: BodyVars = 37; -const BodyInvInertiaZY: BodyVars = 38; -const BodyInvInertiaXZ: BodyVars = 39; -const BodyInvInertiaYZ: BodyVars = 40; -const BodyInvInertiaZZ: BodyVars = 41; -const BodyRadius: BodyVars = 42; -fn BodyCom(idx: i32) -> vec3 { - return vec3(Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComX))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComY))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComZ))]); -} -fn BodyInertia(idx: i32) -> mat3x3f { - return mat3x3f(Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInertiaXX))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInertiaYX))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInertiaZX))], - Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInertiaXY))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInertiaYY))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInertiaZY))], - Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInertiaXZ))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInertiaYZ))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInertiaZZ))]); -} -fn BodyInvInertia(idx: i32) -> mat3x3f { - return mat3x3f(Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInvInertiaXX))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInvInertiaYX))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInvInertiaZX))], - Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInvInertiaXY))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInvInertiaYY))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInvInertiaZY))], - Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInvInertiaXZ))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInvInertiaYZ))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInvInertiaZZ))]); -} - -//////// import: "contact.go" -alias ContactVars = i32; //enums:enum -const ContactA: ContactVars = 0; -const ContactB: ContactVars = 1; -const ContactPointIdx: ContactVars = 2; -const ContactAPointX: ContactVars = 3; -const ContactAPointY: ContactVars = 4; -const ContactAPointZ: ContactVars = 5; -const ContactBPointX: ContactVars = 6; -const ContactBPointY: ContactVars = 7; -const ContactBPointZ: ContactVars = 8; -const ContactAOffX: ContactVars = 9; -const ContactAOffY: ContactVars = 10; -const ContactAOffZ: ContactVars = 11; -const ContactBOffX: ContactVars = 12; -const ContactBOffY: ContactVars = 13; -const ContactBOffZ: ContactVars = 14; -const ContactAThick: ContactVars = 15; -const ContactBThick: ContactVars = 16; -const ContactNormX: ContactVars = 17; -const ContactNormY: ContactVars = 18; -const ContactNormZ: ContactVars = 19; -const ContactWeight: ContactVars = 20; -const ContactADeltaX: ContactVars = 21; -const ContactADeltaY: ContactVars = 22; -const ContactADeltaZ: ContactVars = 23; -const ContactAAngDeltaX: ContactVars = 24; -const ContactAAngDeltaY: ContactVars = 25; -const ContactAAngDeltaZ: ContactVars = 26; -const ContactBDeltaX: ContactVars = 27; -const ContactBDeltaY: ContactVars = 28; -const ContactBDeltaZ: ContactVars = 29; -const ContactBAngDeltaX: ContactVars = 30; -const ContactBAngDeltaY: ContactVars = 31; -const ContactBAngDeltaZ: ContactVars = 32; -const BroadContactVarsN = ContactAPointX; - -//////// import: "control.go" -alias JointControlVars = i32; //enums:enum -const JointControlForce: JointControlVars = 0; -const JointTargetPos: JointControlVars = 1; -const JointTargetStiff: JointControlVars = 2; -const JointTargetVel: JointControlVars = 3; -const JointTargetDamp: JointControlVars = 4; - -//////// import: "dynamics.go" -alias DynamicVars = i32; //enums:enum -const DynBody: DynamicVars = 0; -const DynPosX: DynamicVars = 1; -const DynPosY: DynamicVars = 2; -const DynPosZ: DynamicVars = 3; -const DynQuatX: DynamicVars = 4; -const DynQuatY: DynamicVars = 5; -const DynQuatZ: DynamicVars = 6; -const DynQuatW: DynamicVars = 7; -const DynVelX: DynamicVars = 8; -const DynVelY: DynamicVars = 9; -const DynVelZ: DynamicVars = 10; -const DynAngVelX: DynamicVars = 11; -const DynAngVelY: DynamicVars = 12; -const DynAngVelZ: DynamicVars = 13; -const DynAccX: DynamicVars = 14; -const DynAccY: DynamicVars = 15; -const DynAccZ: DynamicVars = 16; -const DynAngAccX: DynamicVars = 17; -const DynAngAccY: DynamicVars = 18; -const DynAngAccZ: DynamicVars = 19; -const DynForceX: DynamicVars = 20; -const DynForceY: DynamicVars = 21; -const DynForceZ: DynamicVars = 22; -const DynTorqueX: DynamicVars = 23; -const DynTorqueY: DynamicVars = 24; -const DynTorqueZ: DynamicVars = 25; -const DynDeltaX: DynamicVars = 26; -const DynDeltaY: DynamicVars = 27; -const DynDeltaZ: DynamicVars = 28; -const DynAngDeltaX: DynamicVars = 29; -const DynAngDeltaY: DynamicVars = 30; -const DynAngDeltaZ: DynamicVars = 31; -const DynContactWeight: DynamicVars = 32; -fn DynamicBody(idx: i32) -> i32 { return i32(bitcast(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(0), u32(DynBody))])); } -fn DynamicPos(idx: i32,cni: i32) -> vec3 { return vec3(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosZ))]); } -fn SetDynamicPos(idx: i32,cni: i32, pos: vec3) { Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosX))] = pos.x;; Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosY))] = pos.y;; Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosZ))] = pos.z; } -fn DynamicQuat(idx: i32,cni: i32) -> vec4 { return vec4(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatZ))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatW))]); } -fn SetDynamicQuat(idx: i32,cni: i32, rot: vec4) { Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatX))] = rot.x;; Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatY))] = rot.y;; Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatZ))] = rot.z;; Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatW))] = rot.w; } -fn DynamicDelta(idx: i32,cni: i32) -> vec3 { - return vec3(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaZ))]); -} -fn SetDynamicDelta(idx: i32,cni: i32, delta: vec3) { - Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaX))] = delta.x; - Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaY))] = delta.y; - Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaZ))] = delta.z; -} -fn DynamicAngDelta(idx: i32,cni: i32) -> vec3 { - return vec3(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaZ))]); -} -fn SetDynamicAngDelta(idx: i32,cni: i32, angDelta: vec3) { - Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaX))] = angDelta.x; - Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaY))] = angDelta.y; - Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaZ))] = angDelta.z; -} - -//////// import: "enumgen.go" -const BodyVarsN: BodyVars = 43; -const ContactVarsN: ContactVars = 33; -const JointControlVarsN: JointControlVars = 5; -const DynamicVarsN: DynamicVars = 33; -const GPUVarsN: GPUVars = 12; -const JointTypesN: JointTypes = 8; -const JointVarsN: JointVars = 50; -const JointDoFVarsN: JointDoFVars = 5; -const ShapesN: Shapes = 6; - -//////// import: "joint.go" -const JointLimitUnlimited = 1e10; -alias JointTypes = i32; //enums:enum -const Prismatic: JointTypes = 0; -const Revolute: JointTypes = 1; -const Ball: JointTypes = 2; -const Fixed: JointTypes = 3; -const Free: JointTypes = 4; -const Distance: JointTypes = 5; -const D6: JointTypes = 6; -const PlaneXZ: JointTypes = 7; -alias JointVars = i32; //enums:enum -const JointType: JointVars = 0; -const JointEnabled: JointVars = 1; -const JointParent: JointVars = 2; -const JointChild: JointVars = 3; -const JointPPosX: JointVars = 4; -const JointPPosY: JointVars = 5; -const JointPPosZ: JointVars = 6; -const JointPQuatX: JointVars = 7; -const JointPQuatY: JointVars = 8; -const JointPQuatZ: JointVars = 9; -const JointPQuatW: JointVars = 10; -const JointCPosX: JointVars = 11; -const JointCPosY: JointVars = 12; -const JointCPosZ: JointVars = 13; -const JointCQuatX: JointVars = 14; -const JointCQuatY: JointVars = 15; -const JointCQuatZ: JointVars = 16; -const JointCQuatW: JointVars = 17; -const JointLinearDoFN: JointVars = 18; -const JointAngularDoFN: JointVars = 19; -const JointDoF1: JointVars = 20; -const JointDoF2: JointVars = 21; -const JointDoF3: JointVars = 22; -const JointDoF4: JointVars = 23; -const JointDoF5: JointVars = 24; -const JointDoF6: JointVars = 25; -const JointPForceX: JointVars = 26; -const JointPForceY: JointVars = 27; -const JointPForceZ: JointVars = 28; -const JointPTorqueX: JointVars = 29; -const JointPTorqueY: JointVars = 30; -const JointPTorqueZ: JointVars = 31; -const JointCForceX: JointVars = 32; -const JointCForceY: JointVars = 33; -const JointCForceZ: JointVars = 34; -const JointCTorqueX: JointVars = 35; -const JointCTorqueY: JointVars = 36; -const JointCTorqueZ: JointVars = 37; -const JointPDeltaX: JointVars = 38; -const JointPDeltaY: JointVars = 39; -const JointPDeltaZ: JointVars = 40; -const JointPAngDeltaX: JointVars = 41; -const JointPAngDeltaY: JointVars = 42; -const JointPAngDeltaZ: JointVars = 43; -const JointCDeltaX: JointVars = 44; -const JointCDeltaY: JointVars = 45; -const JointCDeltaZ: JointVars = 46; -const JointCAngDeltaX: JointVars = 47; -const JointCAngDeltaY: JointVars = 48; -const JointCAngDeltaZ: JointVars = 49; -fn JointPDelta(idx: i32) -> vec3 { - return vec3(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPDeltaX))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPDeltaY))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPDeltaZ))]); -} -fn JointPAngDelta(idx: i32) -> vec3 { - return vec3(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPAngDeltaX))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPAngDeltaY))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPAngDeltaZ))]); -} -fn JointCDelta(idx: i32) -> vec3 { - return vec3(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCDeltaX))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCDeltaY))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCDeltaZ))]); -} -fn JointCAngDelta(idx: i32) -> vec3 { - return vec3(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCAngDeltaX))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCAngDeltaY))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCAngDeltaZ))]); -} -alias JointDoFVars = i32; //enums:enum -const JointAxisX: JointDoFVars = 0; -const JointAxisY: JointDoFVars = 1; -const JointAxisZ: JointDoFVars = 2; -const JointLimitLower: JointDoFVars = 3; -const JointLimitUpper: JointDoFVars = 4; - -//////// import: "params.go" -struct PhysParams { - Iterations: i32, - Dt: f32, - SubSteps: i32, - ContactMargin: f32, - ContactRelax: f32, // 0.8 def - ContactWeighting: i32, // true - Restitution: i32, // false - JointLinearRelax: f32, // 0.7 def - JointAngularRelax: f32, // 0.4 def - JointLinearComply: f32, // 0 def - JointAngularComply: f32, // 0 def - AngularDamping: f32, // 0 def - SoftRelax: f32, - MaxForce: f32, - MaxGeomIter: i32, - ContactsMax: i32, - Cur: i32, - Next: i32, - BodiesN: i32, - DynamicsN: i32, - JointsN: i32, - JointDoFsN: i32, - BodyJointsMax: i32, - BodyCollidePairsN: i32, - Gravity: vec4, -} - -//////// import: "shapecollide.go" -struct GeomData { - BodyIdx: i32, - Shape: Shapes, - MinSize: f32, - Thick: f32, - Radius: f32, - Size: vec3, - WbR: vec3, - WbQ: vec4, - BwR: vec3, - BwQ: vec4, -} - -//////// import: "shapegeom.go" - -//////// import: "shapes.go" -alias Shapes = i32; //enums:enum -const Plane: Shapes = 0; -const Sphere: Shapes = 1; -const Capsule: Shapes = 2; -const Cylinder: Shapes = 3; -const Box: Shapes = 4; -const Cone: Shapes = 5; - -//////// import: "slmath-matrix3.go" - -//////// import: "slmath-quaternion.go" -fn QuatLength(q: vec4) -> f32 { - return sqrt(q.x*q.x + q.y*q.y + q.z*q.z + q.w*q.w); -} -fn QuatNormalize(q: vec4) -> vec4 { - var nq = q; - var l = QuatLength(q); - if (l == 0) { - nq.x = f32(0); - nq.y = f32(0); - nq.z = f32(0); - nq.w = f32(1); - } else { - l = 1 / l; - nq.x *= l; - nq.y *= l; - nq.z *= l; - nq.w *= l; - }return nq; -} -fn MulQuatVector(q: vec4, v: vec3) -> vec3 { - var xyz = vec3(q.x, q.y, q.z); - var t = Cross3(xyz, v)*(2); -return v+(t*(q.w))+(Cross3(xyz, t)); -} -fn MulQuatVectorInverse(q: vec4, v: vec3) -> vec3 { - var xyz = vec3(q.x, q.y, q.z); - var t = Cross3(xyz, v)*(2); -return v-(t*(q.w))+(Cross3(xyz, t)); -} -fn MulQuats(a: vec4,b: vec4) -> vec4 { - var q: vec4; - q.x = a.x*b.w + a.w*b.x + a.y*b.z - a.z*b.y; - q.y = a.y*b.w + a.w*b.y + a.z*b.x - a.x*b.z; - q.z = a.z*b.w + a.w*b.z + a.x*b.y - a.y*b.x; - q.w = a.w*b.w - a.x*b.x - a.y*b.y - a.z*b.z; -return q; -} - -//////// import: "slmath-vector2.go" - -//////// import: "slmath-vector3.go" -fn Length3(v: vec3) -> f32 { - return sqrt(v.x*v.x + v.y*v.y + v.z*v.z); -} -fn Cross3(v: vec3,o: vec3) -> vec3 { - return vec3(v.y*o.z-v.z*o.y, v.z*o.x-v.x*o.z, v.x*o.y-v.y*o.x); -} - -//////// import: "step.go" - -//////// import: "step_body.go" -fn StepBodyJointDeltas(i: u32) { //gosl:kernel - var params = Params[0]; - var di = i32(i); - if (di >= params.DynamicsN) { - return; - } - var bi = DynamicBody(di); - var invMass = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyInvMass))]; - if (invMass == 0) { - return; // no updates - } - var np = BodyJoints[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(di), u32(0), u32(0))]; - var nc = BodyJoints[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(di), u32(1), u32(0))]; - var linDel = vec3(0, 0, 0); - var angDel = vec3(0, 0, 0); - for (var i = i32(1); - i <= np; i++) { - var ji = BodyJoints[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(di), u32(0), u32(i))]; - var d = JointPDelta(ji); - linDel = linDel+(d); - var a = JointPAngDelta(ji); - angDel = angDel+(a); - } - for (var i = i32(1); - i <= nc; i++) { - var ji = BodyJoints[Index3D(TensorStrides[30], TensorStrides[31], TensorStrides[32], u32(di), u32(1), u32(i))]; - var d = JointCDelta(ji); - linDel = linDel+(d); - var a = JointCAngDelta(ji); - angDel = angDel+(a); - } - StepBodyDeltas(di, bi, false, f32(f32(0)), linDel, angDel); - Params[0] = params; -} -fn StepBodyDeltas(di: i32,bi: i32, contacts: bool, cWt: f32, linDel: vec3,angDel: vec3) { - var params = Params[0]; - var invMass = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyInvMass))]; - var inertia = BodyInertia(bi); - var invInertia = BodyInvInertia(bi); - var r0 = DynamicPos(di, params.Next); - var q0 = DynamicQuat(di, params.Next); - var v0 = DynamicDelta(di, params.Next); - var w0 = DynamicAngDelta(di, params.Next); - var weight = f32(1.0); - if (contacts && params.ContactWeighting == 1) { - if (cWt > 0) { - weight = 1.0 / cWt; - } - } - var dp = linDel*(invMass * weight); - var dq = angDel*(weight); - var wb = MulQuatVectorInverse(q0, w0); - var dwb = invInertia*(MulQuatVectorInverse(q0, dq)); - var tb = Cross3(dwb, inertia*(wb+(dwb)))+(Cross3(wb, inertia*(dwb))); - var dw1 = MulQuatVector(q0, dwb-(invInertia*(tb)*(params.Dt))); - var q1 = q0+(MulQuats(vec4(dw1.x, dw1.y, dw1.z, 0), q0)*(0.5 * params.Dt)); - q1 = QuatNormalize(q1); - var com = BodyCom(bi); - var pcom = MulQuatVector(q0, com)+(r0); - var p1 = pcom+(dp*(params.Dt)); - p1 = p1-(MulQuatVector(q1, com)); - var v1 = v0+(dp); - var w1 = w0+(dw1); - if (Length3(v1) < 1e-4) { - v1 = vec3(0, 0, 0); - } - if (Length3(w1) < 1e-4) { - w1 = vec3(0, 0, 0); - } - SetDynamicPos(di, params.Next, p1); - SetDynamicQuat(di, params.Next, q1); - SetDynamicDelta(di, params.Next, v1); - SetDynamicAngDelta(di, params.Next, w1); - Params[0] = params; -} - -//////// import: "step_joint.go" \ No newline at end of file diff --git a/physics/shaders/StepIntegrateBodies.wgsl b/physics/shaders/StepIntegrateBodies.wgsl index d2b0e0d9..0923e40d 100644 --- a/physics/shaders/StepIntegrateBodies.wgsl +++ b/physics/shaders/StepIntegrateBodies.wgsl @@ -172,26 +172,26 @@ const DynAngDeltaX: DynamicVars = 29; const DynAngDeltaY: DynamicVars = 30; const DynAngDeltaZ: DynamicVars = 31; const DynContactWeight: DynamicVars = 32; -fn DynamicBody(idx: i32) -> i32 { return i32(bitcast(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(0), u32(DynBody))])); } -fn DynamicPos(idx: i32,cni: i32) -> vec3 { return vec3(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosZ))]); } -fn SetDynamicPos(idx: i32,cni: i32, pos: vec3) { Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosX))] = pos.x;; Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosY))] = pos.y;; Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosZ))] = pos.z; } -fn DynamicQuat(idx: i32,cni: i32) -> vec4 { return vec4(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatZ))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatW))]); } -fn SetDynamicQuat(idx: i32,cni: i32, rot: vec4) { Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatX))] = rot.x;; Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatY))] = rot.y;; Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatZ))] = rot.z;; Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatW))] = rot.w; } -fn DynamicForce(idx: i32,cni: i32) -> vec3 { return vec3(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynForceX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynForceY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynForceZ))]); } -fn DynamicTorque(idx: i32,cni: i32) -> vec3 { return vec3(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynTorqueX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynTorqueY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynTorqueZ))]); } -fn DynamicDelta(idx: i32,cni: i32) -> vec3 { return vec3(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaZ))]); } -fn SetDynamicDelta(idx: i32,cni: i32, delta: vec3) { Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaX))] = delta.x;; Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaY))] = delta.y;; Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaZ))] = delta.z; } -fn DynamicAngDelta(idx: i32,cni: i32) -> vec3 { return vec3(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaZ))]); } -fn SetDynamicAngDelta(idx: i32,cni: i32, angDelta: vec3) { Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaX))] = angDelta.x;; Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaY))] = angDelta.y;; Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaZ))] = angDelta.z; } +fn DynamicBody(idx: i32) -> i32 { return i32(bitcast(Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(0), u32(DynBody))])); } +fn DynamicPos(idx: i32,cni: i32) -> vec3 { return vec3(Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynPosX))], Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynPosY))], Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynPosZ))]); } +fn SetDynamicPos(idx: i32,cni: i32, pos: vec3) { Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynPosX))] = pos.x;; Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynPosY))] = pos.y;; Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynPosZ))] = pos.z; } +fn DynamicQuat(idx: i32,cni: i32) -> vec4 { return vec4(Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynQuatX))], Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynQuatY))], Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynQuatZ))], Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynQuatW))]); } +fn SetDynamicQuat(idx: i32,cni: i32, rot: vec4) { Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynQuatX))] = rot.x;; Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynQuatY))] = rot.y;; Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynQuatZ))] = rot.z;; Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynQuatW))] = rot.w; } +fn DynamicForce(idx: i32,cni: i32) -> vec3 { return vec3(Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynForceX))], Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynForceY))], Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynForceZ))]); } +fn DynamicTorque(idx: i32,cni: i32) -> vec3 { return vec3(Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynTorqueX))], Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynTorqueY))], Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynTorqueZ))]); } +fn DynamicDelta(idx: i32,cni: i32) -> vec3 { return vec3(Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynDeltaX))], Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynDeltaY))], Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynDeltaZ))]); } +fn SetDynamicDelta(idx: i32,cni: i32, delta: vec3) { Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynDeltaX))] = delta.x;; Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynDeltaY))] = delta.y;; Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynDeltaZ))] = delta.z; } +fn DynamicAngDelta(idx: i32,cni: i32) -> vec3 { return vec3(Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynAngDeltaX))], Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynAngDeltaY))], Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynAngDeltaZ))]); } +fn SetDynamicAngDelta(idx: i32,cni: i32, angDelta: vec3) { Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynAngDeltaX))] = angDelta.x;; Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynAngDeltaY))] = angDelta.y;; Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynAngDeltaZ))] = angDelta.z; } //////// import: "enumgen.go" const BodyVarsN: BodyVars = 43; const ContactVarsN: ContactVars = 33; const JointControlVarsN: JointControlVars = 5; const DynamicVarsN: DynamicVars = 33; -const GPUVarsN: GPUVars = 12; +const GPUVarsN: GPUVars = 13; const JointTypesN: JointTypes = 8; -const JointVarsN: JointVars = 50; +const JointVarsN: JointVars = 39; const JointDoFVarsN: JointDoFVars = 5; const ShapesN: Shapes = 6; @@ -209,54 +209,43 @@ const PlaneXZ: JointTypes = 7; alias JointVars = i32; //enums:enum const JointType: JointVars = 0; const JointEnabled: JointVars = 1; -const JointParent: JointVars = 2; -const JointChild: JointVars = 3; -const JointPPosX: JointVars = 4; -const JointPPosY: JointVars = 5; -const JointPPosZ: JointVars = 6; -const JointPQuatX: JointVars = 7; -const JointPQuatY: JointVars = 8; -const JointPQuatZ: JointVars = 9; -const JointPQuatW: JointVars = 10; -const JointCPosX: JointVars = 11; -const JointCPosY: JointVars = 12; -const JointCPosZ: JointVars = 13; -const JointCQuatX: JointVars = 14; -const JointCQuatY: JointVars = 15; -const JointCQuatZ: JointVars = 16; -const JointCQuatW: JointVars = 17; -const JointLinearDoFN: JointVars = 18; -const JointAngularDoFN: JointVars = 19; -const JointDoF1: JointVars = 20; -const JointDoF2: JointVars = 21; -const JointDoF3: JointVars = 22; -const JointDoF4: JointVars = 23; -const JointDoF5: JointVars = 24; -const JointDoF6: JointVars = 25; -const JointPForceX: JointVars = 26; -const JointPForceY: JointVars = 27; -const JointPForceZ: JointVars = 28; -const JointPTorqueX: JointVars = 29; -const JointPTorqueY: JointVars = 30; -const JointPTorqueZ: JointVars = 31; -const JointCForceX: JointVars = 32; -const JointCForceY: JointVars = 33; -const JointCForceZ: JointVars = 34; -const JointCTorqueX: JointVars = 35; -const JointCTorqueY: JointVars = 36; -const JointCTorqueZ: JointVars = 37; -const JointPDeltaX: JointVars = 38; -const JointPDeltaY: JointVars = 39; -const JointPDeltaZ: JointVars = 40; -const JointPAngDeltaX: JointVars = 41; -const JointPAngDeltaY: JointVars = 42; -const JointPAngDeltaZ: JointVars = 43; -const JointCDeltaX: JointVars = 44; -const JointCDeltaY: JointVars = 45; -const JointCDeltaZ: JointVars = 46; -const JointCAngDeltaX: JointVars = 47; -const JointCAngDeltaY: JointVars = 48; -const JointCAngDeltaZ: JointVars = 49; +const JointParentFixed: JointVars = 2; +const JointParent: JointVars = 3; +const JointChild: JointVars = 4; +const JointPPosX: JointVars = 5; +const JointPPosY: JointVars = 6; +const JointPPosZ: JointVars = 7; +const JointPQuatX: JointVars = 8; +const JointPQuatY: JointVars = 9; +const JointPQuatZ: JointVars = 10; +const JointPQuatW: JointVars = 11; +const JointCPosX: JointVars = 12; +const JointCPosY: JointVars = 13; +const JointCPosZ: JointVars = 14; +const JointCQuatX: JointVars = 15; +const JointCQuatY: JointVars = 16; +const JointCQuatZ: JointVars = 17; +const JointCQuatW: JointVars = 18; +const JointLinearDoFN: JointVars = 19; +const JointAngularDoFN: JointVars = 20; +const JointDoF1: JointVars = 21; +const JointDoF2: JointVars = 22; +const JointDoF3: JointVars = 23; +const JointDoF4: JointVars = 24; +const JointDoF5: JointVars = 25; +const JointDoF6: JointVars = 26; +const JointPForceX: JointVars = 27; +const JointPForceY: JointVars = 28; +const JointPForceZ: JointVars = 29; +const JointPTorqueX: JointVars = 30; +const JointPTorqueY: JointVars = 31; +const JointPTorqueZ: JointVars = 32; +const JointCForceX: JointVars = 33; +const JointCForceY: JointVars = 34; +const JointCForceZ: JointVars = 35; +const JointCTorqueX: JointVars = 36; +const JointCTorqueY: JointVars = 37; +const JointCTorqueZ: JointVars = 38; alias JointDoFVars = i32; //enums:enum const JointAxisX: JointDoFVars = 0; const JointAxisY: JointDoFVars = 1; @@ -286,10 +275,14 @@ struct PhysParams { Next: i32, BodiesN: i32, DynamicsN: i32, + ObjectsN: i32, + MaxObjectJoints: i32, JointsN: i32, JointDoFsN: i32, BodyJointsMax: i32, BodyCollidePairsN: i32, + pad: i32, + pad1: i32, Gravity: vec4, } diff --git a/physics/shaders/StepJointForces.wgsl b/physics/shaders/StepJointForces.wgsl index c49cfcc7..e536e986 100644 --- a/physics/shaders/StepJointForces.wgsl +++ b/physics/shaders/StepJointForces.wgsl @@ -9,9 +9,9 @@ var Params: array; // // Bodies are the rigid body elements (dynamic and static), // specifying the constant, non-dynamic properties, // which is initial state for dynamics. // [body][BodyVarsN] @group(1) @binding(0) var Bodies: array; -@group(1) @binding(1) +@group(1) @binding(3) var Joints: array; -@group(1) @binding(2) +@group(1) @binding(4) var JointDoFs: array; // // Dynamics are the dynamic rigid body elements: these actually move. // Two alternating states are used: Params.Cur and Params.Next. // [dyn body][cur/next][DynamicVarsN] @group(2) @binding(0) @@ -133,7 +133,7 @@ const JointTargetStiff: JointControlVars = 2; const JointTargetVel: JointControlVars = 3; const JointTargetDamp: JointControlVars = 4; fn JointControl(idx: i32,dof: i32, vr: JointControlVars) -> f32 { - return JointControls[Index2D(TensorStrides[100], TensorStrides[101], u32(JointDoFIndex(idx, dof)), u32(vr))]; + return JointControls[Index2D(TensorStrides[110], TensorStrides[111], u32(JointDoFIndex(idx, dof)), u32(vr))]; } //////// import: "dynamics.go" @@ -171,18 +171,18 @@ const DynAngDeltaX: DynamicVars = 29; const DynAngDeltaY: DynamicVars = 30; const DynAngDeltaZ: DynamicVars = 31; const DynContactWeight: DynamicVars = 32; -fn DynamicBody(idx: i32) -> i32 { return i32(bitcast(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(0), u32(DynBody))])); } -fn DynamicPos(idx: i32,cni: i32) -> vec3 { return vec3(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosZ))]); } -fn DynamicQuat(idx: i32,cni: i32) -> vec4 { return vec4(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatZ))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatW))]); } +fn DynamicBody(idx: i32) -> i32 { return i32(bitcast(Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(0), u32(DynBody))])); } +fn DynamicPos(idx: i32,cni: i32) -> vec3 { return vec3(Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynPosX))], Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynPosY))], Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynPosZ))]); } +fn DynamicQuat(idx: i32,cni: i32) -> vec4 { return vec4(Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynQuatX))], Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynQuatY))], Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynQuatZ))], Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynQuatW))]); } //////// import: "enumgen.go" const BodyVarsN: BodyVars = 43; const ContactVarsN: ContactVars = 33; const JointControlVarsN: JointControlVars = 5; const DynamicVarsN: DynamicVars = 33; -const GPUVarsN: GPUVars = 12; +const GPUVarsN: GPUVars = 13; const JointTypesN: JointTypes = 8; -const JointVarsN: JointVars = 50; +const JointVarsN: JointVars = 39; const JointDoFVarsN: JointDoFVars = 5; const ShapesN: Shapes = 6; @@ -200,101 +200,91 @@ const PlaneXZ: JointTypes = 7; alias JointVars = i32; //enums:enum const JointType: JointVars = 0; const JointEnabled: JointVars = 1; -const JointParent: JointVars = 2; -const JointChild: JointVars = 3; -const JointPPosX: JointVars = 4; -const JointPPosY: JointVars = 5; -const JointPPosZ: JointVars = 6; -const JointPQuatX: JointVars = 7; -const JointPQuatY: JointVars = 8; -const JointPQuatZ: JointVars = 9; -const JointPQuatW: JointVars = 10; -const JointCPosX: JointVars = 11; -const JointCPosY: JointVars = 12; -const JointCPosZ: JointVars = 13; -const JointCQuatX: JointVars = 14; -const JointCQuatY: JointVars = 15; -const JointCQuatZ: JointVars = 16; -const JointCQuatW: JointVars = 17; -const JointLinearDoFN: JointVars = 18; -const JointAngularDoFN: JointVars = 19; -const JointDoF1: JointVars = 20; -const JointDoF2: JointVars = 21; -const JointDoF3: JointVars = 22; -const JointDoF4: JointVars = 23; -const JointDoF5: JointVars = 24; -const JointDoF6: JointVars = 25; -const JointPForceX: JointVars = 26; -const JointPForceY: JointVars = 27; -const JointPForceZ: JointVars = 28; -const JointPTorqueX: JointVars = 29; -const JointPTorqueY: JointVars = 30; -const JointPTorqueZ: JointVars = 31; -const JointCForceX: JointVars = 32; -const JointCForceY: JointVars = 33; -const JointCForceZ: JointVars = 34; -const JointCTorqueX: JointVars = 35; -const JointCTorqueY: JointVars = 36; -const JointCTorqueZ: JointVars = 37; -const JointPDeltaX: JointVars = 38; -const JointPDeltaY: JointVars = 39; -const JointPDeltaZ: JointVars = 40; -const JointPAngDeltaX: JointVars = 41; -const JointPAngDeltaY: JointVars = 42; -const JointPAngDeltaZ: JointVars = 43; -const JointCDeltaX: JointVars = 44; -const JointCDeltaY: JointVars = 45; -const JointCDeltaZ: JointVars = 46; -const JointCAngDeltaX: JointVars = 47; -const JointCAngDeltaY: JointVars = 48; -const JointCAngDeltaZ: JointVars = 49; +const JointParentFixed: JointVars = 2; +const JointParent: JointVars = 3; +const JointChild: JointVars = 4; +const JointPPosX: JointVars = 5; +const JointPPosY: JointVars = 6; +const JointPPosZ: JointVars = 7; +const JointPQuatX: JointVars = 8; +const JointPQuatY: JointVars = 9; +const JointPQuatZ: JointVars = 10; +const JointPQuatW: JointVars = 11; +const JointCPosX: JointVars = 12; +const JointCPosY: JointVars = 13; +const JointCPosZ: JointVars = 14; +const JointCQuatX: JointVars = 15; +const JointCQuatY: JointVars = 16; +const JointCQuatZ: JointVars = 17; +const JointCQuatW: JointVars = 18; +const JointLinearDoFN: JointVars = 19; +const JointAngularDoFN: JointVars = 20; +const JointDoF1: JointVars = 21; +const JointDoF2: JointVars = 22; +const JointDoF3: JointVars = 23; +const JointDoF4: JointVars = 24; +const JointDoF5: JointVars = 25; +const JointDoF6: JointVars = 26; +const JointPForceX: JointVars = 27; +const JointPForceY: JointVars = 28; +const JointPForceZ: JointVars = 29; +const JointPTorqueX: JointVars = 30; +const JointPTorqueY: JointVars = 31; +const JointPTorqueZ: JointVars = 32; +const JointCForceX: JointVars = 33; +const JointCForceY: JointVars = 34; +const JointCForceZ: JointVars = 35; +const JointCTorqueX: JointVars = 36; +const JointCTorqueY: JointVars = 37; +const JointCTorqueZ: JointVars = 38; fn GetJointType(idx: i32) -> JointTypes { - return JointTypes(bitcast(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointType))])); + return JointTypes(bitcast(Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointType))])); } fn GetJointEnabled(idx: i32) -> bool { - var je = bitcast(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointEnabled))]); + var je = bitcast(Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointEnabled))]); return je != 0; } fn JointParentIndex(idx: i32) -> i32 { - return i32(bitcast(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointParent))])); + return i32(bitcast(Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointParent))])); } fn JointChildIndex(idx: i32) -> i32 { - return i32(bitcast(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointChild))])); + return i32(bitcast(Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointChild))])); } fn GetJointLinearDoFN(idx: i32) -> i32 { - return i32(bitcast(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointLinearDoFN))])); + return i32(bitcast(Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointLinearDoFN))])); } fn GetJointAngularDoFN(idx: i32) -> i32 { - return i32(bitcast(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointAngularDoFN))])); + return i32(bitcast(Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointAngularDoFN))])); } fn JointDoFIndex(idx: i32,dof: i32) -> i32 { - return i32(bitcast(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(i32(JointDoF1) + dof))])); + return i32(bitcast(Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(i32(JointDoF1) + dof))])); } fn JointPPos(idx: i32) -> vec3 { - return vec3(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPPosX))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPPosY))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPPosZ))]); + return vec3(Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointPPosX))], Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointPPosY))], Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointPPosZ))]); } fn JointPQuat(idx: i32) -> vec4 { - return vec4(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPQuatX))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPQuatY))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPQuatZ))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPQuatW))]); + return vec4(Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointPQuatX))], Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointPQuatY))], Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointPQuatZ))], Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointPQuatW))]); } fn SetJointPForce(idx: i32, f: vec3) { - Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPForceX))] = f.x; - Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPForceY))] = f.y; - Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPForceZ))] = f.z; + Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointPForceX))] = f.x; + Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointPForceY))] = f.y; + Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointPForceZ))] = f.z; } fn SetJointPTorque(idx: i32, t: vec3) { - Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPTorqueX))] = t.x; - Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPTorqueY))] = t.y; - Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPTorqueZ))] = t.z; + Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointPTorqueX))] = t.x; + Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointPTorqueY))] = t.y; + Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointPTorqueZ))] = t.z; } fn SetJointCForce(idx: i32, f: vec3) { - Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCForceX))] = f.x; - Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCForceY))] = f.y; - Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCForceZ))] = f.z; + Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointCForceX))] = f.x; + Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointCForceY))] = f.y; + Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointCForceZ))] = f.z; } fn SetJointCTorque(idx: i32, t: vec3) { - Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCTorqueX))] = t.x; - Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCTorqueY))] = t.y; - Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCTorqueZ))] = t.z; + Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointCTorqueX))] = t.x; + Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointCTorqueY))] = t.y; + Joints[Index2D(TensorStrides[30], TensorStrides[31], + u32(idx), u32(JointCTorqueZ))] = t.z; } alias JointDoFVars = i32; //enums:enum const JointAxisX: JointDoFVars = 0; @@ -303,7 +293,7 @@ const JointAxisZ: JointDoFVars = 2; const JointLimitLower: JointDoFVars = 3; const JointLimitUpper: JointDoFVars = 4; fn JointAxisDoF(didx: i32) -> vec3 { - return vec3(JointDoFs[Index2D(TensorStrides[20], TensorStrides[21], u32(didx), u32(JointAxisX))], JointDoFs[Index2D(TensorStrides[20], TensorStrides[21], u32(didx), u32(JointAxisY))], JointDoFs[Index2D(TensorStrides[20], TensorStrides[21], u32(didx), u32(JointAxisZ))]); + return vec3(JointDoFs[Index2D(TensorStrides[40], TensorStrides[41], u32(didx), u32(JointAxisX))], JointDoFs[Index2D(TensorStrides[40], TensorStrides[41], u32(didx), u32(JointAxisY))], JointDoFs[Index2D(TensorStrides[40], TensorStrides[41], u32(didx), u32(JointAxisZ))]); } fn JointAxis(idx: i32,dof: i32) -> vec3 { return JointAxisDoF(JointDoFIndex(idx, dof)); @@ -331,10 +321,14 @@ struct PhysParams { Next: i32, BodiesN: i32, DynamicsN: i32, + ObjectsN: i32, + MaxObjectJoints: i32, JointsN: i32, JointDoFsN: i32, BodyJointsMax: i32, BodyCollidePairsN: i32, + pad: i32, + pad1: i32, Gravity: vec4, } diff --git a/physics/shaders/StepSolveJoints.wgsl b/physics/shaders/StepSolveJoints.wgsl index a56ed959..ec5fb496 100644 --- a/physics/shaders/StepSolveJoints.wgsl +++ b/physics/shaders/StepSolveJoints.wgsl @@ -10,8 +10,10 @@ var Params: array; @group(1) @binding(0) var Bodies: array; @group(1) @binding(1) +var Objects: array; +@group(1) @binding(3) var Joints: array; -@group(1) @binding(2) +@group(1) @binding(4) var JointDoFs: array; // // Dynamics are the dynamic rigid body elements: these actually move. // Two alternating states are used: Params.Cur and Params.Next. // [dyn body][cur/next][DynamicVarsN] @group(2) @binding(0) @@ -87,6 +89,11 @@ const BodyRadius: BodyVars = 42; fn BodyCom(idx: i32) -> vec3 { return vec3(Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComX))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComY))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyComZ))]); } +fn BodyInertia(idx: i32) -> mat3x3f { + return mat3x3f(Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInertiaXX))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInertiaYX))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInertiaZX))], + Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInertiaXY))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInertiaYY))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInertiaZY))], + Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInertiaXZ))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInertiaYZ))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInertiaZZ))]); +} fn BodyInvInertia(idx: i32) -> mat3x3f { return mat3x3f(Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInvInertiaXX))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInvInertiaYX))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInvInertiaZX))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInvInertiaXY))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInvInertiaYY))], Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(idx), u32(BodyInvInertiaZY))], @@ -138,7 +145,7 @@ const JointTargetStiff: JointControlVars = 2; const JointTargetVel: JointControlVars = 3; const JointTargetDamp: JointControlVars = 4; fn JointControl(idx: i32,dof: i32, vr: JointControlVars) -> f32 { - return JointControls[Index2D(TensorStrides[100], TensorStrides[101], u32(JointDoFIndex(idx, dof)), u32(vr))]; + return JointControls[Index2D(TensorStrides[110], TensorStrides[111], u32(JointDoFIndex(idx, dof)), u32(vr))]; } //////// import: "dynamics.go" @@ -176,14 +183,26 @@ const DynAngDeltaX: DynamicVars = 29; const DynAngDeltaY: DynamicVars = 30; const DynAngDeltaZ: DynamicVars = 31; const DynContactWeight: DynamicVars = 32; -fn DynamicBody(idx: i32) -> i32 { return i32(bitcast(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(0), u32(DynBody))])); } -fn DynamicPos(idx: i32,cni: i32) -> vec3 { return vec3(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynPosZ))]); } -fn DynamicQuat(idx: i32,cni: i32) -> vec4 { return vec4(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatZ))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynQuatW))]); } +fn DynamicBody(idx: i32) -> i32 { return i32(bitcast(Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(0), u32(DynBody))])); } +fn DynamicPos(idx: i32,cni: i32) -> vec3 { return vec3(Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynPosX))], Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynPosY))], Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynPosZ))]); } +fn SetDynamicPos(idx: i32,cni: i32, pos: vec3) { Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynPosX))] = pos.x;; Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynPosY))] = pos.y;; Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynPosZ))] = pos.z; } +fn DynamicQuat(idx: i32,cni: i32) -> vec4 { return vec4(Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynQuatX))], Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynQuatY))], Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynQuatZ))], Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynQuatW))]); } +fn SetDynamicQuat(idx: i32,cni: i32, rot: vec4) { Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynQuatX))] = rot.x;; Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynQuatY))] = rot.y;; Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynQuatZ))] = rot.z;; Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynQuatW))] = rot.w; } fn DynamicDelta(idx: i32,cni: i32) -> vec3 { - return vec3(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynDeltaZ))]); + return vec3(Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynDeltaX))], Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynDeltaY))], Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynDeltaZ))]); +} +fn SetDynamicDelta(idx: i32,cni: i32, delta: vec3) { + Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynDeltaX))] = delta.x; + Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynDeltaY))] = delta.y; + Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynDeltaZ))] = delta.z; } fn DynamicAngDelta(idx: i32,cni: i32) -> vec3 { - return vec3(Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaX))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaY))], Dynamics[Index3D(TensorStrides[50], TensorStrides[51], TensorStrides[52], u32(idx), u32(cni), u32(DynAngDeltaZ))]); + return vec3(Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynAngDeltaX))], Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynAngDeltaY))], Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynAngDeltaZ))]); +} +fn SetDynamicAngDelta(idx: i32,cni: i32, angDelta: vec3) { + Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynAngDeltaX))] = angDelta.x; + Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynAngDeltaY))] = angDelta.y; + Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynAngDeltaZ))] = angDelta.z; } //////// import: "enumgen.go" @@ -191,9 +210,9 @@ const BodyVarsN: BodyVars = 43; const ContactVarsN: ContactVars = 33; const JointControlVarsN: JointControlVars = 5; const DynamicVarsN: DynamicVars = 33; -const GPUVarsN: GPUVars = 12; +const GPUVarsN: GPUVars = 13; const JointTypesN: JointTypes = 8; -const JointVarsN: JointVars = 50; +const JointVarsN: JointVars = 39; const JointDoFVarsN: JointDoFVars = 5; const ShapesN: Shapes = 6; @@ -211,107 +230,80 @@ const PlaneXZ: JointTypes = 7; alias JointVars = i32; //enums:enum const JointType: JointVars = 0; const JointEnabled: JointVars = 1; -const JointParent: JointVars = 2; -const JointChild: JointVars = 3; -const JointPPosX: JointVars = 4; -const JointPPosY: JointVars = 5; -const JointPPosZ: JointVars = 6; -const JointPQuatX: JointVars = 7; -const JointPQuatY: JointVars = 8; -const JointPQuatZ: JointVars = 9; -const JointPQuatW: JointVars = 10; -const JointCPosX: JointVars = 11; -const JointCPosY: JointVars = 12; -const JointCPosZ: JointVars = 13; -const JointCQuatX: JointVars = 14; -const JointCQuatY: JointVars = 15; -const JointCQuatZ: JointVars = 16; -const JointCQuatW: JointVars = 17; -const JointLinearDoFN: JointVars = 18; -const JointAngularDoFN: JointVars = 19; -const JointDoF1: JointVars = 20; -const JointDoF2: JointVars = 21; -const JointDoF3: JointVars = 22; -const JointDoF4: JointVars = 23; -const JointDoF5: JointVars = 24; -const JointDoF6: JointVars = 25; -const JointPForceX: JointVars = 26; -const JointPForceY: JointVars = 27; -const JointPForceZ: JointVars = 28; -const JointPTorqueX: JointVars = 29; -const JointPTorqueY: JointVars = 30; -const JointPTorqueZ: JointVars = 31; -const JointCForceX: JointVars = 32; -const JointCForceY: JointVars = 33; -const JointCForceZ: JointVars = 34; -const JointCTorqueX: JointVars = 35; -const JointCTorqueY: JointVars = 36; -const JointCTorqueZ: JointVars = 37; -const JointPDeltaX: JointVars = 38; -const JointPDeltaY: JointVars = 39; -const JointPDeltaZ: JointVars = 40; -const JointPAngDeltaX: JointVars = 41; -const JointPAngDeltaY: JointVars = 42; -const JointPAngDeltaZ: JointVars = 43; -const JointCDeltaX: JointVars = 44; -const JointCDeltaY: JointVars = 45; -const JointCDeltaZ: JointVars = 46; -const JointCAngDeltaX: JointVars = 47; -const JointCAngDeltaY: JointVars = 48; -const JointCAngDeltaZ: JointVars = 49; +const JointParentFixed: JointVars = 2; +const JointParent: JointVars = 3; +const JointChild: JointVars = 4; +const JointPPosX: JointVars = 5; +const JointPPosY: JointVars = 6; +const JointPPosZ: JointVars = 7; +const JointPQuatX: JointVars = 8; +const JointPQuatY: JointVars = 9; +const JointPQuatZ: JointVars = 10; +const JointPQuatW: JointVars = 11; +const JointCPosX: JointVars = 12; +const JointCPosY: JointVars = 13; +const JointCPosZ: JointVars = 14; +const JointCQuatX: JointVars = 15; +const JointCQuatY: JointVars = 16; +const JointCQuatZ: JointVars = 17; +const JointCQuatW: JointVars = 18; +const JointLinearDoFN: JointVars = 19; +const JointAngularDoFN: JointVars = 20; +const JointDoF1: JointVars = 21; +const JointDoF2: JointVars = 22; +const JointDoF3: JointVars = 23; +const JointDoF4: JointVars = 24; +const JointDoF5: JointVars = 25; +const JointDoF6: JointVars = 26; +const JointPForceX: JointVars = 27; +const JointPForceY: JointVars = 28; +const JointPForceZ: JointVars = 29; +const JointPTorqueX: JointVars = 30; +const JointPTorqueY: JointVars = 31; +const JointPTorqueZ: JointVars = 32; +const JointCForceX: JointVars = 33; +const JointCForceY: JointVars = 34; +const JointCForceZ: JointVars = 35; +const JointCTorqueX: JointVars = 36; +const JointCTorqueY: JointVars = 37; +const JointCTorqueZ: JointVars = 38; fn GetJointType(idx: i32) -> JointTypes { - return JointTypes(bitcast(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointType))])); + return JointTypes(bitcast(Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointType))])); } fn GetJointEnabled(idx: i32) -> bool { - var je = bitcast(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointEnabled))]); + var je = bitcast(Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointEnabled))]); +return je != 0; +} +fn GetJointParentFixed(idx: i32) -> bool { + var je = bitcast(Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointParentFixed))]); return je != 0; } fn JointParentIndex(idx: i32) -> i32 { - return i32(bitcast(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointParent))])); + return i32(bitcast(Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointParent))])); } fn JointChildIndex(idx: i32) -> i32 { - return i32(bitcast(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointChild))])); + return i32(bitcast(Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointChild))])); } fn GetJointLinearDoFN(idx: i32) -> i32 { - return i32(bitcast(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointLinearDoFN))])); + return i32(bitcast(Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointLinearDoFN))])); } fn GetJointAngularDoFN(idx: i32) -> i32 { - return i32(bitcast(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointAngularDoFN))])); + return i32(bitcast(Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointAngularDoFN))])); } fn JointDoFIndex(idx: i32,dof: i32) -> i32 { - return i32(bitcast(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(i32(JointDoF1) + dof))])); + return i32(bitcast(Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(i32(JointDoF1) + dof))])); } fn JointPPos(idx: i32) -> vec3 { - return vec3(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPPosX))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPPosY))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPPosZ))]); + return vec3(Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointPPosX))], Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointPPosY))], Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointPPosZ))]); } fn JointPQuat(idx: i32) -> vec4 { - return vec4(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPQuatX))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPQuatY))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPQuatZ))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPQuatW))]); + return vec4(Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointPQuatX))], Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointPQuatY))], Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointPQuatZ))], Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointPQuatW))]); } fn JointCPos(idx: i32) -> vec3 { - return vec3(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCPosX))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCPosY))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCPosZ))]); + return vec3(Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointCPosX))], Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointCPosY))], Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointCPosZ))]); } fn JointCQuat(idx: i32) -> vec4 { - return vec4(Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCQuatX))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCQuatY))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCQuatZ))], Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCQuatW))]); -} -fn SetJointPDelta(idx: i32, f: vec3) { - Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPDeltaX))] = f.x; - Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPDeltaY))] = f.y; - Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPDeltaZ))] = f.z; -} -fn SetJointPAngDelta(idx: i32, t: vec3) { - Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPAngDeltaX))] = t.x; - Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPAngDeltaY))] = t.y; - Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointPAngDeltaZ))] = t.z; -} -fn SetJointCDelta(idx: i32, f: vec3) { - Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCDeltaX))] = f.x; - Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCDeltaY))] = f.y; - Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCDeltaZ))] = f.z; -} -fn SetJointCAngDelta(idx: i32, t: vec3) { - Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCAngDeltaX))] = t.x; - Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCAngDeltaY))] = t.y; - Joints[Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(JointCAngDeltaZ))] = t.z; + return vec4(Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointCQuatX))], Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointCQuatY))], Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointCQuatZ))], Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointCQuatW))]); } alias JointDoFVars = i32; //enums:enum const JointAxisX: JointDoFVars = 0; @@ -320,13 +312,13 @@ const JointAxisZ: JointDoFVars = 2; const JointLimitLower: JointDoFVars = 3; const JointLimitUpper: JointDoFVars = 4; fn JointAxisDoF(didx: i32) -> vec3 { - return vec3(JointDoFs[Index2D(TensorStrides[20], TensorStrides[21], u32(didx), u32(JointAxisX))], JointDoFs[Index2D(TensorStrides[20], TensorStrides[21], u32(didx), u32(JointAxisY))], JointDoFs[Index2D(TensorStrides[20], TensorStrides[21], u32(didx), u32(JointAxisZ))]); + return vec3(JointDoFs[Index2D(TensorStrides[40], TensorStrides[41], u32(didx), u32(JointAxisX))], JointDoFs[Index2D(TensorStrides[40], TensorStrides[41], u32(didx), u32(JointAxisY))], JointDoFs[Index2D(TensorStrides[40], TensorStrides[41], u32(didx), u32(JointAxisZ))]); } fn JointAxis(idx: i32,dof: i32) -> vec3 { return JointAxisDoF(JointDoFIndex(idx, dof)); } fn JointDoF(idx: i32,dof: i32, vr: JointDoFVars) -> f32 { - return JointDoFs[Index2D(TensorStrides[20], TensorStrides[21], u32(JointDoFIndex(idx, dof)), u32(vr))]; + return JointDoFs[Index2D(TensorStrides[40], TensorStrides[41], u32(JointDoFIndex(idx, dof)), u32(vr))]; } //////// import: "params.go" @@ -351,10 +343,14 @@ struct PhysParams { Next: i32, BodiesN: i32, DynamicsN: i32, + ObjectsN: i32, + MaxObjectJoints: i32, JointsN: i32, JointDoFsN: i32, BodyJointsMax: i32, BodyCollidePairsN: i32, + pad: i32, + pad1: i32, Gravity: vec4, } @@ -521,32 +517,81 @@ fn SetDim3(v: vec3, dim: i32, val: f32) -> vec3 { //////// import: "step.go" //////// import: "step_body.go" +fn StepBodyDeltas(di: i32,bi: i32, contacts: bool, cWt: f32, linDel: vec3,angDel: vec3) { + var params = Params[0]; + var invMass = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(bi), u32(BodyInvMass))]; + var inertia = BodyInertia(bi); + var invInertia = BodyInvInertia(bi); + var r0 = DynamicPos(di, params.Next); + var q0 = DynamicQuat(di, params.Next); + var v0 = DynamicDelta(di, params.Next); + var w0 = DynamicAngDelta(di, params.Next); + var weight = f32(1.0); + if (contacts && params.ContactWeighting == 1) { + if (cWt > 0) { + weight = 1.0 / cWt; + } + } + var dp = linDel*(invMass * weight); + var dq = angDel*(weight); + var wb = MulQuatVectorInverse(q0, w0); + var dwb = invInertia*(MulQuatVectorInverse(q0, dq)); + var tb = Cross3(dwb, inertia*(wb+(dwb)))+(Cross3(wb, inertia*(dwb))); + var dw1 = MulQuatVector(q0, dwb-(invInertia*(tb)*(params.Dt))); + var q1 = q0+(MulQuats(vec4(dw1.x, dw1.y, dw1.z, 0), q0)*(0.5 * params.Dt)); + q1 = QuatNormalize(q1); + var com = BodyCom(bi); + var pcom = MulQuatVector(q0, com)+(r0); + var p1 = pcom+(dp*(params.Dt)); + p1 = p1-(MulQuatVector(q1, com)); + var v1 = v0+(dp); + var w1 = w0+(dw1); + if (Length3(v1) < 1e-4) { + v1 = vec3(0, 0, 0); + } + if (Length3(w1) < 1e-4) { + w1 = vec3(0, 0, 0); + } + SetDynamicPos(di, params.Next, p1); + SetDynamicQuat(di, params.Next, q1); + SetDynamicDelta(di, params.Next, v1); + SetDynamicAngDelta(di, params.Next, w1); + Params[0] = params; +} //////// import: "step_joint.go" fn StepSolveJoints(i: u32) { //gosl:kernel var params = Params[0]; - var ji = i32(i); - if (ji >= params.JointsN) { + var oi = i32(i); + if (oi >= params.ObjectsN) { return; } - var zv = vec3(0, 0, 0); - SetJointPDelta(ji, zv); - SetJointCDelta(ji, zv); - SetJointPAngDelta(ji, zv); - SetJointCAngDelta(ji, zv); - var jt = GetJointType(ji); - if (jt == Free || !GetJointEnabled(ji)) { - return; + var n = Objects[Index2D(TensorStrides[10], TensorStrides[11], u32(oi), u32(0))]; + for (var i = i32(1); + i < n+1; i++) { + var ji = Objects[Index2D(TensorStrides[10], TensorStrides[11], u32(oi), u32(i))]; + var jt = GetJointType(ji); + if (jt == Free || !GetJointEnabled(ji)) { + continue; + } + StepSolveJointLinear(ji); + StepSolveJointAngular(ji); } + Params[0] = params; +} +fn StepSolveJointLinear(ji: i32) { + var params = Params[0]; + var jt = GetJointType(ji); var jPi = JointParentIndex(ji); var jPbi = i32(-1); + var parentFixed = true; if (jPi >= 0) { jPbi = DynamicBody(jPi); + parentFixed = GetJointParentFixed(ji); } var jCi = JointChildIndex(ji); var jCbi = DynamicBody(jCi); var jLinearN = GetJointLinearDoFN(ji); - var jAngularN = GetJointAngularDoFN(ji); var jPR = JointPPos(ji); var jPQ = JointPQuat(ji); var xwPR = jPR; // world xform, parent, pos @@ -568,6 +613,9 @@ fn StepSolveJoints(i: u32) { //gosl:kernel iInvP = BodyInvInertia(jPbi); vP = DynamicDelta(jPi, params.Next); wP = DynamicAngDelta(jPi, params.Next); + if (mInvP == 0) { + parentFixed = true; + } } var poseCR = DynamicPos(jCi, params.Next); var poseCQ = DynamicQuat(jCi, params.Next); @@ -617,7 +665,7 @@ fn StepSolveJoints(i: u32) { //gosl:kernel var angularP = Negate3(Cross3(dP, linearC)); var angularC = Cross3(dC, linearC); var derr = Dot3(linearP, vP) + Dot3(linearC, vC) + Dot3(angularP, wP) + Dot3(angularC, wC); - var lambdaIn = f32(0.0); + var lambdaIn = f32(0.0); // note: multiple iter is supposed to increment these var compliance = params.JointLinearComply; var ke = JointControl(ji, i32(i32(0)), JointTargetStiff); var kd = JointControl(ji, i32(i32(0)), JointTargetDamp); @@ -631,7 +679,7 @@ fn StepSolveJoints(i: u32) { //gosl:kernel linDeltaC = linDeltaC+(linearC*(dLambda * params.JointLinearRelax)); angDeltaC = angDeltaC+(angularC*(dLambda * params.JointAngularRelax)); } - } else { // compute joint target, stiffness, damping + } else { var axisLimitsD: vec3; var axisLimitsA: vec3; var axisTargetPosKeD: vec3; @@ -698,128 +746,190 @@ fn StepSolveJoints(i: u32) { //gosl:kernel var dLambda = PositionalCorrection(err, derrRel, posePQ, poseCQ, mInvP, mInvC, iInvP, iInvC, linearP, linearC, angularP, angularC, lambdaIn, compliance, damping, params.Dt); linDeltaP = linDeltaP+(linearP*(dLambda * params.JointLinearRelax)); - angDeltaP = angDeltaP+(angularP*(dLambda * params.JointAngularRelax)); linDeltaC = linDeltaC+(linearC*(dLambda * params.JointLinearRelax)); + angDeltaP = angDeltaP+(angularP*(dLambda * params.JointAngularRelax)); angDeltaC = angDeltaC+(angularC*(dLambda * params.JointAngularRelax)); } } } - if (jt == Fixed || jt == Prismatic || jt == Revolute || jt == Ball || jt == D6) { - var qP = xwPQ; - var qC = xwCQ; - if (QuatDot(qP, qC) < 0) { - qC = QuatMulScalar(qC, -1.0); - } - var relQ = MulQuats(QuatInverse(qP), qC); - var qtwist = QuatNormalize(vec4(relQ.x, 0.0, 0.0, relQ.w)); - var qswing = MulQuats(relQ, QuatInverse(qtwist)); - var s = sqrt(relQ.x*relQ.x + relQ.w*relQ.w); - if (s == 0) { - s = f32(1); + if (!parentFixed) { + StepBodyDeltas(jPi, jPbi, false, f32(f32(0)), linDeltaP, angDeltaP); + } + if (mInvC > 0) { + StepBodyDeltas(jCi, jCbi, false, f32(f32(0)), linDeltaC, angDeltaC); + } + Params[0] = params; +} +fn StepSolveJointAngular(ji: i32) { + var params = Params[0]; + var jPi = JointParentIndex(ji); + var jPbi = i32(-1); + var parentFixed = true; + if (jPi >= 0) { + jPbi = DynamicBody(jPi); + parentFixed = GetJointParentFixed(ji); + } + var jCi = JointChildIndex(ji); + var jCbi = DynamicBody(jCi); + var jLinearN = GetJointLinearDoFN(ji); + var jAngularN = GetJointAngularDoFN(ji); + var jPR = JointPPos(ji); + var jPQ = JointPQuat(ji); + var xwPR = jPR; // world xform, parent, pos + var xwPQ = jPQ; // quat + var mInvP = f32(0.0); + var iInvP = mat3x3f(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0); + var posePR = jPR; + var posePQ = jPQ; + var wP: vec3; + if (jPi >= 0) { + posePR = DynamicPos(jPi, params.Next); // now using next + posePQ = DynamicQuat(jPi, params.Next); + MulSpatialTransforms(posePR, posePQ, jPR, jPQ, &xwPR, &xwPQ); + mInvP = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(jPbi), u32(BodyInvMass))]; + iInvP = BodyInvInertia(jPbi); + wP = DynamicAngDelta(jPi, params.Next); + if (mInvP == 0) { + parentFixed = true; } - var invs = 1.0 / s; - var invscube = invs * invs * invs; - var err0 = 2.0 * asin(clamp(qtwist.x, -1.0, 1.0)); - var err1 = qswing.y; - var err2 = qswing.z; - var grad0 = vec4(invs-relQ.x*relQ.x*invscube, 0.0, 0.0, -(relQ.w*relQ.x)*invscube); - var grad1 = vec4( - -relQ.w*(relQ.w*relQ.z+relQ.x*relQ.y)*invscube, - relQ.w*invs, - -relQ.x*invs, - relQ.x*(relQ.w*relQ.z+relQ.x*relQ.y)*invscube); - var grad2 = vec4( - relQ.w*(relQ.w*relQ.y-relQ.x*relQ.z)*invscube, - relQ.x*invs, - relQ.w*invs, - relQ.x*(relQ.z*relQ.x-relQ.w*relQ.y)*invscube); - grad0 = QuatMulScalar(grad0, 2.0/abs(qtwist.w)); - var swing_sq = qswing.w * qswing.w; - var angularEps = f32(1.0e-4); - if (swing_sq+angularEps < 1.0) { - var d = sqrt(1.0 - qswing.w*qswing.w); - var theta = 2.0 * acos(clamp(qswing.w, -1.0, 1.0)); - var scale = theta / d; - err1 *= scale; - err2 *= scale; - grad1 = QuatMulScalar(grad1, scale); - grad2 = QuatMulScalar(grad2, scale); + } + var poseCR = DynamicPos(jCi, params.Next); + var poseCQ = DynamicQuat(jCi, params.Next); + var jCR = JointCPos(ji); + var jCQ = JointCQuat(ji); + var xwCR = jCR; + var xwCQ = jCQ; + MulSpatialTransforms(poseCR, poseCQ, jCR, jCQ, &xwCR, &xwCQ); + var mInvC = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(jCbi), u32(BodyInvMass))]; + var iInvC = BodyInvInertia(jCbi); + var wC = DynamicAngDelta(jCi, params.Next); + if (mInvP == 0.0 && mInvC == 0.0) { // connection between two immovable bodies + return; + } + var linDeltaP: vec3; + var angDeltaP: vec3; + var linDeltaC: vec3; + var angDeltaC: vec3; + var relPoseR = xwPR; + var relPoseQ = xwPQ; + SpatialTransformInverse(xwPR, xwPQ, &relPoseR, &relPoseQ); + MulSpatialTransforms(relPoseR, relPoseQ, xwCR, xwCQ, &relPoseR, &relPoseQ); + var qP = xwPQ; + var qC = xwCQ; + if (QuatDot(qP, qC) < 0) { + qC = QuatMulScalar(qC, -1.0); + } + var relQ = MulQuats(QuatInverse(qP), qC); + var qtwist = QuatNormalize(vec4(relQ.x, 0.0, 0.0, relQ.w)); + var qswing = MulQuats(relQ, QuatInverse(qtwist)); + var s = sqrt(relQ.x*relQ.x + relQ.w*relQ.w); + if (s == 0) { + s = f32(1); + } + var invs = 1.0 / s; + var invscube = invs * invs * invs; + var err0 = 2.0 * asin(clamp(qtwist.x, -1.0, 1.0)); + var err1 = qswing.y; + var err2 = qswing.z; + var grad0 = vec4(invs-relQ.x*relQ.x*invscube, 0.0, 0.0, -(relQ.w*relQ.x)*invscube); + var grad1 = vec4( + -relQ.w*(relQ.w*relQ.z+relQ.x*relQ.y)*invscube, + relQ.w*invs, + -relQ.x*invs, + relQ.x*(relQ.w*relQ.z+relQ.x*relQ.y)*invscube); + var grad2 = vec4( + relQ.w*(relQ.w*relQ.y-relQ.x*relQ.z)*invscube, + relQ.x*invs, + relQ.w*invs, + relQ.x*(relQ.z*relQ.x-relQ.w*relQ.y)*invscube); + grad0 = QuatMulScalar(grad0, 2.0/abs(qtwist.w)); + var swing_sq = qswing.w * qswing.w; + var angularEps = f32(1.0e-4); + if (swing_sq+angularEps < 1.0) { + var d = sqrt(1.0 - qswing.w*qswing.w); + var theta = 2.0 * acos(clamp(qswing.w, -1.0, 1.0)); + var scale = theta / d; + err1 *= scale; + err2 *= scale; + grad1 = QuatMulScalar(grad1, scale); + grad2 = QuatMulScalar(grad2, scale); + } + var errs = vec3(err0, err1, err2); + var gradX = vec3(grad0.x, grad1.x, grad2.x); + var gradY = vec3(grad0.y, grad1.y, grad2.y); + var gradZ = vec3(grad0.z, grad1.z, grad2.z); + var gradW = vec3(grad0.w, grad1.w, grad2.w); + var axisLimitsD: vec3; + var axisLimitsA: vec3; + var axisTargetPosKeD: vec3; + var axisTargetPosKeA: vec3; + var axisTargetVelKdD: vec3; + var axisTargetVelKdA: vec3; + for (var dof=0; dof 0.0) { // has position control + JointAxisTarget(axis, targetPos, ke, &axisTargetPosKeD, &axisTargetPosKeA); } - var errs = vec3(err0, err1, err2); - var gradX = vec3(grad0.x, grad1.x, grad2.x); - var gradY = vec3(grad0.y, grad1.y, grad2.y); - var gradZ = vec3(grad0.z, grad1.z, grad2.z); - var gradW = vec3(grad0.w, grad1.w, grad2.w); - var axisLimitsD: vec3; - var axisLimitsA: vec3; - var axisTargetPosKeD: vec3; - var axisTargetPosKeA: vec3; - var axisTargetVelKdD: vec3; - var axisTargetVelKdA: vec3; - for (var dof=0; dof 0.0) { // has position control - JointAxisTarget(axis, targetPos, ke, &axisTargetPosKeD, &axisTargetPosKeA); - } - if (kd > 0.0) { // has velocity control - JointAxisTarget(axis, targetVel, kd, &axisTargetVelKdD, &axisTargetVelKdA); - } + if (kd > 0.0) { // has velocity control + JointAxisTarget(axis, targetVel, kd, &axisTargetVelKdD, &axisTargetVelKdA); } - var axisStiffness = axisTargetPosKeA; - var axisDamping = axisTargetVelKdA; - axisTargetPosKeD = DivSafe3(axisTargetPosKeD, axisStiffness); - axisTargetVelKdD = DivSafe3(axisTargetVelKdD, axisDamping); - var axisLimitsLower = axisLimitsD; - var axisLimitsUpper = axisLimitsA; - for (var dim=0; dim(Dim3(gradX, dim), Dim3(gradY, dim), Dim3(gradZ, dim), Dim3(gradW, dim)); - var quatC = MulQuats(MulQuats(QuatMulScalar(qP, f32(0.5)), grad), QuatInverse(qC)); - var angularC = vec3(quatC.x, quatC.y, quatC.z); - var angularP = Negate3(angularC); - var derr = Dot3(angularP, wP) + Dot3(angularC, wC); - var err = f32(0.0); - var compliance = params.JointLinearComply; - var damping = f32(0.0); - var targetVel = Dim3(axisTargetVelKdD, dim); - var angularClen = Length3(angularC); - var derrRel = derr - targetVel*angularClen; - var lower = Dim3(axisLimitsLower, dim); - var upper = Dim3(axisLimitsUpper, dim); - if (e < lower) { - err = e - lower; - } else if (e > upper) { - err = e - upper; - } else { - var targetPos = Dim3(axisTargetPosKeD, dim); - targetPos = clamp(targetPos, lower, upper); - var ke = Dim3(axisStiffness, dim); - var kd = Dim3(axisDamping, dim); - if (ke > 0.0) { - err = e - targetPos; - compliance = 1.0 / ke; - damping = Dim3(axisDamping, dim); - } else if (kd > 0.0) { - compliance = 1.0 / kd; - damping = kd; - } + } + var axisStiffness = axisTargetPosKeA; + var axisDamping = axisTargetVelKdA; + axisTargetPosKeD = DivSafe3(axisTargetPosKeD, axisStiffness); + axisTargetVelKdD = DivSafe3(axisTargetVelKdD, axisDamping); + var axisLimitsLower = axisLimitsD; + var axisLimitsUpper = axisLimitsA; + for (var dim=0; dim(Dim3(gradX, dim), Dim3(gradY, dim), Dim3(gradZ, dim), Dim3(gradW, dim)); + var quatC = MulQuats(MulQuats(QuatMulScalar(qP, f32(0.5)), grad), QuatInverse(qC)); + var angularC = vec3(quatC.x, quatC.y, quatC.z); + var angularP = Negate3(angularC); + var derr = Dot3(angularP, wP) + Dot3(angularC, wC); + var err = f32(0.0); + var compliance = params.JointLinearComply; + var damping = f32(0.0); + var targetVel = Dim3(axisTargetVelKdD, dim); + var angularClen = Length3(angularC); + var derrRel = derr - targetVel*angularClen; + var lower = Dim3(axisLimitsLower, dim); + var upper = Dim3(axisLimitsUpper, dim); + if (e < lower) { + err = e - lower; + } else if (e > upper) { + err = e - upper; + } else { + var targetPos = Dim3(axisTargetPosKeD, dim); + targetPos = clamp(targetPos, lower, upper); + var ke = Dim3(axisStiffness, dim); + var kd = Dim3(axisDamping, dim); + if (ke > 0.0) { + err = e - targetPos; + compliance = 1.0 / ke; + damping = Dim3(axisDamping, dim); + } else if (kd > 0.0) { + compliance = 1.0 / kd; + damping = kd; } - var lambdaIn = f32(0); - var dLambda = AngularCorrection(err, derrRel, posePQ, poseCQ, iInvP, iInvC, angularP, angularC, lambdaIn, compliance, damping, params.Dt); - angDeltaP = angDeltaP+(angularP*(dLambda)); - angDeltaC = angDeltaC+(angularC*(dLambda)); } + var lambdaIn = f32(0); + var dLambda = AngularCorrection(err, derrRel, posePQ, poseCQ, iInvP, iInvC, angularP, angularC, lambdaIn, compliance, damping, params.Dt); + angDeltaP = angDeltaP+(angularP*(dLambda)); + angDeltaC = angDeltaC+(angularC*(dLambda)); + } + if (!parentFixed) { + StepBodyDeltas(jPi, jPbi, false, f32(f32(0)), linDeltaP, angDeltaP); + } + if (mInvC > 0) { + StepBodyDeltas(jCi, jCbi, false, f32(f32(0)), linDeltaC, angDeltaC); } - SetJointPDelta(ji, linDeltaP); - SetJointCDelta(ji, linDeltaC); - SetJointPAngDelta(ji, angDeltaP); - SetJointCAngDelta(ji, angDeltaC); Params[0] = params; } fn JointAxisTarget(axis: vec3, targ: f32,weight: f32, axisTargets: ptr>,axisWeights: ptr>) { @@ -842,12 +952,12 @@ fn PositionalCorrection(err: f32,derr: f32, tfaQ: vec4,tfbQ: vec4, mIn lambda /= (dt+gamma)*denom + alpha/dt; }return lambda; } -fn AngularCorrection(err: f32,derr: f32, tfaQ: vec4,tfbQ: vec4, iInva: mat3x3f,iInvb: mat3x3f, angulara: vec3,angularb: vec3, lambdaIn: f32,compliance: f32,damping: f32,dt: f32) -> f32 { - var rotAngulara = MulQuatVectorInverse(tfaQ, angulara); - var rotAngularb = MulQuatVectorInverse(tfbQ, angularb); +fn AngularCorrection(err: f32,derr: f32, tfaQ: vec4,tfbQ: vec4, iInvA: mat3x3f,iInvB: mat3x3f, angA: vec3,angB: vec3, lambdaIn: f32,compliance: f32,damping: f32,dt: f32) -> f32 { + var rotAngA = MulQuatVectorInverse(tfaQ, angA); + var rotAngB = MulQuatVectorInverse(tfbQ, angB); var denom = f32(0.0); - denom += Dot3(rotAngulara, iInva*(rotAngulara)); - denom += Dot3(rotAngularb, iInvb*(rotAngularb)); + denom += Dot3(rotAngA, iInvA*(rotAngA)); + denom += Dot3(rotAngB, iInvB*(rotAngB)); var alpha = compliance; var gamma = compliance * damping; var deltaLambda = -(err + alpha*lambdaIn + gamma*derr); diff --git a/physics/step.go b/physics/step.go index d8685392..c4c14769 100644 --- a/physics/step.go +++ b/physics/step.go @@ -120,8 +120,7 @@ func (ml *Model) StepIntegrateBodies() { func (ml *Model) StepSolveJoints() { params := GetParams(0) - RunStepSolveJoints(int(params.JointsN)) - RunStepBodyJointDeltas(int(params.DynamicsN)) + RunStepSolveJoints(int(params.ObjectsN)) } func (ml *Model) StepBodyContacts() { diff --git a/physics/step.goal b/physics/step.goal index 4e15d429..fc0e428c 100644 --- a/physics/step.goal +++ b/physics/step.goal @@ -118,8 +118,7 @@ func (ml *Model) StepIntegrateBodies() { func (ml *Model) StepSolveJoints() { params := GetParams(0) - RunStepSolveJoints(int(params.JointsN)) - RunStepBodyJointDeltas(int(params.DynamicsN)) + RunStepSolveJoints(int(params.ObjectsN)) } func (ml *Model) StepBodyContacts() { diff --git a/physics/step_body.go b/physics/step_body.go index c4115e5d..1585aa60 100644 --- a/physics/step_body.go +++ b/physics/step_body.go @@ -149,42 +149,6 @@ func StepIntegrateBodies(i uint32) { //gosl:kernel SetDynamicAngDelta(di, params.Next, w1) } -// StepBodyJointDeltas gathers raw deltas, angDeltas from joints per dynamic -// and computes updated deltas integrated via StepBodyDeltas. -func StepBodyJointDeltas(i uint32) { //gosl:kernel - params := GetParams(0) - di := int32(i) - if di >= params.DynamicsN { - return - } - bi := DynamicBody(di) - invMass := Bodies.Value(int(bi), int(BodyInvMass)) - if invMass == 0 { - return // no updates - } - - np := BodyJoints.Value(int(di), int(0), int(0)) - nc := BodyJoints.Value(int(di), int(1), int(0)) - - linDel := math32.Vec3(0, 0, 0) - angDel := math32.Vec3(0, 0, 0) - for i := int32(1); i <= np; i++ { - ji := BodyJoints.Value(int(di), int(0), int(i)) - d := JointPDelta(ji) - linDel = linDel.Add(d) - a := JointPAngDelta(ji) - angDel = angDel.Add(a) - } - for i := int32(1); i <= nc; i++ { - ji := BodyJoints.Value(int(di), int(1), int(i)) - d := JointCDelta(ji) - linDel = linDel.Add(d) - a := JointCAngDelta(ji) - angDel = angDel.Add(a) - } - StepBodyDeltas(di, bi, false, 0, linDel, angDel) -} - // newton: solvers/xpbd/kernels.py: apply_body_deltas // StepBodyDeltas updates Next position with deltas from joints diff --git a/physics/step_body.goal b/physics/step_body.goal index 09b0316f..94b70510 100644 --- a/physics/step_body.goal +++ b/physics/step_body.goal @@ -147,42 +147,6 @@ func StepIntegrateBodies(i uint32) { //gosl:kernel SetDynamicAngDelta(di, params.Next, w1) } -// StepBodyJointDeltas gathers raw deltas, angDeltas from joints per dynamic -// and computes updated deltas integrated via StepBodyDeltas. -func StepBodyJointDeltas(i uint32) { //gosl:kernel - params := GetParams(0) - di := int32(i) - if di >= params.DynamicsN { - return - } - bi := DynamicBody(di) - invMass := Bodies[bi, BodyInvMass] - if invMass == 0 { - return // no updates - } - - np := BodyJoints[di, 0, 0] - nc := BodyJoints[di, 1, 0] - - linDel := math32.Vec3(0,0,0) - angDel := math32.Vec3(0,0,0) - for i := int32(1); i <= np; i++ { - ji := BodyJoints[di, 0, i] - d := JointPDelta(ji) - linDel = linDel.Add(d) - a := JointPAngDelta(ji) - angDel = angDel.Add(a) - } - for i := int32(1); i <= nc; i++ { - ji := BodyJoints[di, 1, i] - d := JointCDelta(ji) - linDel = linDel.Add(d) - a := JointCAngDelta(ji) - angDel = angDel.Add(a) - } - StepBodyDeltas(di, bi, false, 0, linDel, angDel) -} - // newton: solvers/xpbd/kernels.py: apply_body_deltas // StepBodyDeltas updates Next position with deltas from joints diff --git a/physics/step_joint.go b/physics/step_joint.go index 4a369594..ae454cf6 100644 --- a/physics/step_joint.go +++ b/physics/step_joint.go @@ -10,14 +10,11 @@ package physics import ( + // "fmt" "cogentcore.org/core/math32" "cogentcore.org/lab/gosl/slmath" ) -// todo: joint types in solve -- seems wrong -// see if does anything when dofs are empty -// limits, damping, etc in test. - // notation convention: // spatial transform: R = position, Q = quat rotation // P = parent, C = child @@ -119,33 +116,43 @@ func StepJointForces(i uint32) { //gosl:kernel // newton: solvers/xpbd/kernels.py: solve_body_joints // StepSolveJoints applies target positions to joints. +// This is per Object because it needs to solve joints in parent -> child order. func StepSolveJoints(i uint32) { //gosl:kernel params := GetParams(0) - ji := int32(i) - if ji >= params.JointsN { + oi := int32(i) + if oi >= params.ObjectsN { return } + n := Objects.Value(int(oi), int(0)) + for i := int32(1); i < n+1; i++ { + ji := Objects.Value(int(oi), int(i)) + jt := GetJointType(ji) + if jt == Free || !GetJointEnabled(ji) { + continue + } + StepSolveJointLinear(ji) + StepSolveJointAngular(ji) + } +} - zv := math32.Vec3(0, 0, 0) - SetJointPDelta(ji, zv) - SetJointCDelta(ji, zv) - SetJointPAngDelta(ji, zv) - SetJointCAngDelta(ji, zv) +// StepSolveJointLinear applies target positions to linear DoFs. +// Position is updated prior to computing angulars. +func StepSolveJointLinear(ji int32) { + params := GetParams(0) jt := GetJointType(ji) - if jt == Free || !GetJointEnabled(ji) { - return - } jPi := JointParentIndex(ji) jPbi := int32(-1) + parentFixed := true if jPi >= 0 { jPbi = DynamicBody(jPi) + parentFixed = GetJointParentFixed(ji) } jCi := JointChildIndex(ji) jCbi := DynamicBody(jCi) jLinearN := GetJointLinearDoFN(ji) - jAngularN := GetJointAngularDoFN(ji) + // jAngularN := GetJointAngularDoFN(ji) jPR := JointPPos(ji) jPQ := JointPQuat(ji) @@ -164,14 +171,14 @@ func StepSolveJoints(i uint32) { //gosl:kernel posePR = DynamicPos(jPi, params.Next) // now using next posePQ = DynamicQuat(jPi, params.Next) slmath.MulSpatialTransforms(posePR, posePQ, jPR, jPQ, &xwPR, &xwPQ) - // if xwPR.IsNaN() || xwPQ.IsNaN() { - // fmt.Println("jpi:", jPi, posePR, posePQ, jPR, jPQ) - // } comP = BodyCom(jPbi) mInvP = Bodies.Value(int(jPbi), int(BodyInvMass)) iInvP = BodyInvInertia(jPbi) vP = DynamicDelta(jPi, params.Next) wP = DynamicAngDelta(jPi, params.Next) + if mInvP == 0 { + parentFixed = true + } } // child transform and moment arm @@ -181,17 +188,7 @@ func StepSolveJoints(i uint32) { //gosl:kernel jCQ := JointCQuat(ji) xwCR := jCR xwCQ := jCQ - // if jCQ.IsNaN() || jCR.IsNaN() { - // fmt.Println("child joint start:", jCQ, jCR) - // } - // - // if poseCQ.IsNaN() || poseCR.IsNaN() { - // fmt.Println("jCi:", jCi, poseCQ, poseCR) - // } slmath.MulSpatialTransforms(poseCR, poseCQ, jCR, jCQ, &xwCR, &xwCQ) - // if xwCQ.IsNaN() || xwCR.IsNaN() { - // fmt.Println("xwCQ:", xwCQ, xwCR, poseCR, poseCQ, jCR, jCQ) - // } comC := BodyCom(jCbi) mInvC := Bodies.Value(int(jCbi), int(BodyInvMass)) iInvC := BodyInvInertia(jCbi) @@ -241,7 +238,7 @@ func StepSolveJoints(i uint32) { //gosl:kernel angularC := slmath.Cross3(dC, linearC) // constraint time derivative derr := slmath.Dot3(linearP, vP) + slmath.Dot3(linearC, vC) + slmath.Dot3(angularP, wP) + slmath.Dot3(angularC, wC) - lambdaIn := float32(0.0) + lambdaIn := float32(0.0) // note: multiple iter is supposed to increment these compliance := params.JointLinearComply ke := JointControl(ji, 0, JointTargetStiff) kd := JointControl(ji, 0, JointTargetDamp) @@ -255,7 +252,7 @@ func StepSolveJoints(i uint32) { //gosl:kernel linDeltaC = linDeltaC.Add(linearC.MulScalar(dLambda * params.JointLinearRelax)) angDeltaC = angDeltaC.Add(angularC.MulScalar(dLambda * params.JointAngularRelax)) } - } else { // compute joint target, stiffness, damping + } else { // all joints impose linear constraints! var axisLimitsD, axisLimitsA math32.Vector3 var axisTargetPosKeD, axisTargetPosKeA math32.Vector3 @@ -334,182 +331,231 @@ func StepSolveJoints(i uint32) { //gosl:kernel iInvP, iInvC, linearP, linearC, angularP, angularC, lambdaIn, compliance, damping, params.Dt) linDeltaP = linDeltaP.Add(linearP.MulScalar(dLambda * params.JointLinearRelax)) - angDeltaP = angDeltaP.Add(angularP.MulScalar(dLambda * params.JointAngularRelax)) linDeltaC = linDeltaC.Add(linearC.MulScalar(dLambda * params.JointLinearRelax)) + angDeltaP = angDeltaP.Add(angularP.MulScalar(dLambda * params.JointAngularRelax)) angDeltaC = angDeltaC.Add(angularC.MulScalar(dLambda * params.JointAngularRelax)) - - // if angDeltaC.IsNaN() || angDeltaP.IsNaN() { - // fmt.Println("joint ang:", angDeltaC, angDeltaP) - // } - // - // if linDeltaC.IsNaN() || linDeltaP.IsNaN() { - // fmt.Println("joint lin:", linDeltaC, linDeltaP) - // } } } } - if jt == Fixed || jt == Prismatic || jt == Revolute || jt == Ball || jt == D6 { - qP := xwPQ - qC := xwCQ - // make quats lie in same hemisphere - if slmath.QuatDot(qP, qC) < 0 { - qC = slmath.QuatMulScalar(qC, -1.0) - } - relQ := slmath.MulQuats(slmath.QuatInverse(qP), qC) - qtwist := slmath.QuatNormalize(math32.NewQuat(relQ.X, 0.0, 0.0, relQ.W)) - qswing := slmath.MulQuats(relQ, slmath.QuatInverse(qtwist)) - - // if qP.IsNaN() || qC.IsNaN() || relQ.IsNaN() { - // fmt.Println("qp, qc:", qP, qC, relQ) - // } - - // decompose to a compound rotation each axis - s := math32.Sqrt(relQ.X*relQ.X + relQ.W*relQ.W) - if s == 0 { - // fmt.Println("s = 0", relQ, qP, qC) - s = 1 - } - invs := 1.0 / s - invscube := invs * invs * invs - - // handle axis-angle joints - // rescale twist from quaternion space to angular - err0 := 2.0 * math32.Asin(math32.Clamp(qtwist.X, -1.0, 1.0)) - err1 := qswing.Y - err2 := qswing.Z - // analytic gradients of swing-twist decomposition - grad0 := math32.NewQuat(invs-relQ.X*relQ.X*invscube, 0.0, 0.0, -(relQ.W*relQ.X)*invscube) - grad1 := math32.NewQuat( - -relQ.W*(relQ.W*relQ.Z+relQ.X*relQ.Y)*invscube, - relQ.W*invs, - -relQ.X*invs, - relQ.X*(relQ.W*relQ.Z+relQ.X*relQ.Y)*invscube) - grad2 := math32.NewQuat( - relQ.W*(relQ.W*relQ.Y-relQ.X*relQ.Z)*invscube, - relQ.X*invs, - relQ.W*invs, - relQ.X*(relQ.Z*relQ.X-relQ.W*relQ.Y)*invscube) - grad0 = slmath.QuatMulScalar(grad0, 2.0/math32.Abs(qtwist.W)) - // # grad0 *= 2.0 / wp.sqrt(1.0-qtwist[0]*qtwist[0]) # derivative of asin(x) = 1/sqrt(1-x^2) - // if grad0.IsNaN() || grad1.IsNaN() || grad2.IsNaN() { - // fmt.Println("grads:", grad0, grad1, grad2) - // } - - // rescale swing - swing_sq := qswing.W * qswing.W - // if swing axis magnitude close to zero vector, just treat in quaternion space - angularEps := float32(1.0e-4) - if swing_sq+angularEps < 1.0 { - d := math32.Sqrt(1.0 - qswing.W*qswing.W) - theta := 2.0 * math32.Acos(math32.Clamp(qswing.W, -1.0, 1.0)) - scale := theta / d - err1 *= scale - err2 *= scale - grad1 = slmath.QuatMulScalar(grad1, scale) - grad2 = slmath.QuatMulScalar(grad2, scale) - } - errs := math32.Vec3(err0, err1, err2) - gradX := math32.Vec3(grad0.X, grad1.X, grad2.X) - gradY := math32.Vec3(grad0.Y, grad1.Y, grad2.Y) - gradZ := math32.Vec3(grad0.Z, grad1.Z, grad2.Z) - gradW := math32.Vec3(grad0.W, grad1.W, grad2.W) + // apply the linear deltas so the + if !parentFixed { + StepBodyDeltas(jPi, jPbi, false, 0, linDeltaP, angDeltaP) + } + if mInvC > 0 { + StepBodyDeltas(jCi, jCbi, false, 0, linDeltaC, angDeltaC) + } +} - // compute joint target, stiffness, damping - var axisLimitsD, axisLimitsA math32.Vector3 - var axisTargetPosKeD, axisTargetPosKeA math32.Vector3 - var axisTargetVelKdD, axisTargetVelKdA math32.Vector3 +// StepSolveJointAngular applies target positions to angular DoFs. +func StepSolveJointAngular(ji int32) { + params := GetParams(0) - for dof := range jAngularN { - di := dof + jLinearN - axis := JointAxis(ji, di) - JointAxisLimitsUpdate(dof, axis, JointDoF(ji, di, JointLimitLower), JointDoF(ji, di, JointLimitUpper), &axisLimitsD, &axisLimitsA) - ke := JointControl(ji, di, JointTargetStiff) - kd := JointControl(ji, di, JointTargetDamp) - targetPos := JointControl(ji, di, JointTargetPos) - targetVel := JointControl(ji, di, JointTargetVel) - if ke > 0.0 { // has position control - JointAxisTarget(axis, targetPos, ke, &axisTargetPosKeD, &axisTargetPosKeA) - } - if kd > 0.0 { // has velocity control - JointAxisTarget(axis, targetVel, kd, &axisTargetVelKdD, &axisTargetVelKdA) - } + jPi := JointParentIndex(ji) + jPbi := int32(-1) + parentFixed := true + if jPi >= 0 { + jPbi = DynamicBody(jPi) + parentFixed = GetJointParentFixed(ji) + } + jCi := JointChildIndex(ji) + jCbi := DynamicBody(jCi) + + jLinearN := GetJointLinearDoFN(ji) + jAngularN := GetJointAngularDoFN(ji) + + jPR := JointPPos(ji) + jPQ := JointPQuat(ji) + + xwPR := jPR // world xform, parent, pos + xwPQ := jPQ // quat + mInvP := float32(0.0) + iInvP := math32.Mat3(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0) + posePR := jPR + posePQ := jPQ + + var wP math32.Vector3 + + // parent transform and moment arm + if jPi >= 0 { + posePR = DynamicPos(jPi, params.Next) // now using next + posePQ = DynamicQuat(jPi, params.Next) + slmath.MulSpatialTransforms(posePR, posePQ, jPR, jPQ, &xwPR, &xwPQ) + mInvP = Bodies.Value(int(jPbi), int(BodyInvMass)) + iInvP = BodyInvInertia(jPbi) + wP = DynamicAngDelta(jPi, params.Next) + if mInvP == 0 { + parentFixed = true } + } - axisStiffness := axisTargetPosKeA - axisDamping := axisTargetVelKdA - axisTargetPosKeD = slmath.DivSafe3(axisTargetPosKeD, axisStiffness) - axisTargetVelKdD = slmath.DivSafe3(axisTargetVelKdD, axisDamping) - axisLimitsLower := axisLimitsD - axisLimitsUpper := axisLimitsA + // child transform and moment arm + poseCR := DynamicPos(jCi, params.Next) + poseCQ := DynamicQuat(jCi, params.Next) + jCR := JointCPos(ji) + jCQ := JointCQuat(ji) + xwCR := jCR + xwCQ := jCQ + slmath.MulSpatialTransforms(poseCR, poseCQ, jCR, jCQ, &xwCR, &xwCQ) + mInvC := Bodies.Value(int(jCbi), int(BodyInvMass)) + iInvC := BodyInvInertia(jCbi) + wC := DynamicAngDelta(jCi, params.Next) - for dim := range int32(3) { - e := slmath.Dim3(errs, dim) - - // analytic gradients of swing-twist decomposition - grad := math32.NewQuat(slmath.Dim3(gradX, dim), slmath.Dim3(gradY, dim), slmath.Dim3(gradZ, dim), slmath.Dim3(gradW, dim)) - // todo: verify -- does the 0.5 go inside?? - // quat_c = 0.5 * q_p * grad * wp.quat_inverse(q_c) - quatC := slmath.MulQuats(slmath.MulQuats(slmath.QuatMulScalar(qP, 0.5), grad), slmath.QuatInverse(qC)) - // if ji == 0 { - // fmt.Println(quatC, qP, qC, grad) - // } - - angularC := math32.Vec3(quatC.X, quatC.Y, quatC.Z) - angularP := slmath.Negate3(angularC) - // constraint time derivative - derr := slmath.Dot3(angularP, wP) + slmath.Dot3(angularC, wC) + if mInvP == 0.0 && mInvC == 0.0 { // connection between two immovable bodies + return + } - err := float32(0.0) - compliance := params.JointLinearComply - damping := float32(0.0) + // accumulate constraint deltas + var linDeltaP, angDeltaP, linDeltaC, angDeltaC math32.Vector3 - targetVel := slmath.Dim3(axisTargetVelKdD, dim) - angularClen := slmath.Length3(angularC) - derrRel := derr - targetVel*angularClen + relPoseR := xwPR + relPoseQ := xwPQ + slmath.SpatialTransformInverse(xwPR, xwPQ, &relPoseR, &relPoseQ) + slmath.MulSpatialTransforms(relPoseR, relPoseQ, xwCR, xwCQ, &relPoseR, &relPoseQ) - // consider joint limits irrespective of axis mode - lower := slmath.Dim3(axisLimitsLower, dim) - upper := slmath.Dim3(axisLimitsUpper, dim) - if e < lower { - err = e - lower - } else if e > upper { - err = e - upper - } else { - targetPos := slmath.Dim3(axisTargetPosKeD, dim) - targetPos = math32.Clamp(targetPos, lower, upper) + qP := xwPQ + qC := xwCQ + // make quats lie in same hemisphere + if slmath.QuatDot(qP, qC) < 0 { + qC = slmath.QuatMulScalar(qC, -1.0) + } + relQ := slmath.MulQuats(slmath.QuatInverse(qP), qC) + qtwist := slmath.QuatNormalize(math32.NewQuat(relQ.X, 0.0, 0.0, relQ.W)) + qswing := slmath.MulQuats(relQ, slmath.QuatInverse(qtwist)) + + // decompose to a compound rotation each axis + s := math32.Sqrt(relQ.X*relQ.X + relQ.W*relQ.W) + if s == 0 { + // fmt.Println("s = 0", relQ, qP, qC) + s = 1 + } + invs := 1.0 / s + invscube := invs * invs * invs + + // handle axis-angle joints + // rescale twist from quaternion space to angular + err0 := 2.0 * math32.Asin(math32.Clamp(qtwist.X, -1.0, 1.0)) + err1 := qswing.Y + err2 := qswing.Z + // analytic gradients of swing-twist decomposition + grad0 := math32.NewQuat(invs-relQ.X*relQ.X*invscube, 0.0, 0.0, -(relQ.W*relQ.X)*invscube) + grad1 := math32.NewQuat( + -relQ.W*(relQ.W*relQ.Z+relQ.X*relQ.Y)*invscube, + relQ.W*invs, + -relQ.X*invs, + relQ.X*(relQ.W*relQ.Z+relQ.X*relQ.Y)*invscube) + grad2 := math32.NewQuat( + relQ.W*(relQ.W*relQ.Y-relQ.X*relQ.Z)*invscube, + relQ.X*invs, + relQ.W*invs, + relQ.X*(relQ.Z*relQ.X-relQ.W*relQ.Y)*invscube) + grad0 = slmath.QuatMulScalar(grad0, 2.0/math32.Abs(qtwist.W)) + // # grad0 *= 2.0 / wp.sqrt(1.0-qtwist[0]*qtwist[0]) # derivative of asin(x) = 1/sqrt(1-x^2) + + // rescale swing + swing_sq := qswing.W * qswing.W + // if swing axis magnitude close to zero vector, just treat in quaternion space + angularEps := float32(1.0e-4) + if swing_sq+angularEps < 1.0 { + d := math32.Sqrt(1.0 - qswing.W*qswing.W) + theta := 2.0 * math32.Acos(math32.Clamp(qswing.W, -1.0, 1.0)) + scale := theta / d + err1 *= scale + err2 *= scale + grad1 = slmath.QuatMulScalar(grad1, scale) + grad2 = slmath.QuatMulScalar(grad2, scale) + } + errs := math32.Vec3(err0, err1, err2) + gradX := math32.Vec3(grad0.X, grad1.X, grad2.X) + gradY := math32.Vec3(grad0.Y, grad1.Y, grad2.Y) + gradZ := math32.Vec3(grad0.Z, grad1.Z, grad2.Z) + gradW := math32.Vec3(grad0.W, grad1.W, grad2.W) + + // compute joint target, stiffness, damping + var axisLimitsD, axisLimitsA math32.Vector3 + var axisTargetPosKeD, axisTargetPosKeA math32.Vector3 + var axisTargetVelKdD, axisTargetVelKdA math32.Vector3 + + for dof := range jAngularN { + di := dof + jLinearN + axis := JointAxis(ji, di) + JointAxisLimitsUpdate(dof, axis, JointDoF(ji, di, JointLimitLower), JointDoF(ji, di, JointLimitUpper), &axisLimitsD, &axisLimitsA) + ke := JointControl(ji, di, JointTargetStiff) + kd := JointControl(ji, di, JointTargetDamp) + targetPos := JointControl(ji, di, JointTargetPos) + targetVel := JointControl(ji, di, JointTargetVel) + if ke > 0.0 { // has position control + JointAxisTarget(axis, targetPos, ke, &axisTargetPosKeD, &axisTargetPosKeA) + } + if kd > 0.0 { // has velocity control + JointAxisTarget(axis, targetVel, kd, &axisTargetVelKdD, &axisTargetVelKdA) + } + } - ke := slmath.Dim3(axisStiffness, dim) - kd := slmath.Dim3(axisDamping, dim) - if ke > 0.0 { - err = e - targetPos - compliance = 1.0 / ke - damping = slmath.Dim3(axisDamping, dim) - } else if kd > 0.0 { - compliance = 1.0 / kd - damping = kd - } + axisStiffness := axisTargetPosKeA + axisDamping := axisTargetVelKdA + axisTargetPosKeD = slmath.DivSafe3(axisTargetPosKeD, axisStiffness) + axisTargetVelKdD = slmath.DivSafe3(axisTargetVelKdD, axisDamping) + axisLimitsLower := axisLimitsD + axisLimitsUpper := axisLimitsA + + for dim := range int32(3) { + e := slmath.Dim3(errs, dim) + + // analytic gradients of swing-twist decomposition + grad := math32.NewQuat(slmath.Dim3(gradX, dim), slmath.Dim3(gradY, dim), slmath.Dim3(gradZ, dim), slmath.Dim3(gradW, dim)) + // todo: verify -- does the 0.5 go inside?? + // quat_c = 0.5 * q_p * grad * wp.quat_inverse(q_c) + quatC := slmath.MulQuats(slmath.MulQuats(slmath.QuatMulScalar(qP, 0.5), grad), slmath.QuatInverse(qC)) + + angularC := math32.Vec3(quatC.X, quatC.Y, quatC.Z) + angularP := slmath.Negate3(angularC) + // constraint time derivative + derr := slmath.Dot3(angularP, wP) + slmath.Dot3(angularC, wC) + + err := float32(0.0) + compliance := params.JointLinearComply + damping := float32(0.0) + + targetVel := slmath.Dim3(axisTargetVelKdD, dim) + angularClen := slmath.Length3(angularC) + derrRel := derr - targetVel*angularClen + + // consider joint limits irrespective of axis mode + lower := slmath.Dim3(axisLimitsLower, dim) + upper := slmath.Dim3(axisLimitsUpper, dim) + if e < lower { + err = e - lower + } else if e > upper { + err = e - upper + } else { + targetPos := slmath.Dim3(axisTargetPosKeD, dim) + targetPos = math32.Clamp(targetPos, lower, upper) + + ke := slmath.Dim3(axisStiffness, dim) + kd := slmath.Dim3(axisDamping, dim) + if ke > 0.0 { + err = e - targetPos + compliance = 1.0 / ke + damping = slmath.Dim3(axisDamping, dim) + } else if kd > 0.0 { + compliance = 1.0 / kd + damping = kd } - lambdaIn := float32(0) - dLambda := AngularCorrection(err, derrRel, posePQ, poseCQ, iInvP, iInvC, angularP, angularC, lambdaIn, compliance, damping, params.Dt) - - // note: no relaxation factors here: - angDeltaP = angDeltaP.Add(angularP.MulScalar(dLambda)) - angDeltaC = angDeltaC.Add(angularC.MulScalar(dLambda)) - // if ji == 0 && dim == 1 { - // fmt.Println(ji, dim, e, err, lambdaIn, angularP, angularC) - // } - // - // if angDeltaC.IsNaN() || angDeltaP.IsNaN() { - // fmt.Println("ang joint ang:", angDeltaC, angDeltaP, angularC, angularP, dLambda) - // } } + lambdaIn := float32(0) + dLambda := AngularCorrection(err, derrRel, posePQ, poseCQ, iInvP, iInvC, angularP, angularC, lambdaIn, compliance, damping, params.Dt) + + // note: no relaxation factors here: + angDeltaP = angDeltaP.Add(angularP.MulScalar(dLambda)) + angDeltaC = angDeltaC.Add(angularC.MulScalar(dLambda)) } - // These are unique to joint: aggregate into dynamics Next in separate step. - SetJointPDelta(ji, linDeltaP) - SetJointCDelta(ji, linDeltaC) - SetJointPAngDelta(ji, angDeltaP) - SetJointCAngDelta(ji, angDeltaC) + if !parentFixed { + StepBodyDeltas(jPi, jPbi, false, 0, linDeltaP, angDeltaP) + } + if mInvC > 0 { + StepBodyDeltas(jCi, jCbi, false, 0, linDeltaC, angDeltaC) + } } func JointAxisTarget(axis math32.Vector3, targ, weight float32, axisTargets, axisWeights *math32.Vector3) { @@ -541,14 +587,14 @@ func PositionalCorrection(err, derr float32, tfaQ, tfbQ math32.Quat, mInvA, mInv return lambda } -func AngularCorrection(err, derr float32, tfaQ, tfbQ math32.Quat, iInva, iInvb math32.Matrix3, angulara, angularb math32.Vector3, lambdaIn, compliance, damping, dt float32) float32 { +func AngularCorrection(err, derr float32, tfaQ, tfbQ math32.Quat, iInvA, iInvB math32.Matrix3, angA, angB math32.Vector3, lambdaIn, compliance, damping, dt float32) float32 { // # Eq. 2-3 (make sure to project into the frame of the body) - rotAngulara := slmath.MulQuatVectorInverse(tfaQ, angulara) - rotAngularb := slmath.MulQuatVectorInverse(tfbQ, angularb) + rotAngA := slmath.MulQuatVectorInverse(tfaQ, angA) + rotAngB := slmath.MulQuatVectorInverse(tfbQ, angB) denom := float32(0.0) - denom += slmath.Dot3(rotAngulara, iInva.MulVector3(rotAngulara)) - denom += slmath.Dot3(rotAngularb, iInvb.MulVector3(rotAngularb)) + denom += slmath.Dot3(rotAngA, iInvA.MulVector3(rotAngA)) + denom += slmath.Dot3(rotAngB, iInvB.MulVector3(rotAngB)) alpha := compliance gamma := compliance * damping diff --git a/physics/step_joint.goal b/physics/step_joint.goal index 4bc4fb14..eac0b9ef 100644 --- a/physics/step_joint.goal +++ b/physics/step_joint.goal @@ -8,14 +8,11 @@ package physics import ( + // "fmt" "cogentcore.org/core/math32" "cogentcore.org/lab/gosl/slmath" ) -// todo: joint types in solve -- seems wrong -// see if does anything when dofs are empty -// limits, damping, etc in test. - // notation convention: // spatial transform: R = position, Q = quat rotation // P = parent, C = child @@ -117,33 +114,43 @@ func StepJointForces(i uint32) { //gosl:kernel // newton: solvers/xpbd/kernels.py: solve_body_joints // StepSolveJoints applies target positions to joints. +// This is per Object because it needs to solve joints in parent -> child order. func StepSolveJoints(i uint32) { //gosl:kernel params := GetParams(0) - ji := int32(i) - if ji >= params.JointsN { + oi := int32(i) + if oi >= params.ObjectsN { return } + n := Objects[oi, 0] + for i :=int32(1); i < n+1; i++ { + ji := Objects[oi, i] + jt := GetJointType(ji) + if jt == Free || !GetJointEnabled(ji) { + continue + } + StepSolveJointLinear(ji) + StepSolveJointAngular(ji) + } +} - zv := math32.Vec3(0, 0, 0) - SetJointPDelta(ji, zv) - SetJointCDelta(ji, zv) - SetJointPAngDelta(ji, zv) - SetJointCAngDelta(ji, zv) +// StepSolveJointLinear applies target positions to linear DoFs. +// Position is updated prior to computing angulars. +func StepSolveJointLinear(ji int32) { + params := GetParams(0) jt := GetJointType(ji) - if jt == Free || !GetJointEnabled(ji) { - return - } jPi := JointParentIndex(ji) jPbi := int32(-1) + parentFixed := true if jPi >= 0 { jPbi = DynamicBody(jPi) + parentFixed = GetJointParentFixed(ji) } jCi := JointChildIndex(ji) jCbi := DynamicBody(jCi) jLinearN := GetJointLinearDoFN(ji) - jAngularN := GetJointAngularDoFN(ji) + // jAngularN := GetJointAngularDoFN(ji) jPR := JointPPos(ji) jPQ := JointPQuat(ji) @@ -162,14 +169,14 @@ func StepSolveJoints(i uint32) { //gosl:kernel posePR = DynamicPos(jPi, params.Next) // now using next posePQ = DynamicQuat(jPi, params.Next) slmath.MulSpatialTransforms(posePR, posePQ, jPR, jPQ, &xwPR, &xwPQ) - // if xwPR.IsNaN() || xwPQ.IsNaN() { - // fmt.Println("jpi:", jPi, posePR, posePQ, jPR, jPQ) - // } comP = BodyCom(jPbi) mInvP = Bodies[jPbi, BodyInvMass] iInvP = BodyInvInertia(jPbi) vP = DynamicDelta(jPi, params.Next) wP = DynamicAngDelta(jPi, params.Next) + if mInvP == 0 { + parentFixed = true + } } // child transform and moment arm @@ -179,16 +186,7 @@ func StepSolveJoints(i uint32) { //gosl:kernel jCQ := JointCQuat(ji) xwCR := jCR xwCQ := jCQ - // if jCQ.IsNaN() || jCR.IsNaN() { - // fmt.Println("child joint start:", jCQ, jCR) - // } - // if poseCQ.IsNaN() || poseCR.IsNaN() { - // fmt.Println("jCi:", jCi, poseCQ, poseCR) - // } slmath.MulSpatialTransforms(poseCR, poseCQ, jCR, jCQ, &xwCR, &xwCQ) - // if xwCQ.IsNaN() || xwCR.IsNaN() { - // fmt.Println("xwCQ:", xwCQ, xwCR, poseCR, poseCQ, jCR, jCQ) - // } comC := BodyCom(jCbi) mInvC := Bodies[jCbi, BodyInvMass] iInvC := BodyInvInertia(jCbi) @@ -238,7 +236,7 @@ func StepSolveJoints(i uint32) { //gosl:kernel angularC := slmath.Cross3(dC, linearC) // constraint time derivative derr := slmath.Dot3(linearP, vP) + slmath.Dot3(linearC, vC) + slmath.Dot3(angularP, wP) + slmath.Dot3(angularC, wC) - lambdaIn := float32(0.0) + lambdaIn := float32(0.0) // note: multiple iter is supposed to increment these compliance := params.JointLinearComply ke := JointControl(ji, 0, JointTargetStiff) kd := JointControl(ji, 0, JointTargetDamp) @@ -252,7 +250,7 @@ func StepSolveJoints(i uint32) { //gosl:kernel linDeltaC = linDeltaC.Add(linearC.MulScalar(dLambda * params.JointLinearRelax)) angDeltaC = angDeltaC.Add(angularC.MulScalar(dLambda * params.JointAngularRelax)) } - } else { // compute joint target, stiffness, damping + } else { // all joints impose linear constraints! var axisLimitsD, axisLimitsA math32.Vector3 var axisTargetPosKeD, axisTargetPosKeA math32.Vector3 @@ -331,180 +329,231 @@ func StepSolveJoints(i uint32) { //gosl:kernel iInvP, iInvC, linearP, linearC, angularP, angularC, lambdaIn, compliance, damping, params.Dt) linDeltaP = linDeltaP.Add(linearP.MulScalar(dLambda * params.JointLinearRelax)) - angDeltaP = angDeltaP.Add(angularP.MulScalar(dLambda * params.JointAngularRelax)) linDeltaC = linDeltaC.Add(linearC.MulScalar(dLambda * params.JointLinearRelax)) + angDeltaP = angDeltaP.Add(angularP.MulScalar(dLambda * params.JointAngularRelax)) angDeltaC = angDeltaC.Add(angularC.MulScalar(dLambda * params.JointAngularRelax)) - - // if angDeltaC.IsNaN() || angDeltaP.IsNaN() { - // fmt.Println("joint ang:", angDeltaC, angDeltaP) - // } - // if linDeltaC.IsNaN() || linDeltaP.IsNaN() { - // fmt.Println("joint lin:", linDeltaC, linDeltaP) - // } } } } - if jt == Fixed || jt == Prismatic || jt == Revolute || jt == Ball || jt == D6 { - qP := xwPQ - qC := xwCQ - // make quats lie in same hemisphere - if slmath.QuatDot(qP, qC) < 0 { - qC = slmath.QuatMulScalar(qC, -1.0) - } - relQ := slmath.MulQuats(slmath.QuatInverse(qP), qC) - qtwist := slmath.QuatNormalize(math32.NewQuat(relQ.X, 0.0, 0.0, relQ.W)) - qswing := slmath.MulQuats(relQ, slmath.QuatInverse(qtwist)) - - // if qP.IsNaN() || qC.IsNaN() || relQ.IsNaN() { - // fmt.Println("qp, qc:", qP, qC, relQ) - // } - - // decompose to a compound rotation each axis - s := math32.Sqrt(relQ.X*relQ.X + relQ.W*relQ.W) - if s == 0 { - // fmt.Println("s = 0", relQ, qP, qC) - s = 1 - } - invs := 1.0 / s - invscube := invs * invs * invs - - // handle axis-angle joints - // rescale twist from quaternion space to angular - err0 := 2.0 * math32.Asin(math32.Clamp(qtwist.X, -1.0, 1.0)) - err1 := qswing.Y - err2 := qswing.Z - // analytic gradients of swing-twist decomposition - grad0 := math32.NewQuat(invs-relQ.X*relQ.X*invscube, 0.0, 0.0, -(relQ.W*relQ.X)*invscube) - grad1 := math32.NewQuat( - -relQ.W*(relQ.W*relQ.Z+relQ.X*relQ.Y)*invscube, - relQ.W*invs, - -relQ.X*invs, - relQ.X*(relQ.W*relQ.Z+relQ.X*relQ.Y)*invscube) - grad2 := math32.NewQuat( - relQ.W*(relQ.W*relQ.Y-relQ.X*relQ.Z)*invscube, - relQ.X*invs, - relQ.W*invs, - relQ.X*(relQ.Z*relQ.X-relQ.W*relQ.Y)*invscube) - grad0 = slmath.QuatMulScalar(grad0, 2.0/math32.Abs(qtwist.W)) - // # grad0 *= 2.0 / wp.sqrt(1.0-qtwist[0]*qtwist[0]) # derivative of asin(x) = 1/sqrt(1-x^2) - // if grad0.IsNaN() || grad1.IsNaN() || grad2.IsNaN() { - // fmt.Println("grads:", grad0, grad1, grad2) - // } - - // rescale swing - swing_sq := qswing.W * qswing.W - // if swing axis magnitude close to zero vector, just treat in quaternion space - angularEps := float32(1.0e-4) - if swing_sq+angularEps < 1.0 { - d := math32.Sqrt(1.0 - qswing.W*qswing.W) - theta := 2.0 * math32.Acos(math32.Clamp(qswing.W, -1.0, 1.0)) - scale := theta / d - err1 *= scale - err2 *= scale - grad1 = slmath.QuatMulScalar(grad1, scale) - grad2 = slmath.QuatMulScalar(grad2, scale) - } - errs := math32.Vec3(err0, err1, err2) - gradX := math32.Vec3(grad0.X, grad1.X, grad2.X) - gradY := math32.Vec3(grad0.Y, grad1.Y, grad2.Y) - gradZ := math32.Vec3(grad0.Z, grad1.Z, grad2.Z) - gradW := math32.Vec3(grad0.W, grad1.W, grad2.W) + // apply the linear deltas so the + if !parentFixed { + StepBodyDeltas(jPi, jPbi, false, 0, linDeltaP, angDeltaP) + } + if mInvC > 0 { + StepBodyDeltas(jCi, jCbi, false, 0, linDeltaC, angDeltaC) + } +} - // compute joint target, stiffness, damping - var axisLimitsD, axisLimitsA math32.Vector3 - var axisTargetPosKeD, axisTargetPosKeA math32.Vector3 - var axisTargetVelKdD, axisTargetVelKdA math32.Vector3 +// StepSolveJointAngular applies target positions to angular DoFs. +func StepSolveJointAngular(ji int32) { + params := GetParams(0) - for dof := range jAngularN { - di := dof + jLinearN - axis := JointAxis(ji, di) - JointAxisLimitsUpdate(dof, axis, JointDoF(ji, di, JointLimitLower), JointDoF(ji, di, JointLimitUpper), &axisLimitsD, &axisLimitsA) - ke := JointControl(ji, di, JointTargetStiff) - kd := JointControl(ji, di, JointTargetDamp) - targetPos := JointControl(ji, di, JointTargetPos) - targetVel := JointControl(ji, di, JointTargetVel) - if ke > 0.0 { // has position control - JointAxisTarget(axis, targetPos, ke, &axisTargetPosKeD, &axisTargetPosKeA) - } - if kd > 0.0 { // has velocity control - JointAxisTarget(axis, targetVel, kd, &axisTargetVelKdD, &axisTargetVelKdA) - } + jPi := JointParentIndex(ji) + jPbi := int32(-1) + parentFixed := true + if jPi >= 0 { + jPbi = DynamicBody(jPi) + parentFixed = GetJointParentFixed(ji) + } + jCi := JointChildIndex(ji) + jCbi := DynamicBody(jCi) + + jLinearN := GetJointLinearDoFN(ji) + jAngularN := GetJointAngularDoFN(ji) + + jPR := JointPPos(ji) + jPQ := JointPQuat(ji) + + xwPR := jPR // world xform, parent, pos + xwPQ := jPQ // quat + mInvP := float32(0.0) + iInvP := math32.Mat3(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0) + posePR := jPR + posePQ := jPQ + + var wP math32.Vector3 + + // parent transform and moment arm + if jPi >= 0 { + posePR = DynamicPos(jPi, params.Next) // now using next + posePQ = DynamicQuat(jPi, params.Next) + slmath.MulSpatialTransforms(posePR, posePQ, jPR, jPQ, &xwPR, &xwPQ) + mInvP = Bodies[jPbi, BodyInvMass] + iInvP = BodyInvInertia(jPbi) + wP = DynamicAngDelta(jPi, params.Next) + if mInvP == 0 { + parentFixed = true } + } - axisStiffness := axisTargetPosKeA - axisDamping := axisTargetVelKdA - axisTargetPosKeD = slmath.DivSafe3(axisTargetPosKeD, axisStiffness) - axisTargetVelKdD = slmath.DivSafe3(axisTargetVelKdD, axisDamping) - axisLimitsLower := axisLimitsD - axisLimitsUpper := axisLimitsA + // child transform and moment arm + poseCR := DynamicPos(jCi, params.Next) + poseCQ := DynamicQuat(jCi, params.Next) + jCR := JointCPos(ji) + jCQ := JointCQuat(ji) + xwCR := jCR + xwCQ := jCQ + slmath.MulSpatialTransforms(poseCR, poseCQ, jCR, jCQ, &xwCR, &xwCQ) + mInvC := Bodies[jCbi, BodyInvMass] + iInvC := BodyInvInertia(jCbi) + wC := DynamicAngDelta(jCi, params.Next) - for dim := range int32(3) { - e := slmath.Dim3(errs, dim) - - // analytic gradients of swing-twist decomposition - grad := math32.NewQuat(slmath.Dim3(gradX, dim), slmath.Dim3(gradY, dim), slmath.Dim3(gradZ, dim), slmath.Dim3(gradW, dim)) - // todo: verify -- does the 0.5 go inside?? - // quat_c = 0.5 * q_p * grad * wp.quat_inverse(q_c) - quatC := slmath.MulQuats(slmath.MulQuats(slmath.QuatMulScalar(qP, 0.5), grad), slmath.QuatInverse(qC)) - // if ji == 0 { - // fmt.Println(quatC, qP, qC, grad) - // } - - angularC := math32.Vec3(quatC.X, quatC.Y, quatC.Z) - angularP := slmath.Negate3(angularC) - // constraint time derivative - derr := slmath.Dot3(angularP, wP) + slmath.Dot3(angularC, wC) + if mInvP == 0.0 && mInvC == 0.0 { // connection between two immovable bodies + return + } - err := float32(0.0) - compliance := params.JointLinearComply - damping := float32(0.0) + // accumulate constraint deltas + var linDeltaP, angDeltaP, linDeltaC, angDeltaC math32.Vector3 - targetVel := slmath.Dim3(axisTargetVelKdD, dim) - angularClen := slmath.Length3(angularC) - derrRel := derr - targetVel*angularClen + relPoseR := xwPR + relPoseQ := xwPQ + slmath.SpatialTransformInverse(xwPR, xwPQ, &relPoseR, &relPoseQ) + slmath.MulSpatialTransforms(relPoseR, relPoseQ, xwCR, xwCQ, &relPoseR, &relPoseQ) - // consider joint limits irrespective of axis mode - lower := slmath.Dim3(axisLimitsLower, dim) - upper := slmath.Dim3(axisLimitsUpper, dim) - if e < lower { - err = e - lower - } else if e > upper { - err = e - upper - } else { - targetPos := slmath.Dim3(axisTargetPosKeD, dim) - targetPos = math32.Clamp(targetPos, lower, upper) + qP := xwPQ + qC := xwCQ + // make quats lie in same hemisphere + if slmath.QuatDot(qP, qC) < 0 { + qC = slmath.QuatMulScalar(qC, -1.0) + } + relQ := slmath.MulQuats(slmath.QuatInverse(qP), qC) + qtwist := slmath.QuatNormalize(math32.NewQuat(relQ.X, 0.0, 0.0, relQ.W)) + qswing := slmath.MulQuats(relQ, slmath.QuatInverse(qtwist)) + + // decompose to a compound rotation each axis + s := math32.Sqrt(relQ.X*relQ.X + relQ.W*relQ.W) + if s == 0 { + // fmt.Println("s = 0", relQ, qP, qC) + s = 1 + } + invs := 1.0 / s + invscube := invs * invs * invs + + // handle axis-angle joints + // rescale twist from quaternion space to angular + err0 := 2.0 * math32.Asin(math32.Clamp(qtwist.X, -1.0, 1.0)) + err1 := qswing.Y + err2 := qswing.Z + // analytic gradients of swing-twist decomposition + grad0 := math32.NewQuat(invs-relQ.X*relQ.X*invscube, 0.0, 0.0, -(relQ.W*relQ.X)*invscube) + grad1 := math32.NewQuat( + -relQ.W*(relQ.W*relQ.Z+relQ.X*relQ.Y)*invscube, + relQ.W*invs, + -relQ.X*invs, + relQ.X*(relQ.W*relQ.Z+relQ.X*relQ.Y)*invscube) + grad2 := math32.NewQuat( + relQ.W*(relQ.W*relQ.Y-relQ.X*relQ.Z)*invscube, + relQ.X*invs, + relQ.W*invs, + relQ.X*(relQ.Z*relQ.X-relQ.W*relQ.Y)*invscube) + grad0 = slmath.QuatMulScalar(grad0, 2.0/math32.Abs(qtwist.W)) + // # grad0 *= 2.0 / wp.sqrt(1.0-qtwist[0]*qtwist[0]) # derivative of asin(x) = 1/sqrt(1-x^2) + + // rescale swing + swing_sq := qswing.W * qswing.W + // if swing axis magnitude close to zero vector, just treat in quaternion space + angularEps := float32(1.0e-4) + if swing_sq+angularEps < 1.0 { + d := math32.Sqrt(1.0 - qswing.W*qswing.W) + theta := 2.0 * math32.Acos(math32.Clamp(qswing.W, -1.0, 1.0)) + scale := theta / d + err1 *= scale + err2 *= scale + grad1 = slmath.QuatMulScalar(grad1, scale) + grad2 = slmath.QuatMulScalar(grad2, scale) + } + errs := math32.Vec3(err0, err1, err2) + gradX := math32.Vec3(grad0.X, grad1.X, grad2.X) + gradY := math32.Vec3(grad0.Y, grad1.Y, grad2.Y) + gradZ := math32.Vec3(grad0.Z, grad1.Z, grad2.Z) + gradW := math32.Vec3(grad0.W, grad1.W, grad2.W) + + // compute joint target, stiffness, damping + var axisLimitsD, axisLimitsA math32.Vector3 + var axisTargetPosKeD, axisTargetPosKeA math32.Vector3 + var axisTargetVelKdD, axisTargetVelKdA math32.Vector3 + + for dof := range jAngularN { + di := dof + jLinearN + axis := JointAxis(ji, di) + JointAxisLimitsUpdate(dof, axis, JointDoF(ji, di, JointLimitLower), JointDoF(ji, di, JointLimitUpper), &axisLimitsD, &axisLimitsA) + ke := JointControl(ji, di, JointTargetStiff) + kd := JointControl(ji, di, JointTargetDamp) + targetPos := JointControl(ji, di, JointTargetPos) + targetVel := JointControl(ji, di, JointTargetVel) + if ke > 0.0 { // has position control + JointAxisTarget(axis, targetPos, ke, &axisTargetPosKeD, &axisTargetPosKeA) + } + if kd > 0.0 { // has velocity control + JointAxisTarget(axis, targetVel, kd, &axisTargetVelKdD, &axisTargetVelKdA) + } + } - ke := slmath.Dim3(axisStiffness, dim) - kd := slmath.Dim3(axisDamping, dim) - if ke > 0.0 { - err = e - targetPos - compliance = 1.0 / ke - damping = slmath.Dim3(axisDamping, dim) - } else if kd > 0.0 { - compliance = 1.0 / kd - damping = kd - } + axisStiffness := axisTargetPosKeA + axisDamping := axisTargetVelKdA + axisTargetPosKeD = slmath.DivSafe3(axisTargetPosKeD, axisStiffness) + axisTargetVelKdD = slmath.DivSafe3(axisTargetVelKdD, axisDamping) + axisLimitsLower := axisLimitsD + axisLimitsUpper := axisLimitsA + + for dim := range int32(3) { + e := slmath.Dim3(errs, dim) + + // analytic gradients of swing-twist decomposition + grad := math32.NewQuat(slmath.Dim3(gradX, dim), slmath.Dim3(gradY, dim), slmath.Dim3(gradZ, dim), slmath.Dim3(gradW, dim)) + // todo: verify -- does the 0.5 go inside?? + // quat_c = 0.5 * q_p * grad * wp.quat_inverse(q_c) + quatC := slmath.MulQuats(slmath.MulQuats(slmath.QuatMulScalar(qP, 0.5), grad), slmath.QuatInverse(qC)) + + angularC := math32.Vec3(quatC.X, quatC.Y, quatC.Z) + angularP := slmath.Negate3(angularC) + // constraint time derivative + derr := slmath.Dot3(angularP, wP) + slmath.Dot3(angularC, wC) + + err := float32(0.0) + compliance := params.JointLinearComply + damping := float32(0.0) + + targetVel := slmath.Dim3(axisTargetVelKdD, dim) + angularClen := slmath.Length3(angularC) + derrRel := derr - targetVel*angularClen + + // consider joint limits irrespective of axis mode + lower := slmath.Dim3(axisLimitsLower, dim) + upper := slmath.Dim3(axisLimitsUpper, dim) + if e < lower { + err = e - lower + } else if e > upper { + err = e - upper + } else { + targetPos := slmath.Dim3(axisTargetPosKeD, dim) + targetPos = math32.Clamp(targetPos, lower, upper) + + ke := slmath.Dim3(axisStiffness, dim) + kd := slmath.Dim3(axisDamping, dim) + if ke > 0.0 { + err = e - targetPos + compliance = 1.0 / ke + damping = slmath.Dim3(axisDamping, dim) + } else if kd > 0.0 { + compliance = 1.0 / kd + damping = kd } - lambdaIn := float32(0) - dLambda := AngularCorrection(err, derrRel, posePQ, poseCQ, iInvP, iInvC, angularP, angularC, lambdaIn, compliance, damping, params.Dt) - - // note: no relaxation factors here: - angDeltaP = angDeltaP.Add(angularP.MulScalar(dLambda)) - angDeltaC = angDeltaC.Add(angularC.MulScalar(dLambda)) - // if ji == 0 && dim == 1 { - // fmt.Println(ji, dim, e, err, lambdaIn, angularP, angularC) - // } - // if angDeltaC.IsNaN() || angDeltaP.IsNaN() { - // fmt.Println("ang joint ang:", angDeltaC, angDeltaP, angularC, angularP, dLambda) - // } } + lambdaIn := float32(0) + dLambda := AngularCorrection(err, derrRel, posePQ, poseCQ, iInvP, iInvC, angularP, angularC, lambdaIn, compliance, damping, params.Dt) + + // note: no relaxation factors here: + angDeltaP = angDeltaP.Add(angularP.MulScalar(dLambda)) + angDeltaC = angDeltaC.Add(angularC.MulScalar(dLambda)) } - // These are unique to joint: aggregate into dynamics Next in separate step. - SetJointPDelta(ji, linDeltaP) - SetJointCDelta(ji, linDeltaC) - SetJointPAngDelta(ji, angDeltaP) - SetJointCAngDelta(ji, angDeltaC) + if !parentFixed { + StepBodyDeltas(jPi, jPbi, false, 0, linDeltaP, angDeltaP) + } + if mInvC > 0 { + StepBodyDeltas(jCi, jCbi, false, 0, linDeltaC, angDeltaC) + } } func JointAxisTarget(axis math32.Vector3, targ, weight float32, axisTargets, axisWeights *math32.Vector3) { @@ -536,14 +585,14 @@ func PositionalCorrection(err, derr float32, tfaQ, tfbQ math32.Quat, mInvA, mInv return lambda } -func AngularCorrection(err, derr float32, tfaQ, tfbQ math32.Quat, iInva, iInvb math32.Matrix3, angulara, angularb math32.Vector3, lambdaIn, compliance, damping, dt float32) float32 { +func AngularCorrection(err, derr float32, tfaQ, tfbQ math32.Quat, iInvA, iInvB math32.Matrix3, angA, angB math32.Vector3, lambdaIn, compliance, damping, dt float32) float32 { // # Eq. 2-3 (make sure to project into the frame of the body) - rotAngulara := slmath.MulQuatVectorInverse(tfaQ, angulara) - rotAngularb := slmath.MulQuatVectorInverse(tfbQ, angularb) + rotAngA := slmath.MulQuatVectorInverse(tfaQ, angA) + rotAngB := slmath.MulQuatVectorInverse(tfbQ, angB) denom := float32(0.0) - denom += slmath.Dot3(rotAngulara, iInva.MulVector3(rotAngulara)) - denom += slmath.Dot3(rotAngularb, iInvb.MulVector3(rotAngularb)) + denom += slmath.Dot3(rotAngA, iInvA.MulVector3(rotAngA)) + denom += slmath.Dot3(rotAngB, iInvB.MulVector3(rotAngB)) alpha := compliance gamma := compliance * damping diff --git a/physics/typegen.go b/physics/typegen.go index 2d1d2e4a..03f17c13 100644 --- a/physics/typegen.go +++ b/physics/typegen.go @@ -22,9 +22,9 @@ var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.JointVars", var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.JointDoFVars", IDName: "joint-do-f-vars", Doc: "JointDoFVars are joint DoF state variables stored in tensor.Float32,\none for each DoF."}) -var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.Model", IDName: "model", Doc: "Model contains and manages all of the physics elements.", Directives: []types.Directive{{Tool: "go", Directive: "generate", Args: []string{"core", "generate", "-add-types", "-gosl"}}}, Fields: []types.Field{{Name: "GPU", Doc: "GPU determines whether to use GPU (else CPU)."}, {Name: "Params", Doc: "Params are global parameters."}, {Name: "CurrentWorld", Doc: "CurrentWorld is the [BodyWorld] value to use when creating new bodies.\nSet to -1 to create global elements that interact with everything,\nwhile 0 and positive numbers only interact amongst themselves."}, {Name: "ReplicasStart", Doc: "ReplicasStart is the starting body index for replicated world bodies,\nwhich is needed for viewers to efficiently select a specific world to view.\nThis is the start of the World=0 first instance."}, {Name: "ReplicasN", Doc: "ReplicasN is the number of body elements within each set of\nreplicated world bodies, which is needed for viewers to efficiently select\na specific world to view."}, {Name: "Bodies", Doc: "Bodies are the rigid body elements (dynamic and static),\nspecifying the constant, non-dynamic properties,\nwhich is initial state for dynamics.\n[body][BodyVarsN]"}, {Name: "Joints", Doc: "Joints is a list of permanent joints connecting bodies,\nwhich do not change (no dynamic variables).\n[joint][JointVarsN]"}, {Name: "JointDoFs", Doc: "JointDoFs is a list of joint DoF parameters, allocated per joint.\n[dof][JointDoFVars]"}, {Name: "BodyJoints", Doc: "BodyJoints is a list of joint indexes for each dynamic body, for aggregating.\n[dyn body][parent, child][Params.BodyJointsMax]"}, {Name: "BodyCollidePairs", Doc: "BodyCollidePairs are pairs of Body indexes that could potentially collide\nbased on precomputed collision logic, using World, Group, and Joint indexes.\n[BodyCollidePairsN][2]"}, {Name: "Dynamics", Doc: "Dynamics are the dynamic rigid body elements: these actually move.\nThe first set of variables are for initial values, and the second current.\n[body][cur/next][DynamicVarsN]"}, {Name: "BroadContactsN", Doc: "BroadContactsN has number of points of broad contact\nbetween bodies. [1]"}, {Name: "BroadContacts", Doc: "BroadContacts are the results of broad-phase contact processing,\nestablishing possible points of contact between bodies.\n[ContactsMax][BroadContactVarsN]"}, {Name: "ContactsN", Doc: "ContactsN has number of points of narrow (final) contact\nbetween bodies. [1]"}, {Name: "Contacts", Doc: "Contacts are the results of narrow-phase contact processing,\nwhere only actual contacts with fully-specified values are present.\n[ContactsMax][ContactVarsN]"}, {Name: "JointControls", Doc: "JointControls are dynamic joint control inputs, per joint DoF\n(in correspondence with [JointDoFs]). This can be uploaded to the\nGPU at every step.\n[dof][JointControlVarsN]"}}}) +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.Model", IDName: "model", Doc: "Model contains and manages all of the physics elements.", Directives: []types.Directive{{Tool: "go", Directive: "generate", Args: []string{"core", "generate", "-add-types", "-gosl"}}}, Fields: []types.Field{{Name: "GPU", Doc: "GPU determines whether to use GPU (else CPU)."}, {Name: "Params", Doc: "Params are global parameters."}, {Name: "CurrentWorld", Doc: "CurrentWorld is the [BodyWorld] value to use when creating new bodies.\nSet to -1 to create global elements that interact with everything,\nwhile 0 and positive numbers only interact amongst themselves."}, {Name: "CurrentObject", Doc: "CurrentObject is the Object to use when creating new joints.\nCall NewObject to increment."}, {Name: "CurrentObjectJoint", Doc: "CurrentObjectJoint is the Joint index in CurrentObject\nto use when creating new joints."}, {Name: "ReplicasStart", Doc: "ReplicasStart is the starting body index for replicated world bodies,\nwhich is needed for viewers to efficiently select a specific world to view.\nThis is the start of the World=0 first instance."}, {Name: "ReplicasN", Doc: "ReplicasN is the number of body elements within each set of\nreplicated world bodies, which is needed for viewers to efficiently select\na specific world to view."}, {Name: "Bodies", Doc: "Bodies are the rigid body elements (dynamic and static),\nspecifying the constant, non-dynamic properties,\nwhich is initial state for dynamics.\n[body][BodyVarsN]"}, {Name: "Objects", Doc: "Objects is a list of joint indexes for each object, where each object\ncontains all the joints interconnecting an overlapping set of bodies.\nJoints must be added in parent -> child order within objects, as joints\nare updated in sequential order within object.\n[object][maxjointsperobj]"}, {Name: "BodyJoints", Doc: "BodyJoints is a list of joint indexes for each dynamic body, for aggregating.\n[dyn body][parent, child][Params.BodyJointsMax]"}, {Name: "Joints", Doc: "Joints is a list of permanent joints connecting bodies,\nwhich do not change (no dynamic variables).\n[joint][JointVarsN]"}, {Name: "JointDoFs", Doc: "JointDoFs is a list of joint DoF parameters, allocated per joint.\n[dof][JointDoFVars]"}, {Name: "BodyCollidePairs", Doc: "BodyCollidePairs are pairs of Body indexes that could potentially collide\nbased on precomputed collision logic, using World, Group, and Joint indexes.\n[BodyCollidePairsN][2]"}, {Name: "Dynamics", Doc: "Dynamics are the dynamic rigid body elements: these actually move.\nThe first set of variables are for initial values, and the second current.\n[body][cur/next][DynamicVarsN]"}, {Name: "BroadContactsN", Doc: "BroadContactsN has number of points of broad contact\nbetween bodies. [1]"}, {Name: "BroadContacts", Doc: "BroadContacts are the results of broad-phase contact processing,\nestablishing possible points of contact between bodies.\n[ContactsMax][BroadContactVarsN]"}, {Name: "ContactsN", Doc: "ContactsN has number of points of narrow (final) contact\nbetween bodies. [1]"}, {Name: "Contacts", Doc: "Contacts are the results of narrow-phase contact processing,\nwhere only actual contacts with fully-specified values are present.\n[ContactsMax][ContactVarsN]"}, {Name: "JointControls", Doc: "JointControls are dynamic joint control inputs, per joint DoF\n(in correspondence with [JointDoFs]). This can be uploaded to the\nGPU at every step.\n[dof][JointControlVarsN]"}}}) -var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.PhysParams", IDName: "phys-params", Doc: "PhysParams are the physics parameters", Directives: []types.Directive{{Tool: "gosl", Directive: "start"}}, Fields: []types.Field{{Name: "Iterations", Doc: "Iterations is the number of integration iterations to perform\nwithin each solver step. Muller et al (2020) report that 1 is best."}, {Name: "Dt", Doc: "Dt is the integration stepsize.\nFor highly kinetic situations (e.g., rapidly moving bouncing balls)\n0.0001 is needed to ensure contact registration. Use SubSteps to\naccomplish a target effective read-out step size."}, {Name: "SubSteps", Doc: "SubSteps is the number of integration steps to take per Step()\nfunction call. These sub steps are taken without any sync to/from\nthe GPU and are therefore much faster."}, {Name: "ContactMargin", Doc: "Contact margin is the extra distance for broadphase collision\naround rigid bodies. This can make some joints potentially unstable if > 0"}, {Name: "ContactRelax", Doc: "ContactRelax is rigid contact relaxation constant.\nHigher values cause errros"}, {Name: "ContactWeighting", Doc: "Contact weighting: balances contact forces?"}, {Name: "Restitution", Doc: "Restitution takes into account bounciness of objects."}, {Name: "JointLinearRelax", Doc: "JointLinearRelax is joint linear relaxation constant."}, {Name: "JointAngularRelax", Doc: "JointAngularRelax is joint angular relaxation constant."}, {Name: "JointLinearComply", Doc: "JointLinearComply is joint linear compliance constant."}, {Name: "JointAngularComply", Doc: "JointAngularComply is joint angular compliance constant."}, {Name: "AngularDamping", Doc: "AngularDamping is damping of angular motion."}, {Name: "SoftRelax", Doc: "SoftRelax is soft-body relaxation constant."}, {Name: "MaxForce", Doc: "MaxForce is the maximum computed force value, which prevents\nrunaway numerical overflow."}, {Name: "MaxGeomIter", Doc: "MaxGeomIter is number of iterations to perform in shape-based\ngeometry collision computations"}, {Name: "ContactsMax", Doc: "Maximum number of contacts to process at any given point."}, {Name: "Cur", Doc: "Index for the current state (0 or 1, alternates with Next)."}, {Name: "Next", Doc: "Index for the next state (1 or 0, alternates with Cur)."}, {Name: "BodiesN", Doc: "BodiesN is number of rigid bodies."}, {Name: "DynamicsN", Doc: "DynamicsN is number of dynamics bodies."}, {Name: "JointsN", Doc: "JointsN is number of joints."}, {Name: "JointDoFsN", Doc: "JointDoFsN is number of joint DoFs."}, {Name: "BodyJointsMax", Doc: "BodyJointsMax is max number of joints per body + 1 for actual n."}, {Name: "BodyCollidePairsN", Doc: "BodyCollidePairsN is the total number of pre-compiled collision pairs\nto examine."}, {Name: "Gravity", Doc: "Gravity is the gravity acceleration function"}}}) +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.PhysParams", IDName: "phys-params", Doc: "PhysParams are the physics parameters", Directives: []types.Directive{{Tool: "gosl", Directive: "start"}}, Fields: []types.Field{{Name: "Iterations", Doc: "Iterations is the number of integration iterations to perform\nwithin each solver step. Muller et al (2020) report that 1 is best."}, {Name: "Dt", Doc: "Dt is the integration stepsize.\nFor highly kinetic situations (e.g., rapidly moving bouncing balls)\n0.0001 is needed to ensure contact registration. Use SubSteps to\naccomplish a target effective read-out step size."}, {Name: "SubSteps", Doc: "SubSteps is the number of integration steps to take per Step()\nfunction call. These sub steps are taken without any sync to/from\nthe GPU and are therefore much faster."}, {Name: "ContactMargin", Doc: "Contact margin is the extra distance for broadphase collision\naround rigid bodies. This can make some joints potentially unstable if > 0"}, {Name: "ContactRelax", Doc: "ContactRelax is rigid contact relaxation constant.\nHigher values cause errros"}, {Name: "ContactWeighting", Doc: "Contact weighting: balances contact forces?"}, {Name: "Restitution", Doc: "Restitution takes into account bounciness of objects."}, {Name: "JointLinearRelax", Doc: "JointLinearRelax is joint linear relaxation constant."}, {Name: "JointAngularRelax", Doc: "JointAngularRelax is joint angular relaxation constant."}, {Name: "JointLinearComply", Doc: "JointLinearComply is joint linear compliance constant."}, {Name: "JointAngularComply", Doc: "JointAngularComply is joint angular compliance constant."}, {Name: "AngularDamping", Doc: "AngularDamping is damping of angular motion."}, {Name: "SoftRelax", Doc: "SoftRelax is soft-body relaxation constant."}, {Name: "MaxForce", Doc: "MaxForce is the maximum computed force value, which prevents\nrunaway numerical overflow."}, {Name: "MaxGeomIter", Doc: "MaxGeomIter is number of iterations to perform in shape-based\ngeometry collision computations"}, {Name: "ContactsMax", Doc: "Maximum number of contacts to process at any given point."}, {Name: "Cur", Doc: "Index for the current state (0 or 1, alternates with Next)."}, {Name: "Next", Doc: "Index for the next state (1 or 0, alternates with Cur)."}, {Name: "BodiesN", Doc: "BodiesN is number of rigid bodies."}, {Name: "DynamicsN", Doc: "DynamicsN is number of dynamics bodies."}, {Name: "ObjectsN", Doc: "ObjectsN is number of objects."}, {Name: "MaxObjectJoints", Doc: "MaxObjectJoints is max number of joints per object."}, {Name: "JointsN", Doc: "JointsN is number of joints."}, {Name: "JointDoFsN", Doc: "JointDoFsN is number of joint DoFs."}, {Name: "BodyJointsMax", Doc: "BodyJointsMax is max number of joints per body + 1 for actual n."}, {Name: "BodyCollidePairsN", Doc: "BodyCollidePairsN is the total number of pre-compiled collision pairs\nto examine."}, {Name: "pad"}, {Name: "pad1"}, {Name: "Gravity", Doc: "Gravity is the gravity acceleration function"}}}) var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.GeomData", IDName: "geom-data", Doc: "GeomData contains all geometric data for narrow-phase collision.", Directives: []types.Directive{{Tool: "gosl", Directive: "start"}}, Fields: []types.Field{{Name: "BodyIdx"}, {Name: "Shape"}, {Name: "MinSize", Doc: "MinSize is the min of the Size dimensions."}, {Name: "Thick", Doc: "Thickness of shape."}, {Name: "Radius", Doc: "Radius is the effective radius for sphere-like elements (Sphere, Capsule, Cone)"}, {Name: "Size"}, {Name: "WbR", Doc: "World-to-Body transform\nPosition (R) (i.e., BodyPos)"}, {Name: "WbQ", Doc: "Quaternion (Q) (i.e., BodyQuat)"}, {Name: "BwR", Doc: "Body-to-World transform (inverse)\nPosition (R)"}, {Name: "BwQ", Doc: "Quaternion (Q)"}}}) diff --git a/physics/vars.go b/physics/vars.go index 0744e56f..9dbd3fb8 100644 --- a/physics/vars.go +++ b/physics/vars.go @@ -30,6 +30,19 @@ var ( //gosl:dims 2 Bodies *tensor.Float32 + // Objects is a list of joint indexes for each object, where each object + // contains all the joints interconnecting an overlapping set of bodies. + // Joints must be added in parent -> child order within objects, as joints + // are updated in sequential order within object. First element is n joints. + // [object][MaxJointsPerObject+1] + //gosl:dims 2 + Objects *tensor.Int32 + + // BodyJoints is a list of joint indexes for each dynamic body, for aggregating. + // [dyn body][parent, child][maxjointsperbody] + //gosl:dims 3 + BodyJoints *tensor.Int32 + // Joints is a list of permanent joints connecting bodies, // which do not change (no dynamic variables, except temps). // [joint][JointVars] @@ -41,11 +54,6 @@ var ( //gosl:dims 2 JointDoFs *tensor.Float32 - // BodyJoints is a list of joint indexes for each dynamic body, for aggregating. - // [dyn body][parent, child][maxjointsperbody] - //gosl:dims 3 - BodyJoints *tensor.Int32 - // BodyCollidePairs are pairs of Body indexes that could potentially collide // based on precomputed collision logic, using World, Group, and Joint indexes. // [BodyCollidePairsN][2] diff --git a/yaegilab/labsymbols/cogentcore_org-lab-physics-builder.go b/yaegilab/labsymbols/cogentcore_org-lab-physics-builder.go index 7db80314..b1af4a00 100644 --- a/yaegilab/labsymbols/cogentcore_org-lab-physics-builder.go +++ b/yaegilab/labsymbols/cogentcore_org-lab-physics-builder.go @@ -14,13 +14,14 @@ func init() { "NewBuilder": reflect.ValueOf(builder.NewBuilder), // type definitions - "Body": reflect.ValueOf((*builder.Body)(nil)), - "Builder": reflect.ValueOf((*builder.Builder)(nil)), - "DoF": reflect.ValueOf((*builder.DoF)(nil)), - "Joint": reflect.ValueOf((*builder.Joint)(nil)), - "Object": reflect.ValueOf((*builder.Object)(nil)), - "Physics": reflect.ValueOf((*builder.Physics)(nil)), - "Pose": reflect.ValueOf((*builder.Pose)(nil)), - "World": reflect.ValueOf((*builder.World)(nil)), + "Body": reflect.ValueOf((*builder.Body)(nil)), + "Builder": reflect.ValueOf((*builder.Builder)(nil)), + "Controls": reflect.ValueOf((*builder.Controls)(nil)), + "DoF": reflect.ValueOf((*builder.DoF)(nil)), + "Joint": reflect.ValueOf((*builder.Joint)(nil)), + "Object": reflect.ValueOf((*builder.Object)(nil)), + "Physics": reflect.ValueOf((*builder.Physics)(nil)), + "Pose": reflect.ValueOf((*builder.Pose)(nil)), + "World": reflect.ValueOf((*builder.World)(nil)), } } diff --git a/yaegilab/labsymbols/cogentcore_org-lab-physics.go b/yaegilab/labsymbols/cogentcore_org-lab-physics.go index 3f7b4e3d..c0609d8c 100644 --- a/yaegilab/labsymbols/cogentcore_org-lab-physics.go +++ b/yaegilab/labsymbols/cogentcore_org-lab-physics.go @@ -234,6 +234,7 @@ func init() { "GetJointAngularDoFN": reflect.ValueOf(physics.GetJointAngularDoFN), "GetJointEnabled": reflect.ValueOf(physics.GetJointEnabled), "GetJointLinearDoFN": reflect.ValueOf(physics.GetJointLinearDoFN), + "GetJointParentFixed": reflect.ValueOf(physics.GetJointParentFixed), "GetJointTargetPos": reflect.ValueOf(physics.GetJointTargetPos), "GetJointTargetVel": reflect.ValueOf(physics.GetJointTargetVel), "GetJointType": reflect.ValueOf(physics.GetJointType), @@ -249,14 +250,6 @@ func init() { "JointAxisX": reflect.ValueOf(physics.JointAxisX), "JointAxisY": reflect.ValueOf(physics.JointAxisY), "JointAxisZ": reflect.ValueOf(physics.JointAxisZ), - "JointCAngDelta": reflect.ValueOf(physics.JointCAngDelta), - "JointCAngDeltaX": reflect.ValueOf(physics.JointCAngDeltaX), - "JointCAngDeltaY": reflect.ValueOf(physics.JointCAngDeltaY), - "JointCAngDeltaZ": reflect.ValueOf(physics.JointCAngDeltaZ), - "JointCDelta": reflect.ValueOf(physics.JointCDelta), - "JointCDeltaX": reflect.ValueOf(physics.JointCDeltaX), - "JointCDeltaY": reflect.ValueOf(physics.JointCDeltaY), - "JointCDeltaZ": reflect.ValueOf(physics.JointCDeltaZ), "JointCForce": reflect.ValueOf(physics.JointCForce), "JointCForceX": reflect.ValueOf(physics.JointCForceX), "JointCForceY": reflect.ValueOf(physics.JointCForceY), @@ -299,14 +292,6 @@ func init() { "JointLimitUnlimited": reflect.ValueOf(constant.MakeFromLiteral("10000000000", token.FLOAT, 0)), "JointLimitUpper": reflect.ValueOf(physics.JointLimitUpper), "JointLinearDoFN": reflect.ValueOf(physics.JointLinearDoFN), - "JointPAngDelta": reflect.ValueOf(physics.JointPAngDelta), - "JointPAngDeltaX": reflect.ValueOf(physics.JointPAngDeltaX), - "JointPAngDeltaY": reflect.ValueOf(physics.JointPAngDeltaY), - "JointPAngDeltaZ": reflect.ValueOf(physics.JointPAngDeltaZ), - "JointPDelta": reflect.ValueOf(physics.JointPDelta), - "JointPDeltaX": reflect.ValueOf(physics.JointPDeltaX), - "JointPDeltaY": reflect.ValueOf(physics.JointPDeltaY), - "JointPDeltaZ": reflect.ValueOf(physics.JointPDeltaZ), "JointPForce": reflect.ValueOf(physics.JointPForce), "JointPForceX": reflect.ValueOf(physics.JointPForceX), "JointPForceY": reflect.ValueOf(physics.JointPForceY), @@ -325,6 +310,7 @@ func init() { "JointPTorqueY": reflect.ValueOf(physics.JointPTorqueY), "JointPTorqueZ": reflect.ValueOf(physics.JointPTorqueZ), "JointParent": reflect.ValueOf(physics.JointParent), + "JointParentFixed": reflect.ValueOf(physics.JointParentFixed), "JointParentIndex": reflect.ValueOf(physics.JointParentIndex), "JointTargetDamp": reflect.ValueOf(physics.JointTargetDamp), "JointTargetPos": reflect.ValueOf(physics.JointTargetPos), @@ -339,12 +325,15 @@ func init() { "JointsVar": reflect.ValueOf(physics.JointsVar), "NewGeomData": reflect.ValueOf(physics.NewGeomData), "NewModel": reflect.ValueOf(physics.NewModel), + "Objects": reflect.ValueOf(&physics.Objects).Elem(), + "ObjectsVar": reflect.ValueOf(physics.ObjectsVar), "OneIfNonzero": reflect.ValueOf(physics.OneIfNonzero), "Params": reflect.ValueOf(&physics.Params).Elem(), "ParamsVar": reflect.ValueOf(physics.ParamsVar), "Plane": reflect.ValueOf(physics.Plane), "PlaneEdge": reflect.ValueOf(physics.PlaneEdge), "PlaneSDF": reflect.ValueOf(physics.PlaneSDF), + "PlaneXZ": reflect.ValueOf(physics.PlaneXZ), "PositionalCorrection": reflect.ValueOf(physics.PositionalCorrection), "Prismatic": reflect.ValueOf(physics.Prismatic), "ReadFromGPU": reflect.ValueOf(physics.ReadFromGPU), @@ -373,7 +362,6 @@ func init() { "RunOneInitDynamics": reflect.ValueOf(physics.RunOneInitDynamics), "RunOneStepBodyContactDeltas": reflect.ValueOf(physics.RunOneStepBodyContactDeltas), "RunOneStepBodyContacts": reflect.ValueOf(physics.RunOneStepBodyContacts), - "RunOneStepBodyJointDeltas": reflect.ValueOf(physics.RunOneStepBodyJointDeltas), "RunOneStepInit": reflect.ValueOf(physics.RunOneStepInit), "RunOneStepIntegrateBodies": reflect.ValueOf(physics.RunOneStepIntegrateBodies), "RunOneStepJointForces": reflect.ValueOf(physics.RunOneStepJointForces), @@ -384,9 +372,6 @@ func init() { "RunStepBodyContacts": reflect.ValueOf(physics.RunStepBodyContacts), "RunStepBodyContactsCPU": reflect.ValueOf(physics.RunStepBodyContactsCPU), "RunStepBodyContactsGPU": reflect.ValueOf(physics.RunStepBodyContactsGPU), - "RunStepBodyJointDeltas": reflect.ValueOf(physics.RunStepBodyJointDeltas), - "RunStepBodyJointDeltasCPU": reflect.ValueOf(physics.RunStepBodyJointDeltasCPU), - "RunStepBodyJointDeltasGPU": reflect.ValueOf(physics.RunStepBodyJointDeltasGPU), "RunStepInit": reflect.ValueOf(physics.RunStepInit), "RunStepInitCPU": reflect.ValueOf(physics.RunStepInitCPU), "RunStepInitGPU": reflect.ValueOf(physics.RunStepInitGPU), @@ -443,8 +428,6 @@ func init() { "SetJointAngularDoFN": reflect.ValueOf(physics.SetJointAngularDoFN), "SetJointAxis": reflect.ValueOf(physics.SetJointAxis), "SetJointAxisDoF": reflect.ValueOf(physics.SetJointAxisDoF), - "SetJointCAngDelta": reflect.ValueOf(physics.SetJointCAngDelta), - "SetJointCDelta": reflect.ValueOf(physics.SetJointCDelta), "SetJointCForce": reflect.ValueOf(physics.SetJointCForce), "SetJointCPos": reflect.ValueOf(physics.SetJointCPos), "SetJointCQuat": reflect.ValueOf(physics.SetJointCQuat), @@ -456,13 +439,12 @@ func init() { "SetJointDoFIndex": reflect.ValueOf(physics.SetJointDoFIndex), "SetJointEnabled": reflect.ValueOf(physics.SetJointEnabled), "SetJointLinearDoFN": reflect.ValueOf(physics.SetJointLinearDoFN), - "SetJointPAngDelta": reflect.ValueOf(physics.SetJointPAngDelta), - "SetJointPDelta": reflect.ValueOf(physics.SetJointPDelta), "SetJointPForce": reflect.ValueOf(physics.SetJointPForce), "SetJointPPos": reflect.ValueOf(physics.SetJointPPos), "SetJointPQuat": reflect.ValueOf(physics.SetJointPQuat), "SetJointPTorque": reflect.ValueOf(physics.SetJointPTorque), "SetJointParent": reflect.ValueOf(physics.SetJointParent), + "SetJointParentFixed": reflect.ValueOf(physics.SetJointParentFixed), "SetJointTargetAngle": reflect.ValueOf(physics.SetJointTargetAngle), "SetJointTargetPos": reflect.ValueOf(physics.SetJointTargetPos), "SetJointTargetVel": reflect.ValueOf(physics.SetJointTargetVel), @@ -475,10 +457,11 @@ func init() { "StepBodyContactDeltas": reflect.ValueOf(physics.StepBodyContactDeltas), "StepBodyContacts": reflect.ValueOf(physics.StepBodyContacts), "StepBodyDeltas": reflect.ValueOf(physics.StepBodyDeltas), - "StepBodyJointDeltas": reflect.ValueOf(physics.StepBodyJointDeltas), "StepInit": reflect.ValueOf(physics.StepInit), "StepIntegrateBodies": reflect.ValueOf(physics.StepIntegrateBodies), "StepJointForces": reflect.ValueOf(physics.StepJointForces), + "StepSolveJointAngular": reflect.ValueOf(physics.StepSolveJointAngular), + "StepSolveJointLinear": reflect.ValueOf(physics.StepSolveJointLinear), "StepSolveJoints": reflect.ValueOf(physics.StepSolveJoints), "StepsToMsec": reflect.ValueOf(physics.StepsToMsec), "SyncFromGPU": reflect.ValueOf(physics.SyncFromGPU), From e1616fd02764e4560d29214c01a0fe37cac6e5ec Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Sat, 3 Jan 2026 22:06:09 -0800 Subject: [PATCH 72/97] physics: integrating lambda over time: not obviously beneficial. --- gosl/slmath/math.go | 24 +++++++++++ physics/enumgen.go | 10 ++--- physics/examples/virtroom/virtroom.go | 4 +- physics/joint.go | 30 +++++++++++++ physics/joint.goal | 30 +++++++++++++ physics/shaders/CollisionBroad.wgsl | 11 ++++- physics/shaders/CollisionNarrow.wgsl | 11 ++++- physics/shaders/DynamicsCurToNext.wgsl | 11 ++++- physics/shaders/ForcesFromJoints.wgsl | 11 ++++- physics/shaders/InitDynamics.wgsl | 11 ++++- physics/shaders/StepBodyContactDeltas.wgsl | 11 ++++- physics/shaders/StepBodyContacts.wgsl | 11 ++++- physics/shaders/StepIntegrateBodies.wgsl | 11 ++++- physics/shaders/StepJointForces.wgsl | 14 ++++-- physics/shaders/StepSolveJoints.wgsl | 50 ++++++++++++++++++++-- physics/step_joint.go | 23 ++++++++-- physics/step_joint.goal | 23 ++++++++-- 17 files changed, 269 insertions(+), 27 deletions(-) create mode 100644 gosl/slmath/math.go diff --git a/gosl/slmath/math.go b/gosl/slmath/math.go new file mode 100644 index 00000000..5c659648 --- /dev/null +++ b/gosl/slmath/math.go @@ -0,0 +1,24 @@ +// Copyright (c) 2025, Cogent Core. All rights reserved. +// Use of this source code is governed by a BSD-style +// license that can be found in the LICENSE file. + +package slmath + +//gosl:start + +const Pi = 3.141592653589793 + +// MinAngleDiff returns the minimum difference between two angles +// (in radians): a-b +func MinAngleDiff(a, b float32) float32 { + d := a - b + if d > Pi { + d -= 2 * Pi + } + if d < -Pi { + d += 2 * Pi + } + return d +} + +//gosl:end diff --git a/physics/enumgen.go b/physics/enumgen.go index abc1132f..4cbc4b73 100644 --- a/physics/enumgen.go +++ b/physics/enumgen.go @@ -284,20 +284,20 @@ func (i *JointTypes) UnmarshalText(text []byte) error { return enums.UnmarshalText(i, text, "JointTypes") } -var _JointVarsValues = []JointVars{0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38} +var _JointVarsValues = []JointVars{0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44} // JointVarsN is the highest valid value for type JointVars, plus one. // //gosl:start -const JointVarsN JointVars = 39 +const JointVarsN JointVars = 45 //gosl:end -var _JointVarsValueMap = map[string]JointVars{`JointType`: 0, `JointEnabled`: 1, `JointParentFixed`: 2, `JointParent`: 3, `JointChild`: 4, `JointPPosX`: 5, `JointPPosY`: 6, `JointPPosZ`: 7, `JointPQuatX`: 8, `JointPQuatY`: 9, `JointPQuatZ`: 10, `JointPQuatW`: 11, `JointCPosX`: 12, `JointCPosY`: 13, `JointCPosZ`: 14, `JointCQuatX`: 15, `JointCQuatY`: 16, `JointCQuatZ`: 17, `JointCQuatW`: 18, `JointLinearDoFN`: 19, `JointAngularDoFN`: 20, `JointDoF1`: 21, `JointDoF2`: 22, `JointDoF3`: 23, `JointDoF4`: 24, `JointDoF5`: 25, `JointDoF6`: 26, `JointPForceX`: 27, `JointPForceY`: 28, `JointPForceZ`: 29, `JointPTorqueX`: 30, `JointPTorqueY`: 31, `JointPTorqueZ`: 32, `JointCForceX`: 33, `JointCForceY`: 34, `JointCForceZ`: 35, `JointCTorqueX`: 36, `JointCTorqueY`: 37, `JointCTorqueZ`: 38} +var _JointVarsValueMap = map[string]JointVars{`JointType`: 0, `JointEnabled`: 1, `JointParentFixed`: 2, `JointParent`: 3, `JointChild`: 4, `JointPPosX`: 5, `JointPPosY`: 6, `JointPPosZ`: 7, `JointPQuatX`: 8, `JointPQuatY`: 9, `JointPQuatZ`: 10, `JointPQuatW`: 11, `JointCPosX`: 12, `JointCPosY`: 13, `JointCPosZ`: 14, `JointCQuatX`: 15, `JointCQuatY`: 16, `JointCQuatZ`: 17, `JointCQuatW`: 18, `JointLinearDoFN`: 19, `JointAngularDoFN`: 20, `JointDoF1`: 21, `JointDoF2`: 22, `JointDoF3`: 23, `JointDoF4`: 24, `JointDoF5`: 25, `JointDoF6`: 26, `JointPForceX`: 27, `JointPForceY`: 28, `JointPForceZ`: 29, `JointPTorqueX`: 30, `JointPTorqueY`: 31, `JointPTorqueZ`: 32, `JointCForceX`: 33, `JointCForceY`: 34, `JointCForceZ`: 35, `JointCTorqueX`: 36, `JointCTorqueY`: 37, `JointCTorqueZ`: 38, `JointLinLambdaX`: 39, `JointLinLambdaY`: 40, `JointLinLambdaZ`: 41, `JointAngLambdaX`: 42, `JointAngLambdaY`: 43, `JointAngLambdaZ`: 44} -var _JointVarsDescMap = map[JointVars]string{0: `JointType (as an int32 from bits).`, 1: `JointEnabled allows joints to be dynamically enabled.`, 2: `JointParentFixed means that the parent is NOT updated based on the forces and positions for this joint. This can make dynamics cleaner when full accuracy is not necessary.`, 3: `JointParent is the dynamic body index for parent body. Can be -1 for a fixed parent for absolute anchor.`, 4: `JointChild is the dynamic body index for child body.`, 5: `relative position of joint, in parent frame. This is prior to parent body rotation.`, 6: ``, 7: ``, 8: `relative orientation of joint, in parent frame. This is prior to parent body rotation.`, 9: ``, 10: ``, 11: ``, 12: `relative position of joint, in child frame. This is prior to child body rotation.`, 13: ``, 14: ``, 15: `relative orientation of joint, in child frame. This is prior to parent body rotation.`, 16: ``, 17: ``, 18: ``, 19: `JointLinearDoFN is the number of linear degrees-of-freedom for the joint.`, 20: `JointAngularDoFN is the number of angular degrees-of-freedom for the joint.`, 21: `indexes in JointDoFs for each DoF`, 22: ``, 23: ``, 24: `angular starts here for Free, Distance, D6`, 25: ``, 26: ``, 27: `Computed parent joint force value.`, 28: ``, 29: ``, 30: `Computed parent joint torque value.`, 31: ``, 32: ``, 33: `Computed child joint force value.`, 34: ``, 35: ``, 36: `Computed child joint torque value.`, 37: ``, 38: ``} +var _JointVarsDescMap = map[JointVars]string{0: `JointType (as an int32 from bits).`, 1: `JointEnabled allows joints to be dynamically enabled.`, 2: `JointParentFixed means that the parent is NOT updated based on the forces and positions for this joint. This can make dynamics cleaner when full accuracy is not necessary.`, 3: `JointParent is the dynamic body index for parent body. Can be -1 for a fixed parent for absolute anchor.`, 4: `JointChild is the dynamic body index for child body.`, 5: `relative position of joint, in parent frame. This is prior to parent body rotation.`, 6: ``, 7: ``, 8: `relative orientation of joint, in parent frame. This is prior to parent body rotation.`, 9: ``, 10: ``, 11: ``, 12: `relative position of joint, in child frame. This is prior to child body rotation.`, 13: ``, 14: ``, 15: `relative orientation of joint, in child frame. This is prior to parent body rotation.`, 16: ``, 17: ``, 18: ``, 19: `JointLinearDoFN is the number of linear degrees-of-freedom for the joint.`, 20: `JointAngularDoFN is the number of angular degrees-of-freedom for the joint.`, 21: `indexes in JointDoFs for each DoF`, 22: ``, 23: ``, 24: `angular starts here for Free, Distance, D6`, 25: ``, 26: ``, 27: `Computed parent joint force value.`, 28: ``, 29: ``, 30: `Computed parent joint torque value.`, 31: ``, 32: ``, 33: `Computed child joint force value.`, 34: ``, 35: ``, 36: `Computed child joint torque value.`, 37: ``, 38: ``, 39: `Computed linear lambdas.`, 40: ``, 41: ``, 42: `Computed angular lambdas.`, 43: ``, 44: ``} -var _JointVarsMap = map[JointVars]string{0: `JointType`, 1: `JointEnabled`, 2: `JointParentFixed`, 3: `JointParent`, 4: `JointChild`, 5: `JointPPosX`, 6: `JointPPosY`, 7: `JointPPosZ`, 8: `JointPQuatX`, 9: `JointPQuatY`, 10: `JointPQuatZ`, 11: `JointPQuatW`, 12: `JointCPosX`, 13: `JointCPosY`, 14: `JointCPosZ`, 15: `JointCQuatX`, 16: `JointCQuatY`, 17: `JointCQuatZ`, 18: `JointCQuatW`, 19: `JointLinearDoFN`, 20: `JointAngularDoFN`, 21: `JointDoF1`, 22: `JointDoF2`, 23: `JointDoF3`, 24: `JointDoF4`, 25: `JointDoF5`, 26: `JointDoF6`, 27: `JointPForceX`, 28: `JointPForceY`, 29: `JointPForceZ`, 30: `JointPTorqueX`, 31: `JointPTorqueY`, 32: `JointPTorqueZ`, 33: `JointCForceX`, 34: `JointCForceY`, 35: `JointCForceZ`, 36: `JointCTorqueX`, 37: `JointCTorqueY`, 38: `JointCTorqueZ`} +var _JointVarsMap = map[JointVars]string{0: `JointType`, 1: `JointEnabled`, 2: `JointParentFixed`, 3: `JointParent`, 4: `JointChild`, 5: `JointPPosX`, 6: `JointPPosY`, 7: `JointPPosZ`, 8: `JointPQuatX`, 9: `JointPQuatY`, 10: `JointPQuatZ`, 11: `JointPQuatW`, 12: `JointCPosX`, 13: `JointCPosY`, 14: `JointCPosZ`, 15: `JointCQuatX`, 16: `JointCQuatY`, 17: `JointCQuatZ`, 18: `JointCQuatW`, 19: `JointLinearDoFN`, 20: `JointAngularDoFN`, 21: `JointDoF1`, 22: `JointDoF2`, 23: `JointDoF3`, 24: `JointDoF4`, 25: `JointDoF5`, 26: `JointDoF6`, 27: `JointPForceX`, 28: `JointPForceY`, 29: `JointPForceZ`, 30: `JointPTorqueX`, 31: `JointPTorqueY`, 32: `JointPTorqueZ`, 33: `JointCForceX`, 34: `JointCForceY`, 35: `JointCForceZ`, 36: `JointCTorqueX`, 37: `JointCTorqueY`, 38: `JointCTorqueZ`, 39: `JointLinLambdaX`, 40: `JointLinLambdaY`, 41: `JointLinLambdaZ`, 42: `JointAngLambdaX`, 43: `JointAngLambdaY`, 44: `JointAngLambdaZ`} // String returns the string representation of this JointVars value. func (i JointVars) String() string { return enums.String(i, _JointVarsMap) } diff --git a/physics/examples/virtroom/virtroom.go b/physics/examples/virtroom/virtroom.go index 10eaa7ff..c1687fcf 100644 --- a/physics/examples/virtroom/virtroom.go +++ b/physics/examples/virtroom/virtroom.go @@ -321,6 +321,7 @@ func (ev *Env) MakeEmer(wl *builder.World, em *Emer, name string) { // body := physics.NewCylinder(emr, "body", math32.Vec3(0, hh, 0), hh, hw) em.XZ = obj.NewJointPlaneXZ(nil, emr, math32.Vec3(0, 0, 0), math32.Vec3(0, -hh, 0)) emr.Group = 0 // no collide (temporary) + return headPos := math32.Vec3(0, 2*hh+headsz, 0) head := obj.NewDynamicSkin(sc, name+"_head", physics.Box, "tan", mass*.1, math32.Vec3(headsz, headsz, headsz), headPos, rot) @@ -336,7 +337,8 @@ func (ev *Env) MakeEmer(wl *builder.World, em *Emer, name string) { hdsk.UpdateColor(clr, sld) }) } - em.Neck = obj.NewJointBall(emr, head, math32.Vec3(0, hh, 0), math32.Vec3(0, -headsz, 0)) + // em.Neck = obj.NewJointBall(emr, head, math32.Vec3(0, hh, 0), math32.Vec3(0, -headsz, 0)) + em.Neck = obj.NewJointRevolute(emr, head, math32.Vec3(0, hh, 0), math32.Vec3(0, -headsz, 0), math32.Vec3(0, 1, 0)) em.Neck.ParentFixed = true // obj.NewJointFixed(emr, head, math32.Vec3(0, hh, 0), math32.Vec3(0, -headsz, 0)) diff --git a/physics/joint.go b/physics/joint.go index e79bdde1..4fc03c00 100644 --- a/physics/joint.go +++ b/physics/joint.go @@ -133,6 +133,16 @@ const ( JointCTorqueX JointCTorqueY JointCTorqueZ + + // Computed linear lambdas. + JointLinLambdaX + JointLinLambdaY + JointLinLambdaZ + + // Computed angular lambdas. + JointAngLambdaX + JointAngLambdaY + JointAngLambdaZ ) func GetJointType(idx int32) JointTypes { @@ -291,6 +301,26 @@ func SetJointCTorque(idx int32, t math32.Vector3) { Joints.Set(t.Z, int(idx), int(JointCTorqueZ)) } +func JointLinLambda(idx int32) math32.Vector3 { + return math32.Vec3(Joints.Value(int(idx), int(JointLinLambdaX)), Joints.Value(int(idx), int(JointLinLambdaY)), Joints.Value(int(idx), int(JointLinLambdaZ))) +} + +func SetJointLinLambda(idx int32, t math32.Vector3) { + Joints.Set(t.X, int(idx), int(JointLinLambdaX)) + Joints.Set(t.Y, int(idx), int(JointLinLambdaY)) + Joints.Set(t.Z, int(idx), int(JointLinLambdaZ)) +} + +func JointAngLambda(idx int32) math32.Vector3 { + return math32.Vec3(Joints.Value(int(idx), int(JointAngLambdaX)), Joints.Value(int(idx), int(JointAngLambdaY)), Joints.Value(int(idx), int(JointAngLambdaZ))) +} + +func SetJointAngLambda(idx int32, t math32.Vector3) { + Joints.Set(t.X, int(idx), int(JointAngLambdaX)) + Joints.Set(t.Y, int(idx), int(JointAngLambdaY)) + Joints.Set(t.Z, int(idx), int(JointAngLambdaZ)) +} + // JointDoFVars are joint DoF state variables stored in tensor.Float32, // one for each DoF. type JointDoFVars int32 //enums:enum diff --git a/physics/joint.goal b/physics/joint.goal index 7fd7ebe3..bef0aa28 100644 --- a/physics/joint.goal +++ b/physics/joint.goal @@ -131,6 +131,16 @@ const ( JointCTorqueX JointCTorqueY JointCTorqueZ + + // Computed linear lambdas. + JointLinLambdaX + JointLinLambdaY + JointLinLambdaZ + + // Computed angular lambdas. + JointAngLambdaX + JointAngLambdaY + JointAngLambdaZ ) func GetJointType(idx int32) JointTypes { @@ -289,6 +299,26 @@ func SetJointCTorque(idx int32, t math32.Vector3) { Joints[idx, JointCTorqueZ] = t.Z } +func JointLinLambda(idx int32) math32.Vector3 { + return math32.Vec3(Joints[idx, JointLinLambdaX], Joints[idx, JointLinLambdaY], Joints[idx, JointLinLambdaZ]) +} + +func SetJointLinLambda(idx int32, t math32.Vector3) { + Joints[idx, JointLinLambdaX] = t.X + Joints[idx, JointLinLambdaY] = t.Y + Joints[idx, JointLinLambdaZ] = t.Z +} + +func JointAngLambda(idx int32) math32.Vector3 { + return math32.Vec3(Joints[idx, JointAngLambdaX], Joints[idx, JointAngLambdaY], Joints[idx, JointAngLambdaZ]) +} + +func SetJointAngLambda(idx int32, t math32.Vector3) { + Joints[idx, JointAngLambdaX] = t.X + Joints[idx, JointAngLambdaY] = t.Y + Joints[idx, JointAngLambdaZ] = t.Z +} + // JointDoFVars are joint DoF state variables stored in tensor.Float32, // one for each DoF. type JointDoFVars int32 //enums:enum diff --git a/physics/shaders/CollisionBroad.wgsl b/physics/shaders/CollisionBroad.wgsl index e85bd1ba..9de7f3c1 100644 --- a/physics/shaders/CollisionBroad.wgsl +++ b/physics/shaders/CollisionBroad.wgsl @@ -278,7 +278,7 @@ const JointControlVarsN: JointControlVars = 5; const DynamicVarsN: DynamicVars = 33; const GPUVarsN: GPUVars = 13; const JointTypesN: JointTypes = 8; -const JointVarsN: JointVars = 39; +const JointVarsN: JointVars = 45; const JointDoFVarsN: JointDoFVars = 5; const ShapesN: Shapes = 6; @@ -333,6 +333,12 @@ const JointCForceZ: JointVars = 35; const JointCTorqueX: JointVars = 36; const JointCTorqueY: JointVars = 37; const JointCTorqueZ: JointVars = 38; +const JointLinLambdaX: JointVars = 39; +const JointLinLambdaY: JointVars = 40; +const JointLinLambdaZ: JointVars = 41; +const JointAngLambdaX: JointVars = 42; +const JointAngLambdaY: JointVars = 43; +const JointAngLambdaZ: JointVars = 44; alias JointDoFVars = i32; //enums:enum const JointAxisX: JointDoFVars = 0; const JointAxisY: JointDoFVars = 1; @@ -470,6 +476,9 @@ fn ShapePairContacts(a: Shapes,b: Shapes, infPlane: bool, ba: ptr) } } +//////// import: "slmath-math.go" +const Pi = 3.141592653589793; + //////// import: "slmath-matrix3.go" //////// import: "slmath-quaternion.go" diff --git a/physics/shaders/CollisionNarrow.wgsl b/physics/shaders/CollisionNarrow.wgsl index 1015c525..cc06730c 100644 --- a/physics/shaders/CollisionNarrow.wgsl +++ b/physics/shaders/CollisionNarrow.wgsl @@ -367,7 +367,7 @@ const JointControlVarsN: JointControlVars = 5; const DynamicVarsN: DynamicVars = 33; const GPUVarsN: GPUVars = 13; const JointTypesN: JointTypes = 8; -const JointVarsN: JointVars = 39; +const JointVarsN: JointVars = 45; const JointDoFVarsN: JointDoFVars = 5; const ShapesN: Shapes = 6; @@ -422,6 +422,12 @@ const JointCForceZ: JointVars = 35; const JointCTorqueX: JointVars = 36; const JointCTorqueY: JointVars = 37; const JointCTorqueZ: JointVars = 38; +const JointLinLambdaX: JointVars = 39; +const JointLinLambdaY: JointVars = 40; +const JointLinLambdaZ: JointVars = 41; +const JointAngLambdaX: JointVars = 42; +const JointAngLambdaY: JointVars = 43; +const JointAngLambdaZ: JointVars = 44; alias JointDoFVars = i32; //enums:enum const JointAxisX: JointDoFVars = 0; const JointAxisY: JointDoFVars = 1; @@ -939,6 +945,9 @@ const Cylinder: Shapes = 3; const Box: Shapes = 4; const Cone: Shapes = 5; +//////// import: "slmath-math.go" +const Pi = 3.141592653589793; + //////// import: "slmath-matrix3.go" //////// import: "slmath-quaternion.go" diff --git a/physics/shaders/DynamicsCurToNext.wgsl b/physics/shaders/DynamicsCurToNext.wgsl index f2371c1c..f0cbdf0f 100644 --- a/physics/shaders/DynamicsCurToNext.wgsl +++ b/physics/shaders/DynamicsCurToNext.wgsl @@ -161,7 +161,7 @@ const JointControlVarsN: JointControlVars = 5; const DynamicVarsN: DynamicVars = 33; const GPUVarsN: GPUVars = 13; const JointTypesN: JointTypes = 8; -const JointVarsN: JointVars = 39; +const JointVarsN: JointVars = 45; const JointDoFVarsN: JointDoFVars = 5; const ShapesN: Shapes = 6; @@ -216,6 +216,12 @@ const JointCForceZ: JointVars = 35; const JointCTorqueX: JointVars = 36; const JointCTorqueY: JointVars = 37; const JointCTorqueZ: JointVars = 38; +const JointLinLambdaX: JointVars = 39; +const JointLinLambdaY: JointVars = 40; +const JointLinLambdaZ: JointVars = 41; +const JointAngLambdaX: JointVars = 42; +const JointAngLambdaY: JointVars = 43; +const JointAngLambdaZ: JointVars = 44; alias JointDoFVars = i32; //enums:enum const JointAxisX: JointDoFVars = 0; const JointAxisY: JointDoFVars = 1; @@ -281,6 +287,9 @@ const Cylinder: Shapes = 3; const Box: Shapes = 4; const Cone: Shapes = 5; +//////// import: "slmath-math.go" +const Pi = 3.141592653589793; + //////// import: "slmath-matrix3.go" //////// import: "slmath-quaternion.go" diff --git a/physics/shaders/ForcesFromJoints.wgsl b/physics/shaders/ForcesFromJoints.wgsl index d51ca38f..bceff2da 100644 --- a/physics/shaders/ForcesFromJoints.wgsl +++ b/physics/shaders/ForcesFromJoints.wgsl @@ -179,7 +179,7 @@ const JointControlVarsN: JointControlVars = 5; const DynamicVarsN: DynamicVars = 33; const GPUVarsN: GPUVars = 13; const JointTypesN: JointTypes = 8; -const JointVarsN: JointVars = 39; +const JointVarsN: JointVars = 45; const JointDoFVarsN: JointDoFVars = 5; const ShapesN: Shapes = 6; @@ -234,6 +234,12 @@ const JointCForceZ: JointVars = 35; const JointCTorqueX: JointVars = 36; const JointCTorqueY: JointVars = 37; const JointCTorqueZ: JointVars = 38; +const JointLinLambdaX: JointVars = 39; +const JointLinLambdaY: JointVars = 40; +const JointLinLambdaZ: JointVars = 41; +const JointAngLambdaX: JointVars = 42; +const JointAngLambdaY: JointVars = 43; +const JointAngLambdaZ: JointVars = 44; fn JointPForce(idx: i32) -> vec3 { return vec3(Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointPForceX))], Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointPForceY))], Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointPForceZ))]); } @@ -311,6 +317,9 @@ const Cylinder: Shapes = 3; const Box: Shapes = 4; const Cone: Shapes = 5; +//////// import: "slmath-math.go" +const Pi = 3.141592653589793; + //////// import: "slmath-matrix3.go" //////// import: "slmath-quaternion.go" diff --git a/physics/shaders/InitDynamics.wgsl b/physics/shaders/InitDynamics.wgsl index 7e032147..4c81f1cc 100644 --- a/physics/shaders/InitDynamics.wgsl +++ b/physics/shaders/InitDynamics.wgsl @@ -168,7 +168,7 @@ const JointControlVarsN: JointControlVars = 5; const DynamicVarsN: DynamicVars = 33; const GPUVarsN: GPUVars = 13; const JointTypesN: JointTypes = 8; -const JointVarsN: JointVars = 39; +const JointVarsN: JointVars = 45; const JointDoFVarsN: JointDoFVars = 5; const ShapesN: Shapes = 6; @@ -223,6 +223,12 @@ const JointCForceZ: JointVars = 35; const JointCTorqueX: JointVars = 36; const JointCTorqueY: JointVars = 37; const JointCTorqueZ: JointVars = 38; +const JointLinLambdaX: JointVars = 39; +const JointLinLambdaY: JointVars = 40; +const JointLinLambdaZ: JointVars = 41; +const JointAngLambdaX: JointVars = 42; +const JointAngLambdaY: JointVars = 43; +const JointAngLambdaZ: JointVars = 44; alias JointDoFVars = i32; //enums:enum const JointAxisX: JointDoFVars = 0; const JointAxisY: JointDoFVars = 1; @@ -288,6 +294,9 @@ const Cylinder: Shapes = 3; const Box: Shapes = 4; const Cone: Shapes = 5; +//////// import: "slmath-math.go" +const Pi = 3.141592653589793; + //////// import: "slmath-matrix3.go" //////// import: "slmath-quaternion.go" diff --git a/physics/shaders/StepBodyContactDeltas.wgsl b/physics/shaders/StepBodyContactDeltas.wgsl index 6d16324b..4b08cf77 100644 --- a/physics/shaders/StepBodyContactDeltas.wgsl +++ b/physics/shaders/StepBodyContactDeltas.wgsl @@ -264,7 +264,7 @@ const JointControlVarsN: JointControlVars = 5; const DynamicVarsN: DynamicVars = 33; const GPUVarsN: GPUVars = 13; const JointTypesN: JointTypes = 8; -const JointVarsN: JointVars = 39; +const JointVarsN: JointVars = 45; const JointDoFVarsN: JointDoFVars = 5; const ShapesN: Shapes = 6; @@ -319,6 +319,12 @@ const JointCForceZ: JointVars = 35; const JointCTorqueX: JointVars = 36; const JointCTorqueY: JointVars = 37; const JointCTorqueZ: JointVars = 38; +const JointLinLambdaX: JointVars = 39; +const JointLinLambdaY: JointVars = 40; +const JointLinLambdaZ: JointVars = 41; +const JointAngLambdaX: JointVars = 42; +const JointAngLambdaY: JointVars = 43; +const JointAngLambdaZ: JointVars = 44; alias JointDoFVars = i32; //enums:enum const JointAxisX: JointDoFVars = 0; const JointAxisY: JointDoFVars = 1; @@ -384,6 +390,9 @@ const Cylinder: Shapes = 3; const Box: Shapes = 4; const Cone: Shapes = 5; +//////// import: "slmath-math.go" +const Pi = 3.141592653589793; + //////// import: "slmath-matrix3.go" //////// import: "slmath-quaternion.go" diff --git a/physics/shaders/StepBodyContacts.wgsl b/physics/shaders/StepBodyContacts.wgsl index ad14c325..baff552b 100644 --- a/physics/shaders/StepBodyContacts.wgsl +++ b/physics/shaders/StepBodyContacts.wgsl @@ -393,7 +393,7 @@ const JointControlVarsN: JointControlVars = 5; const DynamicVarsN: DynamicVars = 33; const GPUVarsN: GPUVars = 13; const JointTypesN: JointTypes = 8; -const JointVarsN: JointVars = 39; +const JointVarsN: JointVars = 45; const JointDoFVarsN: JointDoFVars = 5; const ShapesN: Shapes = 6; @@ -448,6 +448,12 @@ const JointCForceZ: JointVars = 35; const JointCTorqueX: JointVars = 36; const JointCTorqueY: JointVars = 37; const JointCTorqueZ: JointVars = 38; +const JointLinLambdaX: JointVars = 39; +const JointLinLambdaY: JointVars = 40; +const JointLinLambdaZ: JointVars = 41; +const JointAngLambdaX: JointVars = 42; +const JointAngLambdaY: JointVars = 43; +const JointAngLambdaZ: JointVars = 44; alias JointDoFVars = i32; //enums:enum const JointAxisX: JointDoFVars = 0; const JointAxisY: JointDoFVars = 1; @@ -513,6 +519,9 @@ const Cylinder: Shapes = 3; const Box: Shapes = 4; const Cone: Shapes = 5; +//////// import: "slmath-math.go" +const Pi = 3.141592653589793; + //////// import: "slmath-matrix3.go" //////// import: "slmath-quaternion.go" diff --git a/physics/shaders/StepIntegrateBodies.wgsl b/physics/shaders/StepIntegrateBodies.wgsl index 0923e40d..27701141 100644 --- a/physics/shaders/StepIntegrateBodies.wgsl +++ b/physics/shaders/StepIntegrateBodies.wgsl @@ -191,7 +191,7 @@ const JointControlVarsN: JointControlVars = 5; const DynamicVarsN: DynamicVars = 33; const GPUVarsN: GPUVars = 13; const JointTypesN: JointTypes = 8; -const JointVarsN: JointVars = 39; +const JointVarsN: JointVars = 45; const JointDoFVarsN: JointDoFVars = 5; const ShapesN: Shapes = 6; @@ -246,6 +246,12 @@ const JointCForceZ: JointVars = 35; const JointCTorqueX: JointVars = 36; const JointCTorqueY: JointVars = 37; const JointCTorqueZ: JointVars = 38; +const JointLinLambdaX: JointVars = 39; +const JointLinLambdaY: JointVars = 40; +const JointLinLambdaZ: JointVars = 41; +const JointAngLambdaX: JointVars = 42; +const JointAngLambdaY: JointVars = 43; +const JointAngLambdaZ: JointVars = 44; alias JointDoFVars = i32; //enums:enum const JointAxisX: JointDoFVars = 0; const JointAxisY: JointDoFVars = 1; @@ -311,6 +317,9 @@ const Cylinder: Shapes = 3; const Box: Shapes = 4; const Cone: Shapes = 5; +//////// import: "slmath-math.go" +const Pi = 3.141592653589793; + //////// import: "slmath-matrix3.go" //////// import: "slmath-quaternion.go" diff --git a/physics/shaders/StepJointForces.wgsl b/physics/shaders/StepJointForces.wgsl index e536e986..90ca867b 100644 --- a/physics/shaders/StepJointForces.wgsl +++ b/physics/shaders/StepJointForces.wgsl @@ -182,7 +182,7 @@ const JointControlVarsN: JointControlVars = 5; const DynamicVarsN: DynamicVars = 33; const GPUVarsN: GPUVars = 13; const JointTypesN: JointTypes = 8; -const JointVarsN: JointVars = 39; +const JointVarsN: JointVars = 45; const JointDoFVarsN: JointDoFVars = 5; const ShapesN: Shapes = 6; @@ -237,6 +237,12 @@ const JointCForceZ: JointVars = 35; const JointCTorqueX: JointVars = 36; const JointCTorqueY: JointVars = 37; const JointCTorqueZ: JointVars = 38; +const JointLinLambdaX: JointVars = 39; +const JointLinLambdaY: JointVars = 40; +const JointLinLambdaZ: JointVars = 41; +const JointAngLambdaX: JointVars = 42; +const JointAngLambdaY: JointVars = 43; +const JointAngLambdaZ: JointVars = 44; fn GetJointType(idx: i32) -> JointTypes { return JointTypes(bitcast(Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointType))])); } @@ -283,8 +289,7 @@ fn SetJointCForce(idx: i32, f: vec3) { fn SetJointCTorque(idx: i32, t: vec3) { Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointCTorqueX))] = t.x; Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointCTorqueY))] = t.y; - Joints[Index2D(TensorStrides[30], TensorStrides[31], - u32(idx), u32(JointCTorqueZ))] = t.z; + Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointCTorqueZ))] = t.z; } alias JointDoFVars = i32; //enums:enum const JointAxisX: JointDoFVars = 0; @@ -357,6 +362,9 @@ const Cylinder: Shapes = 3; const Box: Shapes = 4; const Cone: Shapes = 5; +//////// import: "slmath-math.go" +const Pi = 3.141592653589793; + //////// import: "slmath-matrix3.go" //////// import: "slmath-quaternion.go" diff --git a/physics/shaders/StepSolveJoints.wgsl b/physics/shaders/StepSolveJoints.wgsl index ec5fb496..9f14ff99 100644 --- a/physics/shaders/StepSolveJoints.wgsl +++ b/physics/shaders/StepSolveJoints.wgsl @@ -212,7 +212,7 @@ const JointControlVarsN: JointControlVars = 5; const DynamicVarsN: DynamicVars = 33; const GPUVarsN: GPUVars = 13; const JointTypesN: JointTypes = 8; -const JointVarsN: JointVars = 39; +const JointVarsN: JointVars = 45; const JointDoFVarsN: JointDoFVars = 5; const ShapesN: Shapes = 6; @@ -267,6 +267,12 @@ const JointCForceZ: JointVars = 35; const JointCTorqueX: JointVars = 36; const JointCTorqueY: JointVars = 37; const JointCTorqueZ: JointVars = 38; +const JointLinLambdaX: JointVars = 39; +const JointLinLambdaY: JointVars = 40; +const JointLinLambdaZ: JointVars = 41; +const JointAngLambdaX: JointVars = 42; +const JointAngLambdaY: JointVars = 43; +const JointAngLambdaZ: JointVars = 44; fn GetJointType(idx: i32) -> JointTypes { return JointTypes(bitcast(Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointType))])); } @@ -305,6 +311,22 @@ fn JointCPos(idx: i32) -> vec3 { fn JointCQuat(idx: i32) -> vec4 { return vec4(Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointCQuatX))], Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointCQuatY))], Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointCQuatZ))], Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointCQuatW))]); } +fn JointLinLambda(idx: i32) -> vec3 { + return vec3(Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointLinLambdaX))], Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointLinLambdaY))], Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointLinLambdaZ))]); +} +fn SetJointLinLambda(idx: i32, t: vec3) { + Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointLinLambdaX))] = t.x; + Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointLinLambdaY))] = t.y; + Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointLinLambdaZ))] = t.z; +} +fn JointAngLambda(idx: i32) -> vec3 { + return vec3(Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointAngLambdaX))], Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointAngLambdaY))], Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointAngLambdaZ))]); +} +fn SetJointAngLambda(idx: i32, t: vec3) { + Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointAngLambdaX))] = t.x; + Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointAngLambdaY))] = t.y; + Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointAngLambdaZ))] = t.z; +} alias JointDoFVars = i32; //enums:enum const JointAxisX: JointDoFVars = 0; const JointAxisY: JointDoFVars = 1; @@ -379,6 +401,18 @@ const Cylinder: Shapes = 3; const Box: Shapes = 4; const Cone: Shapes = 5; +//////// import: "slmath-math.go" +const Pi = 3.141592653589793; +fn MinAngleDiff(a: f32,b: f32) -> f32 { + var d = a - b; + if (d > Pi) { + d -= 2 * Pi; + } + if (d < -Pi) { + d += 2 * Pi; + }return d; +} + //////// import: "slmath-matrix3.go" //////// import: "slmath-quaternion.go" @@ -642,6 +676,8 @@ fn StepSolveJointLinear(ji: i32) { MulSpatialTransforms(relPoseR, relPoseQ, xwCR, xwCQ, &relPoseR, &relPoseQ); var wComP = MulSpatialPoint(posePR, posePQ, comP); var wComC = MulSpatialPoint(poseCR, poseCQ, comC); + var lambdaPrev = JointLinLambda(ji); + var lambdaNext = vec3(0, 0, 0); if (jt == Distance) { var dP = xwPR-(wComP); var dC = xwCR-(wComC); @@ -742,16 +778,18 @@ fn StepSolveJointLinear(ji: i32) { } } if (abs(err) > 1e-9 || abs(derrRel) > 1e-9) { - var lambdaIn = f32(0.0); + var lambdaIn = Dim3(lambdaPrev, dim); var dLambda = PositionalCorrection(err, derrRel, posePQ, poseCQ, mInvP, mInvC, iInvP, iInvC, linearP, linearC, angularP, angularC, lambdaIn, compliance, damping, params.Dt); linDeltaP = linDeltaP+(linearP*(dLambda * params.JointLinearRelax)); linDeltaC = linDeltaC+(linearC*(dLambda * params.JointLinearRelax)); angDeltaP = angDeltaP+(angularP*(dLambda * params.JointAngularRelax)); angDeltaC = angDeltaC+(angularC*(dLambda * params.JointAngularRelax)); + lambdaNext = SetDim3(lambdaNext, dim, dLambda); } } } + SetJointLinLambda(ji, lambdaNext); if (!parentFixed) { StepBodyDeltas(jPi, jPbi, false, f32(f32(0)), linDeltaP, angDeltaP); } @@ -865,6 +903,8 @@ fn StepSolveJointAngular(ji: i32) { var axisTargetPosKeA: vec3; var axisTargetVelKdD: vec3; var axisTargetVelKdA: vec3; + var lambdaPrev = JointAngLambda(ji); + var lambdaNext = vec3(0, 0, 0); for (var dof=0; dof 0.0) { - err = e - targetPos; + err = MinAngleDiff(e, targetPos); compliance = 1.0 / ke; damping = Dim3(axisDamping, dim); } else if (kd > 0.0) { @@ -919,11 +959,13 @@ fn StepSolveJointAngular(ji: i32) { damping = kd; } } - var lambdaIn = f32(0); + var lambdaIn = Dim3(lambdaPrev, dim); var dLambda = AngularCorrection(err, derrRel, posePQ, poseCQ, iInvP, iInvC, angularP, angularC, lambdaIn, compliance, damping, params.Dt); angDeltaP = angDeltaP+(angularP*(dLambda)); angDeltaC = angDeltaC+(angularC*(dLambda)); + lambdaNext = SetDim3(lambdaNext, dim, dLambda); } + SetJointAngLambda(ji, lambdaNext); if (!parentFixed) { StepBodyDeltas(jPi, jPbi, false, f32(f32(0)), linDeltaP, angDeltaP); } diff --git a/physics/step_joint.go b/physics/step_joint.go index ae454cf6..8706d803 100644 --- a/physics/step_joint.go +++ b/physics/step_joint.go @@ -210,6 +210,9 @@ func StepSolveJointLinear(ji int32) { wComP := slmath.MulSpatialPoint(posePR, posePQ, comP) wComC := slmath.MulSpatialPoint(poseCR, poseCQ, comC) + lambdaPrev := JointLinLambda(ji) + lambdaNext := math32.Vec3(0, 0, 0) + // handle positional constraints if jt == Distance { dP := xwPR.Sub(wComP) @@ -326,7 +329,8 @@ func StepSolveJointLinear(ji int32) { } } if math32.Abs(err) > 1e-9 || math32.Abs(derrRel) > 1e-9 { - lambdaIn := float32(0.0) + lambdaIn := slmath.Dim3(lambdaPrev, dim) + // lambdaIn := float32(0) dLambda := PositionalCorrection(err, derrRel, posePQ, poseCQ, mInvP, mInvC, iInvP, iInvC, linearP, linearC, angularP, angularC, lambdaIn, compliance, damping, params.Dt) @@ -334,9 +338,15 @@ func StepSolveJointLinear(ji int32) { linDeltaC = linDeltaC.Add(linearC.MulScalar(dLambda * params.JointLinearRelax)) angDeltaP = angDeltaP.Add(angularP.MulScalar(dLambda * params.JointAngularRelax)) angDeltaC = angDeltaC.Add(angularC.MulScalar(dLambda * params.JointAngularRelax)) + + lambdaNext = slmath.SetDim3(lambdaNext, dim, dLambda) + // if angularP.Length() > 0.001 && dLambda > 0.001 { + // fmt.Println("lin:", angularP.Length(), dLambda, angularP, angDeltaP, dP, linearC, angularC.Length()) + // } } } } + SetJointLinLambda(ji, lambdaNext) // apply the linear deltas so the if !parentFixed { StepBodyDeltas(jPi, jPbi, false, 0, linDeltaP, angDeltaP) @@ -474,6 +484,8 @@ func StepSolveJointAngular(ji int32) { var axisLimitsD, axisLimitsA math32.Vector3 var axisTargetPosKeD, axisTargetPosKeA math32.Vector3 var axisTargetVelKdD, axisTargetVelKdA math32.Vector3 + lambdaPrev := JointAngLambda(ji) + lambdaNext := math32.Vec3(0, 0, 0) for dof := range jAngularN { di := dof + jLinearN @@ -534,21 +546,26 @@ func StepSolveJointAngular(ji int32) { ke := slmath.Dim3(axisStiffness, dim) kd := slmath.Dim3(axisDamping, dim) if ke > 0.0 { - err = e - targetPos + err = slmath.MinAngleDiff(e, targetPos) compliance = 1.0 / ke damping = slmath.Dim3(axisDamping, dim) } else if kd > 0.0 { compliance = 1.0 / kd damping = kd } + // if ji == 0 && dim == 1 { + // fmt.Println(targetPos, e, err) + // } } - lambdaIn := float32(0) + lambdaIn := slmath.Dim3(lambdaPrev, dim) dLambda := AngularCorrection(err, derrRel, posePQ, poseCQ, iInvP, iInvC, angularP, angularC, lambdaIn, compliance, damping, params.Dt) // note: no relaxation factors here: angDeltaP = angDeltaP.Add(angularP.MulScalar(dLambda)) angDeltaC = angDeltaC.Add(angularC.MulScalar(dLambda)) + lambdaNext = slmath.SetDim3(lambdaNext, dim, dLambda) } + SetJointAngLambda(ji, lambdaNext) if !parentFixed { StepBodyDeltas(jPi, jPbi, false, 0, linDeltaP, angDeltaP) diff --git a/physics/step_joint.goal b/physics/step_joint.goal index eac0b9ef..7970273b 100644 --- a/physics/step_joint.goal +++ b/physics/step_joint.goal @@ -208,6 +208,9 @@ func StepSolveJointLinear(ji int32) { wComP := slmath.MulSpatialPoint(posePR, posePQ, comP) wComC := slmath.MulSpatialPoint(poseCR, poseCQ, comC) + lambdaPrev := JointLinLambda(ji) + lambdaNext := math32.Vec3(0,0,0) + // handle positional constraints if jt == Distance { dP := xwPR.Sub(wComP) @@ -324,7 +327,8 @@ func StepSolveJointLinear(ji int32) { } } if math32.Abs(err) > 1e-9 || math32.Abs(derrRel) > 1e-9 { - lambdaIn := float32(0.0) + lambdaIn := slmath.Dim3(lambdaPrev, dim) + // lambdaIn := float32(0) dLambda := PositionalCorrection(err, derrRel, posePQ, poseCQ, mInvP, mInvC, iInvP, iInvC, linearP, linearC, angularP, angularC, lambdaIn, compliance, damping, params.Dt) @@ -332,9 +336,15 @@ func StepSolveJointLinear(ji int32) { linDeltaC = linDeltaC.Add(linearC.MulScalar(dLambda * params.JointLinearRelax)) angDeltaP = angDeltaP.Add(angularP.MulScalar(dLambda * params.JointAngularRelax)) angDeltaC = angDeltaC.Add(angularC.MulScalar(dLambda * params.JointAngularRelax)) + + lambdaNext = slmath.SetDim3(lambdaNext, dim, dLambda) + // if angularP.Length() > 0.001 && dLambda > 0.001 { + // fmt.Println("lin:", angularP.Length(), dLambda, angularP, angDeltaP, dP, linearC, angularC.Length()) + // } } } } + SetJointLinLambda(ji, lambdaNext) // apply the linear deltas so the if !parentFixed { StepBodyDeltas(jPi, jPbi, false, 0, linDeltaP, angDeltaP) @@ -472,6 +482,8 @@ func StepSolveJointAngular(ji int32) { var axisLimitsD, axisLimitsA math32.Vector3 var axisTargetPosKeD, axisTargetPosKeA math32.Vector3 var axisTargetVelKdD, axisTargetVelKdA math32.Vector3 + lambdaPrev := JointAngLambda(ji) + lambdaNext := math32.Vec3(0,0,0) for dof := range jAngularN { di := dof + jLinearN @@ -532,21 +544,26 @@ func StepSolveJointAngular(ji int32) { ke := slmath.Dim3(axisStiffness, dim) kd := slmath.Dim3(axisDamping, dim) if ke > 0.0 { - err = e - targetPos + err = slmath.MinAngleDiff(e, targetPos) compliance = 1.0 / ke damping = slmath.Dim3(axisDamping, dim) } else if kd > 0.0 { compliance = 1.0 / kd damping = kd } + // if ji == 0 && dim == 1 { + // fmt.Println(targetPos, e, err) + // } } - lambdaIn := float32(0) + lambdaIn := slmath.Dim3(lambdaPrev, dim) dLambda := AngularCorrection(err, derrRel, posePQ, poseCQ, iInvP, iInvC, angularP, angularC, lambdaIn, compliance, damping, params.Dt) // note: no relaxation factors here: angDeltaP = angDeltaP.Add(angularP.MulScalar(dLambda)) angDeltaC = angDeltaC.Add(angularC.MulScalar(dLambda)) + lambdaNext = slmath.SetDim3(lambdaNext, dim, dLambda) } + SetJointAngLambda(ji, lambdaNext) if !parentFixed { StepBodyDeltas(jPi, jPbi, false, 0, linDeltaP, angDeltaP) From 016fe7870a1cf4e4e7351767c319c1d7d7eb55c3 Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Sun, 4 Jan 2026 07:18:15 -0800 Subject: [PATCH 73/97] physics: integrating changes in target position over time fixes stability issues for ball joints, but not XZ joint in virtroom. --- docs/content/physics.md | 6 +++--- gosl/gotosl/testdata/Compute.golden | 3 +++ gosl/gotosl/testdata/CycleUpdt.golden | 3 +++ physics/config.go | 10 ++++++++++ physics/config.goal | 11 +++++++++++ physics/control.go | 11 +++++++++-- physics/control.goal | 11 +++++++++-- physics/enumgen.go | 10 +++++----- physics/examples/pendula/typegen.go | 9 +++++++++ physics/gosl.go | 7 ++++--- physics/model.go | 17 +++++++++-------- physics/model.goal | 17 +++++++++-------- physics/params.go | 17 +++++++++++------ physics/phyxyz/editor.go | 7 +++---- physics/shaders/CollisionBroad.wgsl | 15 ++++++++------- physics/shaders/CollisionNarrow.wgsl | 15 ++++++++------- physics/shaders/DynamicsCurToNext.wgsl | 15 ++++++++------- physics/shaders/ForcesFromJoints.wgsl | 15 ++++++++------- physics/shaders/InitDynamics.wgsl | 15 ++++++++------- physics/shaders/StepBodyContactDeltas.wgsl | 15 ++++++++------- physics/shaders/StepBodyContacts.wgsl | 15 ++++++++------- physics/shaders/StepIntegrateBodies.wgsl | 15 ++++++++------- physics/shaders/StepJointForces.wgsl | 15 ++++++++------- physics/shaders/StepSolveJoints.wgsl | 19 ++++++++++--------- physics/step.go | 6 ++++++ physics/step.goal | 6 ++++++ physics/step_joint.go | 4 ++-- physics/step_joint.goal | 4 ++-- physics/typegen.go | 4 ++-- physics/vars.go | 5 +++-- .../labsymbols/cogentcore_org-lab-physics.go | 13 ++++++++++++- 31 files changed, 213 insertions(+), 122 deletions(-) create mode 100644 physics/examples/pendula/typegen.go diff --git a/docs/content/physics.md b/docs/content/physics.md index 9ab18e90..22fa1a92 100644 --- a/docs/content/physics.md +++ b/docs/content/physics.md @@ -197,9 +197,9 @@ ed.SetControlFunc(func(timeStep int) { }) func update() { - posXstr = fmt.Sprintf("Pos X: %g", posX) - posYstr = fmt.Sprintf("Pos Y: %g", posY) - posZstr = fmt.Sprintf("Pos Z: %g", posZ) + posXstr = fmt.Sprintf("Angle X: %g", posX) + posYstr = fmt.Sprintf("Angle Y: %g", posY) + posZstr = fmt.Sprintf("Angle Z: %g", posZ) stiffStr = fmt.Sprintf("Stiff: %g", stiff) dampStr = fmt.Sprintf("Damp: %g", damp) } diff --git a/gosl/gotosl/testdata/Compute.golden b/gosl/gotosl/testdata/Compute.golden index a3abc2cf..06c47c53 100644 --- a/gosl/gotosl/testdata/Compute.golden +++ b/gosl/gotosl/testdata/Compute.golden @@ -241,6 +241,9 @@ fn Compute(i: u32) { //gosl:kernel ParamStruct_IntegFromRaw(params, i32(i)); } +//////// import: "slmath-math.go" +const Pi = 3.141592653589793; + //////// import: "slmath-matrix3.go" //////// import: "slmath-quaternion.go" diff --git a/gosl/gotosl/testdata/CycleUpdt.golden b/gosl/gotosl/testdata/CycleUpdt.golden index 9ce275b4..5e83c868 100644 --- a/gosl/gotosl/testdata/CycleUpdt.golden +++ b/gosl/gotosl/testdata/CycleUpdt.golden @@ -66,6 +66,9 @@ fn CycleUpdt(i: u32) { //gosl:kernel read-write:Ctx Ctx[0] = ctx; } +//////// import: "slmath-math.go" +const Pi = 3.141592653589793; + //////// import: "slmath-matrix3.go" //////// import: "slmath-quaternion.go" diff --git a/physics/config.go b/physics/config.go index 1e0071c3..76cbc92e 100644 --- a/physics/config.go +++ b/physics/config.go @@ -87,9 +87,19 @@ func (ml *Model) ConfigBodies() { } } +// InitControlState initializes the JointTargetPosCur values to 0. +// This is done on the CPU prior to copying up to GPU, in InitState. +func (ml *Model) InitControlState() { + params := GetParams(0) + for j := range params.JointDoFsN { + JointControls.Set(0, int(j), int(JointTargetPosCur)) + } +} + // InitState initializes the simulation state. func (ml *Model) InitState() { params := GetParams(0) + ml.InitControlState() ml.ToGPUInfra() RunInitDynamics(int(params.DynamicsN)) RunDone(DynamicsVar) diff --git a/physics/config.goal b/physics/config.goal index 64a02e8d..050264b8 100644 --- a/physics/config.goal +++ b/physics/config.goal @@ -85,9 +85,20 @@ func (ml *Model) ConfigBodies() { } } +// InitControlState initializes the JointTargetPosCur values to 0. +// This is done on the CPU prior to copying up to GPU, in InitState. +func (ml *Model) InitControlState() { + params := GetParams(0) + for j := range params.JointDoFsN { + JointControls[j, JointTargetPosCur] = 0 + } +} + + // InitState initializes the simulation state. func (ml *Model) InitState() { params := GetParams(0) + ml.InitControlState() ml.ToGPUInfra() RunInitDynamics(int(params.DynamicsN)) RunDone(DynamicsVar) diff --git a/physics/control.go b/physics/control.go index 296af7ff..90873a57 100644 --- a/physics/control.go +++ b/physics/control.go @@ -18,10 +18,17 @@ const ( // Joint force and torque inputs JointControlForce JointControlVars = iota - // JointTargetPos is the position target value, where 0 is the initial - // position. For angular joints, this is in radians. + // JointTargetPos is the position target value input to the model, + // where 0 is the initial position. For angular joints, this is in radians. + // This is subject to a graded transition over time, [JointTargetPosCur] + // has the current effective value. JointTargetPos + // JointTargetPosCur is the current position target value, + // updated from [JointTargetPos] input using the [Params.ControlDt] + // time constant. + JointTargetPosCur + // JointTargetStiff determines how strongly the target position // is enforced: 0 = not at all; larger = stronger (e.g., 1000 or higher). // Set to 0 to allow the joint to be fully flexible. diff --git a/physics/control.goal b/physics/control.goal index a8632dc5..116d0034 100644 --- a/physics/control.goal +++ b/physics/control.goal @@ -16,10 +16,17 @@ const ( // Joint force and torque inputs JointControlForce JointControlVars = iota - // JointTargetPos is the position target value, where 0 is the initial - // position. For angular joints, this is in radians. + // JointTargetPos is the position target value input to the model, + // where 0 is the initial position. For angular joints, this is in radians. + // This is subject to a graded transition over time, [JointTargetPosCur] + // has the current effective value. JointTargetPos + // JointTargetPosCur is the current position target value, + // updated from [JointTargetPos] input using the [Params.ControlDt] + // time constant. + JointTargetPosCur + // JointTargetStiff determines how strongly the target position // is enforced: 0 = not at all; larger = stronger (e.g., 1000 or higher). // Set to 0 to allow the joint to be fully flexible. diff --git a/physics/enumgen.go b/physics/enumgen.go index 4cbc4b73..49908db4 100644 --- a/physics/enumgen.go +++ b/physics/enumgen.go @@ -98,20 +98,20 @@ func (i *ContactVars) UnmarshalText(text []byte) error { return enums.UnmarshalText(i, text, "ContactVars") } -var _JointControlVarsValues = []JointControlVars{0, 1, 2, 3, 4} +var _JointControlVarsValues = []JointControlVars{0, 1, 2, 3, 4, 5} // JointControlVarsN is the highest valid value for type JointControlVars, plus one. // //gosl:start -const JointControlVarsN JointControlVars = 5 +const JointControlVarsN JointControlVars = 6 //gosl:end -var _JointControlVarsValueMap = map[string]JointControlVars{`JointControlForce`: 0, `JointTargetPos`: 1, `JointTargetStiff`: 2, `JointTargetVel`: 3, `JointTargetDamp`: 4} +var _JointControlVarsValueMap = map[string]JointControlVars{`JointControlForce`: 0, `JointTargetPos`: 1, `JointTargetPosCur`: 2, `JointTargetStiff`: 3, `JointTargetVel`: 4, `JointTargetDamp`: 5} -var _JointControlVarsDescMap = map[JointControlVars]string{0: `Joint force and torque inputs`, 1: `JointTargetPos is the position target value, where 0 is the initial position. For angular joints, this is in radians.`, 2: `JointTargetStiff determines how strongly the target position is enforced: 0 = not at all; larger = stronger (e.g., 1000 or higher). Set to 0 to allow the joint to be fully flexible.`, 3: `JointTargetVel is the velocity target value. For example, 0 effectively damps joint movement in proportion to Damp parameter.`, 4: `JointTargetDamp determines how strongly the target velocity is enforced: 0 = not at all; larger = stronger (e.g., 1 is reasonable). Set to 0 to allow the joint to be fully flexible.`} +var _JointControlVarsDescMap = map[JointControlVars]string{0: `Joint force and torque inputs`, 1: `JointTargetPos is the position target value input to the model, where 0 is the initial position. For angular joints, this is in radians. This is subject to a graded transition over time, [JointTargetPosCur] has the current effective value.`, 2: `JointTargetPosCur is the current position target value, updated from [JointTargetPos] input using the [Params.ControlDt] time constant.`, 3: `JointTargetStiff determines how strongly the target position is enforced: 0 = not at all; larger = stronger (e.g., 1000 or higher). Set to 0 to allow the joint to be fully flexible.`, 4: `JointTargetVel is the velocity target value. For example, 0 effectively damps joint movement in proportion to Damp parameter.`, 5: `JointTargetDamp determines how strongly the target velocity is enforced: 0 = not at all; larger = stronger (e.g., 1 is reasonable). Set to 0 to allow the joint to be fully flexible.`} -var _JointControlVarsMap = map[JointControlVars]string{0: `JointControlForce`, 1: `JointTargetPos`, 2: `JointTargetStiff`, 3: `JointTargetVel`, 4: `JointTargetDamp`} +var _JointControlVarsMap = map[JointControlVars]string{0: `JointControlForce`, 1: `JointTargetPos`, 2: `JointTargetPosCur`, 3: `JointTargetStiff`, 4: `JointTargetVel`, 5: `JointTargetDamp`} // String returns the string representation of this JointControlVars value. func (i JointControlVars) String() string { return enums.String(i, _JointControlVarsMap) } diff --git a/physics/examples/pendula/typegen.go b/physics/examples/pendula/typegen.go new file mode 100644 index 00000000..46db7929 --- /dev/null +++ b/physics/examples/pendula/typegen.go @@ -0,0 +1,9 @@ +// Code generated by "core generate -add-types"; DO NOT EDIT. + +package main + +import ( + "cogentcore.org/core/types" +) + +var _ = types.AddType(&types.Type{Name: "main.Pendula", IDName: "pendula", Doc: "Pendula has sim params", Fields: []types.Field{{Name: "NPendula", Doc: "Number of bar elements to add to the pendulum. More interesting the more you add!"}, {Name: "StartVert", Doc: "StartVert starts the pendulum in the vertical orientation\n(else horizontal, so it has somewhere to go). Need to add force if vertical."}, {Name: "TargetDegFromVert", Doc: "TargetDegFromVert is the target number of degrees off of vertical\nfor each joint. Critical for this to not be 0 for StartVert."}, {Name: "ForceOn", Doc: "Timestep in msec to add a force"}, {Name: "ForceOff", Doc: "Timestep in msec to stop adding force"}, {Name: "Force", Doc: "Force to add"}, {Name: "HSize", Doc: "half-size of the pendulum elements."}, {Name: "Mass", Doc: "Mass of each bar (kg)"}, {Name: "Collide", Doc: "do the elements collide with each other? this is currently bad!"}, {Name: "Stiff", Doc: "Stiff is the strength of the positional constraint to keep\neach bar in a vertical position."}, {Name: "Damp", Doc: "Damp is the strength of the velocity constraint to keep each\nbar not moving."}}}) diff --git a/physics/gosl.go b/physics/gosl.go index 2c30106d..a360159d 100644 --- a/physics/gosl.go +++ b/physics/gosl.go @@ -83,7 +83,7 @@ func GPUInit() { _ = vr vr = sgp.Add("TensorStrides", gpu.Uint32, 1, gpu.ComputeShader) vr.ReadOnly = true - vr = sgp.AddStruct("Params", int(unsafe.Sizeof(PhysParams{})), 1, gpu.ComputeShader) + vr = sgp.AddStruct("Params", int(unsafe.Sizeof(PhysicsParams{})), 1, gpu.ComputeShader) sgp.SetNValues(1) } { @@ -167,6 +167,7 @@ func GPUInit() { pl.AddVarUsed(0, "TensorStrides") pl.AddVarUsed(2, "BroadContactsN") pl.AddVarUsed(2, "ContactsN") + pl.AddVarUsed(3, "JointControls") pl.AddVarUsed(0, "Params") pl = gpu.NewComputePipelineShaderFS(shaders, "shaders/StepIntegrateBodies.wgsl", sy) pl.AddVarUsed(0, "TensorStrides") @@ -897,8 +898,8 @@ func SyncFromGPU(vars ...GPUVars) { } // GetParams returns a pointer to the given global variable: -// [Params] []PhysParams at given index. This directly processed in the GPU code, +// [Params] []PhysicsParams at given index. This directly processed in the GPU code, // so this function call is an equivalent for the CPU. -func GetParams(idx uint32) *PhysParams { +func GetParams(idx uint32) *PhysicsParams { return &Params[idx] } diff --git a/physics/model.go b/physics/model.go index cfa1fff0..4cef353a 100644 --- a/physics/model.go +++ b/physics/model.go @@ -19,7 +19,7 @@ type Model struct { GPU bool // Params are global parameters. - Params []PhysParams + Params []PhysicsParams // CurrentWorld is the [BodyWorld] value to use when creating new bodies. // Set to -1 to create global elements that interact with everything, @@ -28,21 +28,21 @@ type Model struct { // CurrentObject is the Object to use when creating new joints. // Call NewObject to increment. - CurrentObject int + CurrentObject int `edit:"-"` // CurrentObjectJoint is the Joint index in CurrentObject // to use when creating new joints. - CurrentObjectJoint int + CurrentObjectJoint int `edit:"-"` // ReplicasStart is the starting body index for replicated world bodies, // which is needed for viewers to efficiently select a specific world to view. // This is the start of the World=0 first instance. - ReplicasStart int32 + ReplicasStart int32 `edit:"-"` // ReplicasN is the number of body elements within each set of // replicated world bodies, which is needed for viewers to efficiently select // a specific world to view. - ReplicasN int32 + ReplicasN int32 `edit:"-"` // Bodies are the rigid body elements (dynamic and static), // specifying the constant, non-dynamic properties, @@ -52,9 +52,10 @@ type Model struct { // Objects is a list of joint indexes for each object, where each object // contains all the joints interconnecting an overlapping set of bodies. + // This is known as an articulation in other physics software. // Joints must be added in parent -> child order within objects, as joints // are updated in sequential order within object. - // [object][maxjointsperobj] + // [object][MaxObjectJoints+1] Objects *tensor.Int32 `display:"no-inline"` // BodyJoints is a list of joint indexes for each dynamic body, for aggregating. @@ -82,7 +83,7 @@ type Model struct { // BroadContactsN has number of points of broad contact // between bodies. [1] - BroadContactsN *tensor.Int32 + BroadContactsN *tensor.Int32 `display:"no-inline"` // BroadContacts are the results of broad-phase contact processing, // establishing possible points of contact between bodies. @@ -115,7 +116,7 @@ func NewModel() *Model { // Must call Config once configured. func (ml *Model) Init() { ml.GPU = true - ml.Params = make([]PhysParams, 1) + ml.Params = make([]PhysicsParams, 1) ml.Params[0].Defaults() ml.Reset() } diff --git a/physics/model.goal b/physics/model.goal index 6a7f4348..02c45b42 100644 --- a/physics/model.goal +++ b/physics/model.goal @@ -17,7 +17,7 @@ type Model struct { GPU bool // Params are global parameters. - Params []PhysParams + Params []PhysicsParams // CurrentWorld is the [BodyWorld] value to use when creating new bodies. // Set to -1 to create global elements that interact with everything, @@ -26,21 +26,21 @@ type Model struct { // CurrentObject is the Object to use when creating new joints. // Call NewObject to increment. - CurrentObject int + CurrentObject int `edit:"-"` // CurrentObjectJoint is the Joint index in CurrentObject // to use when creating new joints. - CurrentObjectJoint int + CurrentObjectJoint int `edit:"-"` // ReplicasStart is the starting body index for replicated world bodies, // which is needed for viewers to efficiently select a specific world to view. // This is the start of the World=0 first instance. - ReplicasStart int32 + ReplicasStart int32 `edit:"-"` // ReplicasN is the number of body elements within each set of // replicated world bodies, which is needed for viewers to efficiently select // a specific world to view. - ReplicasN int32 + ReplicasN int32 `edit:"-"` // Bodies are the rigid body elements (dynamic and static), // specifying the constant, non-dynamic properties, @@ -50,9 +50,10 @@ type Model struct { // Objects is a list of joint indexes for each object, where each object // contains all the joints interconnecting an overlapping set of bodies. + // This is known as an articulation in other physics software. // Joints must be added in parent -> child order within objects, as joints // are updated in sequential order within object. - // [object][maxjointsperobj] + // [object][MaxObjectJoints+1] Objects *tensor.Int32 `display:"no-inline"` // BodyJoints is a list of joint indexes for each dynamic body, for aggregating. @@ -80,7 +81,7 @@ type Model struct { // BroadContactsN has number of points of broad contact // between bodies. [1] - BroadContactsN *tensor.Int32 + BroadContactsN *tensor.Int32 `display:"no-inline"` // BroadContacts are the results of broad-phase contact processing, // establishing possible points of contact between bodies. @@ -113,7 +114,7 @@ func NewModel() *Model { // Must call Config once configured. func (ml *Model) Init() { ml.GPU = true - ml.Params = make([]PhysParams, 1) + ml.Params = make([]PhysicsParams, 1) ml.Params[0].Defaults() ml.Reset() } diff --git a/physics/params.go b/physics/params.go index fc722069..f97b5295 100644 --- a/physics/params.go +++ b/physics/params.go @@ -12,8 +12,8 @@ import ( //gosl:start -// PhysParams are the physics parameters -type PhysParams struct { +// PhysicsParams are the physics parameters +type PhysicsParams struct { // Iterations is the number of integration iterations to perform // within each solver step. Muller et al (2020) report that 1 is best. Iterations int32 `default:"1"` @@ -29,6 +29,10 @@ type PhysParams struct { // the GPU and are therefore much faster. SubSteps int32 `default:"10" min:"1"` + // ControlDt is the stepsize for integrating joint control position values + // [JointTargetPos] over time, to avoid sudden strong changes in force. + ControlDt float32 `default:"0.1"` + // Contact margin is the extra distance for broadphase collision // around rigid bodies. This can make some joints potentially unstable if > 0 ContactMargin float32 `default:"0,0.1"` @@ -103,16 +107,17 @@ type PhysParams struct { // to examine. BodyCollidePairsN int32 `edit:"-"` - pad, pad1 int32 + pad int32 // Gravity is the gravity acceleration function Gravity slvec.Vector3 } -func (pr *PhysParams) Defaults() { +func (pr *PhysicsParams) Defaults() { pr.Iterations = 1 pr.Dt = 0.0001 pr.SubSteps = 10 + pr.ControlDt = 0.1 pr.Gravity.Set(0, -9.81, 0) pr.ContactMargin = 0 @@ -132,7 +137,7 @@ func (pr *PhysParams) Defaults() { } // Reset resets the N's -func (pr *PhysParams) Reset() { +func (pr *PhysicsParams) Reset() { pr.BodiesN = 0 pr.DynamicsN = 0 pr.ObjectsN = 0 @@ -145,7 +150,7 @@ func (pr *PhysParams) Reset() { // StepsToMsec returns the given number of individual Step calls // converted into milliseconds, suitable for driving controls. -func (pr *PhysParams) StepsToMsec(steps int) int { +func (pr *PhysicsParams) StepsToMsec(steps int) int { msper := 1000 * pr.Dt * float32(pr.SubSteps) return int(math32.Round(float32(steps) * msper)) } diff --git a/physics/phyxyz/editor.go b/physics/phyxyz/editor.go index 355942e2..d0c2b12e 100644 --- a/physics/phyxyz/editor.go +++ b/physics/phyxyz/editor.go @@ -163,6 +163,7 @@ func (pe *Editor) ConfigModel() { pe.Scene.Init(pe.Model) pe.stop = false pe.TimeStep = 0 + pe.editor.NeedsRender() } // Restart restarts the simulation, returning true if successful (i.e., not running). @@ -173,7 +174,8 @@ func (pe *Editor) Restart() bool { } pe.stop = false pe.TimeStep = 0 - pe.Scene.Init(pe.Model) + pe.Scene.InitState(pe.Model) + pe.editor.NeedsRender() return true } @@ -258,9 +260,6 @@ func (pe *Editor) MakeToolbar(p *tree.Plan) { w.SetText("Rebuild").SetIcon(icons.Reset). SetTooltip("Rebuild the environment, when you change parameters"). OnClick(func(e events.Event) { - if !pe.Restart() { - return - } pe.ConfigModel() }) w.FirstStyler(func(s *styles.Style) { s.SetEnabled(!pe.isRunning) }) diff --git a/physics/shaders/CollisionBroad.wgsl b/physics/shaders/CollisionBroad.wgsl index 9de7f3c1..9ce7b23d 100644 --- a/physics/shaders/CollisionBroad.wgsl +++ b/physics/shaders/CollisionBroad.wgsl @@ -5,7 +5,7 @@ @group(0) @binding(0) var TensorStrides: array; @group(0) @binding(1) -var Params: array; +var Params: array; // // Bodies are the rigid body elements (dynamic and static), // specifying the constant, non-dynamic properties, // which is initial state for dynamics. // [body][BodyVarsN] @group(1) @binding(0) var Bodies: array; @@ -225,9 +225,10 @@ fn AddBroadContacts(biA: i32,biB: i32,nci: i32,ncA: i32,ncB: i32) { alias JointControlVars = i32; //enums:enum const JointControlForce: JointControlVars = 0; const JointTargetPos: JointControlVars = 1; -const JointTargetStiff: JointControlVars = 2; -const JointTargetVel: JointControlVars = 3; -const JointTargetDamp: JointControlVars = 4; +const JointTargetPosCur: JointControlVars = 2; +const JointTargetStiff: JointControlVars = 3; +const JointTargetVel: JointControlVars = 4; +const JointTargetDamp: JointControlVars = 5; //////// import: "dynamics.go" alias DynamicVars = i32; //enums:enum @@ -274,7 +275,7 @@ fn DynamicQuat(idx: i32,cni: i32) -> vec4 { //////// import: "enumgen.go" const BodyVarsN: BodyVars = 43; const ContactVarsN: ContactVars = 33; -const JointControlVarsN: JointControlVars = 5; +const JointControlVarsN: JointControlVars = 6; const DynamicVarsN: DynamicVars = 33; const GPUVarsN: GPUVars = 13; const JointTypesN: JointTypes = 8; @@ -347,10 +348,11 @@ const JointLimitLower: JointDoFVars = 3; const JointLimitUpper: JointDoFVars = 4; //////// import: "params.go" -struct PhysParams { +struct PhysicsParams { Iterations: i32, Dt: f32, SubSteps: i32, + ControlDt: f32, ContactMargin: f32, ContactRelax: f32, // 0.8 def ContactWeighting: i32, // true @@ -375,7 +377,6 @@ struct PhysParams { BodyJointsMax: i32, BodyCollidePairsN: i32, pad: i32, - pad1: i32, Gravity: vec4, } diff --git a/physics/shaders/CollisionNarrow.wgsl b/physics/shaders/CollisionNarrow.wgsl index cc06730c..90f29f7e 100644 --- a/physics/shaders/CollisionNarrow.wgsl +++ b/physics/shaders/CollisionNarrow.wgsl @@ -5,7 +5,7 @@ @group(0) @binding(0) var TensorStrides: array; @group(0) @binding(1) -var Params: array; +var Params: array; // // Bodies are the rigid body elements (dynamic and static), // specifying the constant, non-dynamic properties, // which is initial state for dynamics. // [body][BodyVarsN] @group(1) @binding(0) var Bodies: array; @@ -314,9 +314,10 @@ fn CollisionNarrow(i: u32) { //gosl:kernel alias JointControlVars = i32; //enums:enum const JointControlForce: JointControlVars = 0; const JointTargetPos: JointControlVars = 1; -const JointTargetStiff: JointControlVars = 2; -const JointTargetVel: JointControlVars = 3; -const JointTargetDamp: JointControlVars = 4; +const JointTargetPosCur: JointControlVars = 2; +const JointTargetStiff: JointControlVars = 3; +const JointTargetVel: JointControlVars = 4; +const JointTargetDamp: JointControlVars = 5; //////// import: "dynamics.go" alias DynamicVars = i32; //enums:enum @@ -363,7 +364,7 @@ fn DynamicQuat(idx: i32,cni: i32) -> vec4 { //////// import: "enumgen.go" const BodyVarsN: BodyVars = 43; const ContactVarsN: ContactVars = 33; -const JointControlVarsN: JointControlVars = 5; +const JointControlVarsN: JointControlVars = 6; const DynamicVarsN: DynamicVars = 33; const GPUVarsN: GPUVars = 13; const JointTypesN: JointTypes = 8; @@ -436,10 +437,11 @@ const JointLimitLower: JointDoFVars = 3; const JointLimitUpper: JointDoFVars = 4; //////// import: "params.go" -struct PhysParams { +struct PhysicsParams { Iterations: i32, Dt: f32, SubSteps: i32, + ControlDt: f32, ContactMargin: f32, ContactRelax: f32, // 0.8 def ContactWeighting: i32, // true @@ -464,7 +466,6 @@ struct PhysParams { BodyJointsMax: i32, BodyCollidePairsN: i32, pad: i32, - pad1: i32, Gravity: vec4, } diff --git a/physics/shaders/DynamicsCurToNext.wgsl b/physics/shaders/DynamicsCurToNext.wgsl index f0cbdf0f..a2293a23 100644 --- a/physics/shaders/DynamicsCurToNext.wgsl +++ b/physics/shaders/DynamicsCurToNext.wgsl @@ -5,7 +5,7 @@ @group(0) @binding(0) var TensorStrides: array; @group(0) @binding(1) -var Params: array; +var Params: array; // // Bodies are the rigid body elements (dynamic and static), // specifying the constant, non-dynamic properties, // which is initial state for dynamics. // [body][BodyVarsN] // // Dynamics are the dynamic rigid body elements: these actually move. // Two alternating states are used: Params.Cur and Params.Next. // [dyn body][cur/next][DynamicVarsN] @group(2) @binding(0) @@ -114,9 +114,10 @@ const BroadContactVarsN = ContactAPointX; alias JointControlVars = i32; //enums:enum const JointControlForce: JointControlVars = 0; const JointTargetPos: JointControlVars = 1; -const JointTargetStiff: JointControlVars = 2; -const JointTargetVel: JointControlVars = 3; -const JointTargetDamp: JointControlVars = 4; +const JointTargetPosCur: JointControlVars = 2; +const JointTargetStiff: JointControlVars = 3; +const JointTargetVel: JointControlVars = 4; +const JointTargetDamp: JointControlVars = 5; //////// import: "dynamics.go" alias DynamicVars = i32; //enums:enum @@ -157,7 +158,7 @@ const DynContactWeight: DynamicVars = 32; //////// import: "enumgen.go" const BodyVarsN: BodyVars = 43; const ContactVarsN: ContactVars = 33; -const JointControlVarsN: JointControlVars = 5; +const JointControlVarsN: JointControlVars = 6; const DynamicVarsN: DynamicVars = 33; const GPUVarsN: GPUVars = 13; const JointTypesN: JointTypes = 8; @@ -230,10 +231,11 @@ const JointLimitLower: JointDoFVars = 3; const JointLimitUpper: JointDoFVars = 4; //////// import: "params.go" -struct PhysParams { +struct PhysicsParams { Iterations: i32, Dt: f32, SubSteps: i32, + ControlDt: f32, ContactMargin: f32, ContactRelax: f32, // 0.8 def ContactWeighting: i32, // true @@ -258,7 +260,6 @@ struct PhysParams { BodyJointsMax: i32, BodyCollidePairsN: i32, pad: i32, - pad1: i32, Gravity: vec4, } diff --git a/physics/shaders/ForcesFromJoints.wgsl b/physics/shaders/ForcesFromJoints.wgsl index bceff2da..5866d557 100644 --- a/physics/shaders/ForcesFromJoints.wgsl +++ b/physics/shaders/ForcesFromJoints.wgsl @@ -5,7 +5,7 @@ @group(0) @binding(0) var TensorStrides: array; @group(0) @binding(1) -var Params: array; +var Params: array; // // Bodies are the rigid body elements (dynamic and static), // specifying the constant, non-dynamic properties, // which is initial state for dynamics. // [body][BodyVarsN] @group(1) @binding(2) var BodyJoints: array; @@ -122,9 +122,10 @@ const BroadContactVarsN = ContactAPointX; alias JointControlVars = i32; //enums:enum const JointControlForce: JointControlVars = 0; const JointTargetPos: JointControlVars = 1; -const JointTargetStiff: JointControlVars = 2; -const JointTargetVel: JointControlVars = 3; -const JointTargetDamp: JointControlVars = 4; +const JointTargetPosCur: JointControlVars = 2; +const JointTargetStiff: JointControlVars = 3; +const JointTargetVel: JointControlVars = 4; +const JointTargetDamp: JointControlVars = 5; //////// import: "dynamics.go" alias DynamicVars = i32; //enums:enum @@ -175,7 +176,7 @@ fn SetDynamicTorque(idx: i32,cni: i32, torque: vec3) { //////// import: "enumgen.go" const BodyVarsN: BodyVars = 43; const ContactVarsN: ContactVars = 33; -const JointControlVarsN: JointControlVars = 5; +const JointControlVarsN: JointControlVars = 6; const DynamicVarsN: DynamicVars = 33; const GPUVarsN: GPUVars = 13; const JointTypesN: JointTypes = 8; @@ -260,10 +261,11 @@ const JointLimitLower: JointDoFVars = 3; const JointLimitUpper: JointDoFVars = 4; //////// import: "params.go" -struct PhysParams { +struct PhysicsParams { Iterations: i32, Dt: f32, SubSteps: i32, + ControlDt: f32, ContactMargin: f32, ContactRelax: f32, // 0.8 def ContactWeighting: i32, // true @@ -288,7 +290,6 @@ struct PhysParams { BodyJointsMax: i32, BodyCollidePairsN: i32, pad: i32, - pad1: i32, Gravity: vec4, } diff --git a/physics/shaders/InitDynamics.wgsl b/physics/shaders/InitDynamics.wgsl index 4c81f1cc..61060e24 100644 --- a/physics/shaders/InitDynamics.wgsl +++ b/physics/shaders/InitDynamics.wgsl @@ -5,7 +5,7 @@ @group(0) @binding(0) var TensorStrides: array; @group(0) @binding(1) -var Params: array; +var Params: array; // // Bodies are the rigid body elements (dynamic and static), // specifying the constant, non-dynamic properties, // which is initial state for dynamics. // [body][BodyVarsN] @group(1) @binding(0) var Bodies: array; @@ -120,9 +120,10 @@ const BroadContactVarsN = ContactAPointX; alias JointControlVars = i32; //enums:enum const JointControlForce: JointControlVars = 0; const JointTargetPos: JointControlVars = 1; -const JointTargetStiff: JointControlVars = 2; -const JointTargetVel: JointControlVars = 3; -const JointTargetDamp: JointControlVars = 4; +const JointTargetPosCur: JointControlVars = 2; +const JointTargetStiff: JointControlVars = 3; +const JointTargetVel: JointControlVars = 4; +const JointTargetDamp: JointControlVars = 5; //////// import: "dynamics.go" alias DynamicVars = i32; //enums:enum @@ -164,7 +165,7 @@ fn DynamicBody(idx: i32) -> i32 { return i32(bitcast(Dynamics[Index3D(Tenso //////// import: "enumgen.go" const BodyVarsN: BodyVars = 43; const ContactVarsN: ContactVars = 33; -const JointControlVarsN: JointControlVars = 5; +const JointControlVarsN: JointControlVars = 6; const DynamicVarsN: DynamicVars = 33; const GPUVarsN: GPUVars = 13; const JointTypesN: JointTypes = 8; @@ -237,10 +238,11 @@ const JointLimitLower: JointDoFVars = 3; const JointLimitUpper: JointDoFVars = 4; //////// import: "params.go" -struct PhysParams { +struct PhysicsParams { Iterations: i32, Dt: f32, SubSteps: i32, + ControlDt: f32, ContactMargin: f32, ContactRelax: f32, // 0.8 def ContactWeighting: i32, // true @@ -265,7 +267,6 @@ struct PhysParams { BodyJointsMax: i32, BodyCollidePairsN: i32, pad: i32, - pad1: i32, Gravity: vec4, } diff --git a/physics/shaders/StepBodyContactDeltas.wgsl b/physics/shaders/StepBodyContactDeltas.wgsl index 4b08cf77..9b95ae41 100644 --- a/physics/shaders/StepBodyContactDeltas.wgsl +++ b/physics/shaders/StepBodyContactDeltas.wgsl @@ -5,7 +5,7 @@ @group(0) @binding(0) var TensorStrides: array; @group(0) @binding(1) -var Params: array; +var Params: array; // // Bodies are the rigid body elements (dynamic and static), // specifying the constant, non-dynamic properties, // which is initial state for dynamics. // [body][BodyVarsN] @group(1) @binding(0) var Bodies: array; @@ -196,9 +196,10 @@ fn StepBodyContactDeltas(i: u32) { //gosl:kernel alias JointControlVars = i32; //enums:enum const JointControlForce: JointControlVars = 0; const JointTargetPos: JointControlVars = 1; -const JointTargetStiff: JointControlVars = 2; -const JointTargetVel: JointControlVars = 3; -const JointTargetDamp: JointControlVars = 4; +const JointTargetPosCur: JointControlVars = 2; +const JointTargetStiff: JointControlVars = 3; +const JointTargetVel: JointControlVars = 4; +const JointTargetDamp: JointControlVars = 5; //////// import: "dynamics.go" alias DynamicVars = i32; //enums:enum @@ -260,7 +261,7 @@ fn SetDynamicAngDelta(idx: i32,cni: i32, angDelta: vec3) { //////// import: "enumgen.go" const BodyVarsN: BodyVars = 43; const ContactVarsN: ContactVars = 33; -const JointControlVarsN: JointControlVars = 5; +const JointControlVarsN: JointControlVars = 6; const DynamicVarsN: DynamicVars = 33; const GPUVarsN: GPUVars = 13; const JointTypesN: JointTypes = 8; @@ -333,10 +334,11 @@ const JointLimitLower: JointDoFVars = 3; const JointLimitUpper: JointDoFVars = 4; //////// import: "params.go" -struct PhysParams { +struct PhysicsParams { Iterations: i32, Dt: f32, SubSteps: i32, + ControlDt: f32, ContactMargin: f32, ContactRelax: f32, // 0.8 def ContactWeighting: i32, // true @@ -361,7 +363,6 @@ struct PhysParams { BodyJointsMax: i32, BodyCollidePairsN: i32, pad: i32, - pad1: i32, Gravity: vec4, } diff --git a/physics/shaders/StepBodyContacts.wgsl b/physics/shaders/StepBodyContacts.wgsl index baff552b..9f1b9dcc 100644 --- a/physics/shaders/StepBodyContacts.wgsl +++ b/physics/shaders/StepBodyContacts.wgsl @@ -5,7 +5,7 @@ @group(0) @binding(0) var TensorStrides: array; @group(0) @binding(1) -var Params: array; +var Params: array; // // Bodies are the rigid body elements (dynamic and static), // specifying the constant, non-dynamic properties, // which is initial state for dynamics. // [body][BodyVarsN] @group(1) @binding(0) var Bodies: array; @@ -334,9 +334,10 @@ fn ContactConstraint(err: f32, q0A: vec4,q0B: vec4, mInvA: f32,mInvB: alias JointControlVars = i32; //enums:enum const JointControlForce: JointControlVars = 0; const JointTargetPos: JointControlVars = 1; -const JointTargetStiff: JointControlVars = 2; -const JointTargetVel: JointControlVars = 3; -const JointTargetDamp: JointControlVars = 4; +const JointTargetPosCur: JointControlVars = 2; +const JointTargetStiff: JointControlVars = 3; +const JointTargetVel: JointControlVars = 4; +const JointTargetDamp: JointControlVars = 5; //////// import: "dynamics.go" alias DynamicVars = i32; //enums:enum @@ -389,7 +390,7 @@ fn DynamicAngDelta(idx: i32,cni: i32) -> vec3 { //////// import: "enumgen.go" const BodyVarsN: BodyVars = 43; const ContactVarsN: ContactVars = 33; -const JointControlVarsN: JointControlVars = 5; +const JointControlVarsN: JointControlVars = 6; const DynamicVarsN: DynamicVars = 33; const GPUVarsN: GPUVars = 13; const JointTypesN: JointTypes = 8; @@ -462,10 +463,11 @@ const JointLimitLower: JointDoFVars = 3; const JointLimitUpper: JointDoFVars = 4; //////// import: "params.go" -struct PhysParams { +struct PhysicsParams { Iterations: i32, Dt: f32, SubSteps: i32, + ControlDt: f32, ContactMargin: f32, ContactRelax: f32, // 0.8 def ContactWeighting: i32, // true @@ -490,7 +492,6 @@ struct PhysParams { BodyJointsMax: i32, BodyCollidePairsN: i32, pad: i32, - pad1: i32, Gravity: vec4, } diff --git a/physics/shaders/StepIntegrateBodies.wgsl b/physics/shaders/StepIntegrateBodies.wgsl index 27701141..29d7dab4 100644 --- a/physics/shaders/StepIntegrateBodies.wgsl +++ b/physics/shaders/StepIntegrateBodies.wgsl @@ -5,7 +5,7 @@ @group(0) @binding(0) var TensorStrides: array; @group(0) @binding(1) -var Params: array; +var Params: array; // // Bodies are the rigid body elements (dynamic and static), // specifying the constant, non-dynamic properties, // which is initial state for dynamics. // [body][BodyVarsN] @group(1) @binding(0) var Bodies: array; @@ -133,9 +133,10 @@ const BroadContactVarsN = ContactAPointX; alias JointControlVars = i32; //enums:enum const JointControlForce: JointControlVars = 0; const JointTargetPos: JointControlVars = 1; -const JointTargetStiff: JointControlVars = 2; -const JointTargetVel: JointControlVars = 3; -const JointTargetDamp: JointControlVars = 4; +const JointTargetPosCur: JointControlVars = 2; +const JointTargetStiff: JointControlVars = 3; +const JointTargetVel: JointControlVars = 4; +const JointTargetDamp: JointControlVars = 5; //////// import: "dynamics.go" alias DynamicVars = i32; //enums:enum @@ -187,7 +188,7 @@ fn SetDynamicAngDelta(idx: i32,cni: i32, angDelta: vec3) { Dynamics[Index3D //////// import: "enumgen.go" const BodyVarsN: BodyVars = 43; const ContactVarsN: ContactVars = 33; -const JointControlVarsN: JointControlVars = 5; +const JointControlVarsN: JointControlVars = 6; const DynamicVarsN: DynamicVars = 33; const GPUVarsN: GPUVars = 13; const JointTypesN: JointTypes = 8; @@ -260,10 +261,11 @@ const JointLimitLower: JointDoFVars = 3; const JointLimitUpper: JointDoFVars = 4; //////// import: "params.go" -struct PhysParams { +struct PhysicsParams { Iterations: i32, Dt: f32, SubSteps: i32, + ControlDt: f32, ContactMargin: f32, ContactRelax: f32, // 0.8 def ContactWeighting: i32, // true @@ -288,7 +290,6 @@ struct PhysParams { BodyJointsMax: i32, BodyCollidePairsN: i32, pad: i32, - pad1: i32, Gravity: vec4, } diff --git a/physics/shaders/StepJointForces.wgsl b/physics/shaders/StepJointForces.wgsl index 90ca867b..e3868120 100644 --- a/physics/shaders/StepJointForces.wgsl +++ b/physics/shaders/StepJointForces.wgsl @@ -5,7 +5,7 @@ @group(0) @binding(0) var TensorStrides: array; @group(0) @binding(1) -var Params: array; +var Params: array; // // Bodies are the rigid body elements (dynamic and static), // specifying the constant, non-dynamic properties, // which is initial state for dynamics. // [body][BodyVarsN] @group(1) @binding(0) var Bodies: array; @@ -129,9 +129,10 @@ const BroadContactVarsN = ContactAPointX; alias JointControlVars = i32; //enums:enum const JointControlForce: JointControlVars = 0; const JointTargetPos: JointControlVars = 1; -const JointTargetStiff: JointControlVars = 2; -const JointTargetVel: JointControlVars = 3; -const JointTargetDamp: JointControlVars = 4; +const JointTargetPosCur: JointControlVars = 2; +const JointTargetStiff: JointControlVars = 3; +const JointTargetVel: JointControlVars = 4; +const JointTargetDamp: JointControlVars = 5; fn JointControl(idx: i32,dof: i32, vr: JointControlVars) -> f32 { return JointControls[Index2D(TensorStrides[110], TensorStrides[111], u32(JointDoFIndex(idx, dof)), u32(vr))]; } @@ -178,7 +179,7 @@ fn DynamicQuat(idx: i32,cni: i32) -> vec4 { return vec4(Dynamics[Index //////// import: "enumgen.go" const BodyVarsN: BodyVars = 43; const ContactVarsN: ContactVars = 33; -const JointControlVarsN: JointControlVars = 5; +const JointControlVarsN: JointControlVars = 6; const DynamicVarsN: DynamicVars = 33; const GPUVarsN: GPUVars = 13; const JointTypesN: JointTypes = 8; @@ -305,10 +306,11 @@ fn JointAxis(idx: i32,dof: i32) -> vec3 { } //////// import: "params.go" -struct PhysParams { +struct PhysicsParams { Iterations: i32, Dt: f32, SubSteps: i32, + ControlDt: f32, ContactMargin: f32, ContactRelax: f32, // 0.8 def ContactWeighting: i32, // true @@ -333,7 +335,6 @@ struct PhysParams { BodyJointsMax: i32, BodyCollidePairsN: i32, pad: i32, - pad1: i32, Gravity: vec4, } diff --git a/physics/shaders/StepSolveJoints.wgsl b/physics/shaders/StepSolveJoints.wgsl index 9f14ff99..f4fdfbd8 100644 --- a/physics/shaders/StepSolveJoints.wgsl +++ b/physics/shaders/StepSolveJoints.wgsl @@ -5,7 +5,7 @@ @group(0) @binding(0) var TensorStrides: array; @group(0) @binding(1) -var Params: array; +var Params: array; // // Bodies are the rigid body elements (dynamic and static), // specifying the constant, non-dynamic properties, // which is initial state for dynamics. // [body][BodyVarsN] @group(1) @binding(0) var Bodies: array; @@ -141,9 +141,10 @@ const BroadContactVarsN = ContactAPointX; alias JointControlVars = i32; //enums:enum const JointControlForce: JointControlVars = 0; const JointTargetPos: JointControlVars = 1; -const JointTargetStiff: JointControlVars = 2; -const JointTargetVel: JointControlVars = 3; -const JointTargetDamp: JointControlVars = 4; +const JointTargetPosCur: JointControlVars = 2; +const JointTargetStiff: JointControlVars = 3; +const JointTargetVel: JointControlVars = 4; +const JointTargetDamp: JointControlVars = 5; fn JointControl(idx: i32,dof: i32, vr: JointControlVars) -> f32 { return JointControls[Index2D(TensorStrides[110], TensorStrides[111], u32(JointDoFIndex(idx, dof)), u32(vr))]; } @@ -208,7 +209,7 @@ fn SetDynamicAngDelta(idx: i32,cni: i32, angDelta: vec3) { //////// import: "enumgen.go" const BodyVarsN: BodyVars = 43; const ContactVarsN: ContactVars = 33; -const JointControlVarsN: JointControlVars = 5; +const JointControlVarsN: JointControlVars = 6; const DynamicVarsN: DynamicVars = 33; const GPUVarsN: GPUVars = 13; const JointTypesN: JointTypes = 8; @@ -344,10 +345,11 @@ fn JointDoF(idx: i32,dof: i32, vr: JointDoFVars) -> f32 { } //////// import: "params.go" -struct PhysParams { +struct PhysicsParams { Iterations: i32, Dt: f32, SubSteps: i32, + ControlDt: f32, ContactMargin: f32, ContactRelax: f32, // 0.8 def ContactWeighting: i32, // true @@ -372,7 +374,6 @@ struct PhysParams { BodyJointsMax: i32, BodyCollidePairsN: i32, pad: i32, - pad1: i32, Gravity: vec4, } @@ -727,7 +728,7 @@ fn StepSolveJointLinear(ji: i32) { JointAxisLimitsUpdate(dof, axis, JointDoF(ji, dof, JointLimitLower), JointDoF(ji, dof, JointLimitUpper), &axisLimitsD, &axisLimitsA); var ke = JointControl(ji, dof, JointTargetStiff); var kd = JointControl(ji, dof, JointTargetDamp); - var targetPos = JointControl(ji, dof, JointTargetPos); + var targetPos = JointControl(ji, dof, JointTargetPosCur); var targetVel = JointControl(ji, dof, JointTargetVel); if (ke > 0.0) { // has position control JointAxisTarget(axis, targetPos, ke, &axisTargetPosKeD, &axisTargetPosKeA); @@ -911,7 +912,7 @@ fn StepSolveJointAngular(ji: i32) { JointAxisLimitsUpdate(dof, axis, JointDoF(ji, di, JointLimitLower), JointDoF(ji, di, JointLimitUpper), &axisLimitsD, &axisLimitsA); var ke = JointControl(ji, di, JointTargetStiff); var kd = JointControl(ji, di, JointTargetDamp); - var targetPos = JointControl(ji, di, JointTargetPos); + var targetPos = JointControl(ji, di, JointTargetPosCur); var targetVel = JointControl(ji, di, JointTargetVel); if (ke > 0.0) { // has position control JointAxisTarget(axis, targetPos, ke, &axisTargetPosKeD, &axisTargetPosKeA); diff --git a/physics/step.go b/physics/step.go index c4c14769..1edf59cf 100644 --- a/physics/step.go +++ b/physics/step.go @@ -34,6 +34,12 @@ func StepInit(i uint32) { //gosl:kernel read-write:Params params.Cur = 0 params.Next = 1 } + for j := range params.JointDoFsN { + tpos := JointControls.Value(int(j), int(JointTargetPos)) + tcur := JointControls.Value(int(j), int(JointTargetPosCur)) + tcur += params.ControlDt * (tpos - tcur) + JointControls.Set(tcur, int(j), int(JointTargetPosCur)) + } } // step does the following: diff --git a/physics/step.goal b/physics/step.goal index fc0e428c..6bffcfd1 100644 --- a/physics/step.goal +++ b/physics/step.goal @@ -32,6 +32,12 @@ func StepInit(i uint32) { //gosl:kernel read-write:Params params.Cur = 0 params.Next = 1 } + for j := range params.JointDoFsN { + tpos := JointControls[j, JointTargetPos] + tcur := JointControls[j, JointTargetPosCur] + tcur += params.ControlDt * (tpos - tcur) + JointControls[j, JointTargetPosCur] = tcur + } } // step does the following: diff --git a/physics/step_joint.go b/physics/step_joint.go index 8706d803..3210da01 100644 --- a/physics/step_joint.go +++ b/physics/step_joint.go @@ -266,7 +266,7 @@ func StepSolveJointLinear(ji int32) { JointAxisLimitsUpdate(dof, axis, JointDoF(ji, dof, JointLimitLower), JointDoF(ji, dof, JointLimitUpper), &axisLimitsD, &axisLimitsA) ke := JointControl(ji, dof, JointTargetStiff) kd := JointControl(ji, dof, JointTargetDamp) - targetPos := JointControl(ji, dof, JointTargetPos) + targetPos := JointControl(ji, dof, JointTargetPosCur) targetVel := JointControl(ji, dof, JointTargetVel) if ke > 0.0 { // has position control JointAxisTarget(axis, targetPos, ke, &axisTargetPosKeD, &axisTargetPosKeA) @@ -493,7 +493,7 @@ func StepSolveJointAngular(ji int32) { JointAxisLimitsUpdate(dof, axis, JointDoF(ji, di, JointLimitLower), JointDoF(ji, di, JointLimitUpper), &axisLimitsD, &axisLimitsA) ke := JointControl(ji, di, JointTargetStiff) kd := JointControl(ji, di, JointTargetDamp) - targetPos := JointControl(ji, di, JointTargetPos) + targetPos := JointControl(ji, di, JointTargetPosCur) targetVel := JointControl(ji, di, JointTargetVel) if ke > 0.0 { // has position control JointAxisTarget(axis, targetPos, ke, &axisTargetPosKeD, &axisTargetPosKeA) diff --git a/physics/step_joint.goal b/physics/step_joint.goal index 7970273b..9297fd45 100644 --- a/physics/step_joint.goal +++ b/physics/step_joint.goal @@ -264,7 +264,7 @@ func StepSolveJointLinear(ji int32) { JointAxisLimitsUpdate(dof, axis, JointDoF(ji, dof, JointLimitLower), JointDoF(ji, dof, JointLimitUpper), &axisLimitsD, &axisLimitsA) ke := JointControl(ji, dof, JointTargetStiff) kd := JointControl(ji, dof, JointTargetDamp) - targetPos := JointControl(ji, dof, JointTargetPos) + targetPos := JointControl(ji, dof, JointTargetPosCur) targetVel := JointControl(ji, dof, JointTargetVel) if ke > 0.0 { // has position control JointAxisTarget(axis, targetPos, ke, &axisTargetPosKeD, &axisTargetPosKeA) @@ -491,7 +491,7 @@ func StepSolveJointAngular(ji int32) { JointAxisLimitsUpdate(dof, axis, JointDoF(ji, di, JointLimitLower), JointDoF(ji, di, JointLimitUpper), &axisLimitsD, &axisLimitsA) ke := JointControl(ji, di, JointTargetStiff) kd := JointControl(ji, di, JointTargetDamp) - targetPos := JointControl(ji, di, JointTargetPos) + targetPos := JointControl(ji, di, JointTargetPosCur) targetVel := JointControl(ji, di, JointTargetVel) if ke > 0.0 { // has position control JointAxisTarget(axis, targetPos, ke, &axisTargetPosKeD, &axisTargetPosKeA) diff --git a/physics/typegen.go b/physics/typegen.go index 03f17c13..2da4ab86 100644 --- a/physics/typegen.go +++ b/physics/typegen.go @@ -22,9 +22,9 @@ var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.JointVars", var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.JointDoFVars", IDName: "joint-do-f-vars", Doc: "JointDoFVars are joint DoF state variables stored in tensor.Float32,\none for each DoF."}) -var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.Model", IDName: "model", Doc: "Model contains and manages all of the physics elements.", Directives: []types.Directive{{Tool: "go", Directive: "generate", Args: []string{"core", "generate", "-add-types", "-gosl"}}}, Fields: []types.Field{{Name: "GPU", Doc: "GPU determines whether to use GPU (else CPU)."}, {Name: "Params", Doc: "Params are global parameters."}, {Name: "CurrentWorld", Doc: "CurrentWorld is the [BodyWorld] value to use when creating new bodies.\nSet to -1 to create global elements that interact with everything,\nwhile 0 and positive numbers only interact amongst themselves."}, {Name: "CurrentObject", Doc: "CurrentObject is the Object to use when creating new joints.\nCall NewObject to increment."}, {Name: "CurrentObjectJoint", Doc: "CurrentObjectJoint is the Joint index in CurrentObject\nto use when creating new joints."}, {Name: "ReplicasStart", Doc: "ReplicasStart is the starting body index for replicated world bodies,\nwhich is needed for viewers to efficiently select a specific world to view.\nThis is the start of the World=0 first instance."}, {Name: "ReplicasN", Doc: "ReplicasN is the number of body elements within each set of\nreplicated world bodies, which is needed for viewers to efficiently select\na specific world to view."}, {Name: "Bodies", Doc: "Bodies are the rigid body elements (dynamic and static),\nspecifying the constant, non-dynamic properties,\nwhich is initial state for dynamics.\n[body][BodyVarsN]"}, {Name: "Objects", Doc: "Objects is a list of joint indexes for each object, where each object\ncontains all the joints interconnecting an overlapping set of bodies.\nJoints must be added in parent -> child order within objects, as joints\nare updated in sequential order within object.\n[object][maxjointsperobj]"}, {Name: "BodyJoints", Doc: "BodyJoints is a list of joint indexes for each dynamic body, for aggregating.\n[dyn body][parent, child][Params.BodyJointsMax]"}, {Name: "Joints", Doc: "Joints is a list of permanent joints connecting bodies,\nwhich do not change (no dynamic variables).\n[joint][JointVarsN]"}, {Name: "JointDoFs", Doc: "JointDoFs is a list of joint DoF parameters, allocated per joint.\n[dof][JointDoFVars]"}, {Name: "BodyCollidePairs", Doc: "BodyCollidePairs are pairs of Body indexes that could potentially collide\nbased on precomputed collision logic, using World, Group, and Joint indexes.\n[BodyCollidePairsN][2]"}, {Name: "Dynamics", Doc: "Dynamics are the dynamic rigid body elements: these actually move.\nThe first set of variables are for initial values, and the second current.\n[body][cur/next][DynamicVarsN]"}, {Name: "BroadContactsN", Doc: "BroadContactsN has number of points of broad contact\nbetween bodies. [1]"}, {Name: "BroadContacts", Doc: "BroadContacts are the results of broad-phase contact processing,\nestablishing possible points of contact between bodies.\n[ContactsMax][BroadContactVarsN]"}, {Name: "ContactsN", Doc: "ContactsN has number of points of narrow (final) contact\nbetween bodies. [1]"}, {Name: "Contacts", Doc: "Contacts are the results of narrow-phase contact processing,\nwhere only actual contacts with fully-specified values are present.\n[ContactsMax][ContactVarsN]"}, {Name: "JointControls", Doc: "JointControls are dynamic joint control inputs, per joint DoF\n(in correspondence with [JointDoFs]). This can be uploaded to the\nGPU at every step.\n[dof][JointControlVarsN]"}}}) +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.Model", IDName: "model", Doc: "Model contains and manages all of the physics elements.", Directives: []types.Directive{{Tool: "go", Directive: "generate", Args: []string{"core", "generate", "-add-types", "-gosl"}}}, Fields: []types.Field{{Name: "GPU", Doc: "GPU determines whether to use GPU (else CPU)."}, {Name: "Params", Doc: "Params are global parameters."}, {Name: "CurrentWorld", Doc: "CurrentWorld is the [BodyWorld] value to use when creating new bodies.\nSet to -1 to create global elements that interact with everything,\nwhile 0 and positive numbers only interact amongst themselves."}, {Name: "CurrentObject", Doc: "CurrentObject is the Object to use when creating new joints.\nCall NewObject to increment."}, {Name: "CurrentObjectJoint", Doc: "CurrentObjectJoint is the Joint index in CurrentObject\nto use when creating new joints."}, {Name: "ReplicasStart", Doc: "ReplicasStart is the starting body index for replicated world bodies,\nwhich is needed for viewers to efficiently select a specific world to view.\nThis is the start of the World=0 first instance."}, {Name: "ReplicasN", Doc: "ReplicasN is the number of body elements within each set of\nreplicated world bodies, which is needed for viewers to efficiently select\na specific world to view."}, {Name: "Bodies", Doc: "Bodies are the rigid body elements (dynamic and static),\nspecifying the constant, non-dynamic properties,\nwhich is initial state for dynamics.\n[body][BodyVarsN]"}, {Name: "Objects", Doc: "Objects is a list of joint indexes for each object, where each object\ncontains all the joints interconnecting an overlapping set of bodies.\nThis is known as an articulation in other physics software.\nJoints must be added in parent -> child order within objects, as joints\nare updated in sequential order within object.\n[object][MaxObjectJoints+1]"}, {Name: "BodyJoints", Doc: "BodyJoints is a list of joint indexes for each dynamic body, for aggregating.\n[dyn body][parent, child][Params.BodyJointsMax]"}, {Name: "Joints", Doc: "Joints is a list of permanent joints connecting bodies,\nwhich do not change (no dynamic variables).\n[joint][JointVarsN]"}, {Name: "JointDoFs", Doc: "JointDoFs is a list of joint DoF parameters, allocated per joint.\n[dof][JointDoFVars]"}, {Name: "BodyCollidePairs", Doc: "BodyCollidePairs are pairs of Body indexes that could potentially collide\nbased on precomputed collision logic, using World, Group, and Joint indexes.\n[BodyCollidePairsN][2]"}, {Name: "Dynamics", Doc: "Dynamics are the dynamic rigid body elements: these actually move.\nThe first set of variables are for initial values, and the second current.\n[body][cur/next][DynamicVarsN]"}, {Name: "BroadContactsN", Doc: "BroadContactsN has number of points of broad contact\nbetween bodies. [1]"}, {Name: "BroadContacts", Doc: "BroadContacts are the results of broad-phase contact processing,\nestablishing possible points of contact between bodies.\n[ContactsMax][BroadContactVarsN]"}, {Name: "ContactsN", Doc: "ContactsN has number of points of narrow (final) contact\nbetween bodies. [1]"}, {Name: "Contacts", Doc: "Contacts are the results of narrow-phase contact processing,\nwhere only actual contacts with fully-specified values are present.\n[ContactsMax][ContactVarsN]"}, {Name: "JointControls", Doc: "JointControls are dynamic joint control inputs, per joint DoF\n(in correspondence with [JointDoFs]). This can be uploaded to the\nGPU at every step.\n[dof][JointControlVarsN]"}}}) -var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.PhysParams", IDName: "phys-params", Doc: "PhysParams are the physics parameters", Directives: []types.Directive{{Tool: "gosl", Directive: "start"}}, Fields: []types.Field{{Name: "Iterations", Doc: "Iterations is the number of integration iterations to perform\nwithin each solver step. Muller et al (2020) report that 1 is best."}, {Name: "Dt", Doc: "Dt is the integration stepsize.\nFor highly kinetic situations (e.g., rapidly moving bouncing balls)\n0.0001 is needed to ensure contact registration. Use SubSteps to\naccomplish a target effective read-out step size."}, {Name: "SubSteps", Doc: "SubSteps is the number of integration steps to take per Step()\nfunction call. These sub steps are taken without any sync to/from\nthe GPU and are therefore much faster."}, {Name: "ContactMargin", Doc: "Contact margin is the extra distance for broadphase collision\naround rigid bodies. This can make some joints potentially unstable if > 0"}, {Name: "ContactRelax", Doc: "ContactRelax is rigid contact relaxation constant.\nHigher values cause errros"}, {Name: "ContactWeighting", Doc: "Contact weighting: balances contact forces?"}, {Name: "Restitution", Doc: "Restitution takes into account bounciness of objects."}, {Name: "JointLinearRelax", Doc: "JointLinearRelax is joint linear relaxation constant."}, {Name: "JointAngularRelax", Doc: "JointAngularRelax is joint angular relaxation constant."}, {Name: "JointLinearComply", Doc: "JointLinearComply is joint linear compliance constant."}, {Name: "JointAngularComply", Doc: "JointAngularComply is joint angular compliance constant."}, {Name: "AngularDamping", Doc: "AngularDamping is damping of angular motion."}, {Name: "SoftRelax", Doc: "SoftRelax is soft-body relaxation constant."}, {Name: "MaxForce", Doc: "MaxForce is the maximum computed force value, which prevents\nrunaway numerical overflow."}, {Name: "MaxGeomIter", Doc: "MaxGeomIter is number of iterations to perform in shape-based\ngeometry collision computations"}, {Name: "ContactsMax", Doc: "Maximum number of contacts to process at any given point."}, {Name: "Cur", Doc: "Index for the current state (0 or 1, alternates with Next)."}, {Name: "Next", Doc: "Index for the next state (1 or 0, alternates with Cur)."}, {Name: "BodiesN", Doc: "BodiesN is number of rigid bodies."}, {Name: "DynamicsN", Doc: "DynamicsN is number of dynamics bodies."}, {Name: "ObjectsN", Doc: "ObjectsN is number of objects."}, {Name: "MaxObjectJoints", Doc: "MaxObjectJoints is max number of joints per object."}, {Name: "JointsN", Doc: "JointsN is number of joints."}, {Name: "JointDoFsN", Doc: "JointDoFsN is number of joint DoFs."}, {Name: "BodyJointsMax", Doc: "BodyJointsMax is max number of joints per body + 1 for actual n."}, {Name: "BodyCollidePairsN", Doc: "BodyCollidePairsN is the total number of pre-compiled collision pairs\nto examine."}, {Name: "pad"}, {Name: "pad1"}, {Name: "Gravity", Doc: "Gravity is the gravity acceleration function"}}}) +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.PhysicsParams", IDName: "physics-params", Doc: "PhysicsParams are the physics parameters", Directives: []types.Directive{{Tool: "gosl", Directive: "start"}}, Fields: []types.Field{{Name: "Iterations", Doc: "Iterations is the number of integration iterations to perform\nwithin each solver step. Muller et al (2020) report that 1 is best."}, {Name: "Dt", Doc: "Dt is the integration stepsize.\nFor highly kinetic situations (e.g., rapidly moving bouncing balls)\n0.0001 is needed to ensure contact registration. Use SubSteps to\naccomplish a target effective read-out step size."}, {Name: "SubSteps", Doc: "SubSteps is the number of integration steps to take per Step()\nfunction call. These sub steps are taken without any sync to/from\nthe GPU and are therefore much faster."}, {Name: "ControlDt", Doc: "ControlDt is the stepsize for integrating joint control position values\n[JointTargetPos] over time, to avoid sudden strong changes in force."}, {Name: "ContactMargin", Doc: "Contact margin is the extra distance for broadphase collision\naround rigid bodies. This can make some joints potentially unstable if > 0"}, {Name: "ContactRelax", Doc: "ContactRelax is rigid contact relaxation constant.\nHigher values cause errros"}, {Name: "ContactWeighting", Doc: "Contact weighting: balances contact forces?"}, {Name: "Restitution", Doc: "Restitution takes into account bounciness of objects."}, {Name: "JointLinearRelax", Doc: "JointLinearRelax is joint linear relaxation constant."}, {Name: "JointAngularRelax", Doc: "JointAngularRelax is joint angular relaxation constant."}, {Name: "JointLinearComply", Doc: "JointLinearComply is joint linear compliance constant."}, {Name: "JointAngularComply", Doc: "JointAngularComply is joint angular compliance constant."}, {Name: "AngularDamping", Doc: "AngularDamping is damping of angular motion."}, {Name: "SoftRelax", Doc: "SoftRelax is soft-body relaxation constant."}, {Name: "MaxForce", Doc: "MaxForce is the maximum computed force value, which prevents\nrunaway numerical overflow."}, {Name: "MaxGeomIter", Doc: "MaxGeomIter is number of iterations to perform in shape-based\ngeometry collision computations"}, {Name: "ContactsMax", Doc: "Maximum number of contacts to process at any given point."}, {Name: "Cur", Doc: "Index for the current state (0 or 1, alternates with Next)."}, {Name: "Next", Doc: "Index for the next state (1 or 0, alternates with Cur)."}, {Name: "BodiesN", Doc: "BodiesN is number of rigid bodies."}, {Name: "DynamicsN", Doc: "DynamicsN is number of dynamics bodies."}, {Name: "ObjectsN", Doc: "ObjectsN is number of objects."}, {Name: "MaxObjectJoints", Doc: "MaxObjectJoints is max number of joints per object."}, {Name: "JointsN", Doc: "JointsN is number of joints."}, {Name: "JointDoFsN", Doc: "JointDoFsN is number of joint DoFs."}, {Name: "BodyJointsMax", Doc: "BodyJointsMax is max number of joints per body + 1 for actual n."}, {Name: "BodyCollidePairsN", Doc: "BodyCollidePairsN is the total number of pre-compiled collision pairs\nto examine."}, {Name: "pad"}, {Name: "Gravity", Doc: "Gravity is the gravity acceleration function"}}}) var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.GeomData", IDName: "geom-data", Doc: "GeomData contains all geometric data for narrow-phase collision.", Directives: []types.Directive{{Tool: "gosl", Directive: "start"}}, Fields: []types.Field{{Name: "BodyIdx"}, {Name: "Shape"}, {Name: "MinSize", Doc: "MinSize is the min of the Size dimensions."}, {Name: "Thick", Doc: "Thickness of shape."}, {Name: "Radius", Doc: "Radius is the effective radius for sphere-like elements (Sphere, Capsule, Cone)"}, {Name: "Size"}, {Name: "WbR", Doc: "World-to-Body transform\nPosition (R) (i.e., BodyPos)"}, {Name: "WbQ", Doc: "Quaternion (Q) (i.e., BodyQuat)"}, {Name: "BwR", Doc: "Body-to-World transform (inverse)\nPosition (R)"}, {Name: "BwQ", Doc: "Quaternion (Q)"}}}) diff --git a/physics/vars.go b/physics/vars.go index 9dbd3fb8..035e4aaa 100644 --- a/physics/vars.go +++ b/physics/vars.go @@ -20,7 +20,7 @@ var CurModel *Model var ( // Params are global parameters. //gosl:group Params - Params []PhysParams + Params []PhysicsParams // Bodies are the rigid body elements (dynamic and static), // specifying the constant, non-dynamic properties, @@ -32,9 +32,10 @@ var ( // Objects is a list of joint indexes for each object, where each object // contains all the joints interconnecting an overlapping set of bodies. + // This is known as an articulation in other physics software. // Joints must be added in parent -> child order within objects, as joints // are updated in sequential order within object. First element is n joints. - // [object][MaxJointsPerObject+1] + // [object][MaxObjectJoints+1] //gosl:dims 2 Objects *tensor.Int32 diff --git a/yaegilab/labsymbols/cogentcore_org-lab-physics.go b/yaegilab/labsymbols/cogentcore_org-lab-physics.go index c0609d8c..77add5ea 100644 --- a/yaegilab/labsymbols/cogentcore_org-lab-physics.go +++ b/yaegilab/labsymbols/cogentcore_org-lab-physics.go @@ -242,6 +242,10 @@ func init() { "GroupsCollide": reflect.ValueOf(physics.GroupsCollide), "InitDynamics": reflect.ValueOf(physics.InitDynamics), "InitGeomData": reflect.ValueOf(physics.InitGeomData), + "JointAngLambda": reflect.ValueOf(physics.JointAngLambda), + "JointAngLambdaX": reflect.ValueOf(physics.JointAngLambdaX), + "JointAngLambdaY": reflect.ValueOf(physics.JointAngLambdaY), + "JointAngLambdaZ": reflect.ValueOf(physics.JointAngLambdaZ), "JointAngularDoFN": reflect.ValueOf(physics.JointAngularDoFN), "JointAxis": reflect.ValueOf(physics.JointAxis), "JointAxisDoF": reflect.ValueOf(physics.JointAxisDoF), @@ -291,6 +295,10 @@ func init() { "JointLimitLower": reflect.ValueOf(physics.JointLimitLower), "JointLimitUnlimited": reflect.ValueOf(constant.MakeFromLiteral("10000000000", token.FLOAT, 0)), "JointLimitUpper": reflect.ValueOf(physics.JointLimitUpper), + "JointLinLambda": reflect.ValueOf(physics.JointLinLambda), + "JointLinLambdaX": reflect.ValueOf(physics.JointLinLambdaX), + "JointLinLambdaY": reflect.ValueOf(physics.JointLinLambdaY), + "JointLinLambdaZ": reflect.ValueOf(physics.JointLinLambdaZ), "JointLinearDoFN": reflect.ValueOf(physics.JointLinearDoFN), "JointPForce": reflect.ValueOf(physics.JointPForce), "JointPForceX": reflect.ValueOf(physics.JointPForceX), @@ -314,6 +322,7 @@ func init() { "JointParentIndex": reflect.ValueOf(physics.JointParentIndex), "JointTargetDamp": reflect.ValueOf(physics.JointTargetDamp), "JointTargetPos": reflect.ValueOf(physics.JointTargetPos), + "JointTargetPosCur": reflect.ValueOf(physics.JointTargetPosCur), "JointTargetStiff": reflect.ValueOf(physics.JointTargetStiff), "JointTargetVel": reflect.ValueOf(physics.JointTargetVel), "JointType": reflect.ValueOf(physics.JointType), @@ -425,6 +434,7 @@ func init() { "SetDynamicQuat": reflect.ValueOf(physics.SetDynamicQuat), "SetDynamicTorque": reflect.ValueOf(physics.SetDynamicTorque), "SetDynamicVel": reflect.ValueOf(physics.SetDynamicVel), + "SetJointAngLambda": reflect.ValueOf(physics.SetJointAngLambda), "SetJointAngularDoFN": reflect.ValueOf(physics.SetJointAngularDoFN), "SetJointAxis": reflect.ValueOf(physics.SetJointAxis), "SetJointAxisDoF": reflect.ValueOf(physics.SetJointAxisDoF), @@ -438,6 +448,7 @@ func init() { "SetJointDoF": reflect.ValueOf(physics.SetJointDoF), "SetJointDoFIndex": reflect.ValueOf(physics.SetJointDoFIndex), "SetJointEnabled": reflect.ValueOf(physics.SetJointEnabled), + "SetJointLinLambda": reflect.ValueOf(physics.SetJointLinLambda), "SetJointLinearDoFN": reflect.ValueOf(physics.SetJointLinearDoFN), "SetJointPForce": reflect.ValueOf(physics.SetJointPForce), "SetJointPPos": reflect.ValueOf(physics.SetJointPPos), @@ -483,7 +494,7 @@ func init() { "JointTypes": reflect.ValueOf((*physics.JointTypes)(nil)), "JointVars": reflect.ValueOf((*physics.JointVars)(nil)), "Model": reflect.ValueOf((*physics.Model)(nil)), - "PhysParams": reflect.ValueOf((*physics.PhysParams)(nil)), + "PhysicsParams": reflect.ValueOf((*physics.PhysicsParams)(nil)), "Shapes": reflect.ValueOf((*physics.Shapes)(nil)), } } From 5a9285443abaf22f9b8243080f0627989eeaa794 Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Sun, 4 Jan 2026 08:38:03 -0800 Subject: [PATCH 74/97] physics: major progress on XZ finaly: MaxDelta cures many ills! --- gosl/slmath/math.go | 2 +- physics/config.go | 1 + physics/config.goal | 1 + physics/examples/virtroom/virtroom.go | 1 - physics/params.go | 7 +- physics/shaders/CollisionBroad.wgsl | 2 +- physics/shaders/CollisionNarrow.wgsl | 2 +- physics/shaders/DynamicsCurToNext.wgsl | 2 +- physics/shaders/ForcesFromJoints.wgsl | 2 +- physics/shaders/InitDynamics.wgsl | 2 +- physics/shaders/StepBodyContactDeltas.wgsl | 10 ++- physics/shaders/StepBodyContacts.wgsl | 2 +- physics/shaders/StepIntegrateBodies.wgsl | 2 +- physics/shaders/StepJointForces.wgsl | 2 +- physics/shaders/StepSolveJoints.wgsl | 80 ++++---------------- physics/step_body.go | 13 ++++ physics/step_body.goal | 13 ++++ physics/step_joint.go | 83 ++------------------- physics/step_joint.goal | 85 ++-------------------- physics/typegen.go | 2 +- 20 files changed, 78 insertions(+), 236 deletions(-) diff --git a/gosl/slmath/math.go b/gosl/slmath/math.go index 5c659648..97794d91 100644 --- a/gosl/slmath/math.go +++ b/gosl/slmath/math.go @@ -9,7 +9,7 @@ package slmath const Pi = 3.141592653589793 // MinAngleDiff returns the minimum difference between two angles -// (in radians): a-b +// (in radians): a-b, dealing with the wrap-around issues with angles. func MinAngleDiff(a, b float32) float32 { d := a - b if d > Pi { diff --git a/physics/config.go b/physics/config.go index 76cbc92e..ded970f4 100644 --- a/physics/config.go +++ b/physics/config.go @@ -68,6 +68,7 @@ func (ml *Model) ConfigJoints() { } } if nj == 0 { + ml.Objects = tensor.NewInt32(1, 1) ml.Joints = tensor.NewFloat32(1, int(JointVarsN)) ml.JointDoFs = tensor.NewFloat32(1, int(JointDoFVarsN)) ml.JointControls = tensor.NewFloat32(1, int(JointControlVarsN)) diff --git a/physics/config.goal b/physics/config.goal index 050264b8..e1972c10 100644 --- a/physics/config.goal +++ b/physics/config.goal @@ -66,6 +66,7 @@ func (ml *Model) ConfigJoints() { } } if nj == 0 { + ml.Objects = tensor.NewInt32(1, 1) ml.Joints = tensor.NewFloat32(1, int(JointVarsN)) ml.JointDoFs = tensor.NewFloat32(1, int(JointDoFVarsN)) ml.JointControls = tensor.NewFloat32(1, int(JointControlVarsN)) diff --git a/physics/examples/virtroom/virtroom.go b/physics/examples/virtroom/virtroom.go index c1687fcf..b67f0bf3 100644 --- a/physics/examples/virtroom/virtroom.go +++ b/physics/examples/virtroom/virtroom.go @@ -321,7 +321,6 @@ func (ev *Env) MakeEmer(wl *builder.World, em *Emer, name string) { // body := physics.NewCylinder(emr, "body", math32.Vec3(0, hh, 0), hh, hw) em.XZ = obj.NewJointPlaneXZ(nil, emr, math32.Vec3(0, 0, 0), math32.Vec3(0, -hh, 0)) emr.Group = 0 // no collide (temporary) - return headPos := math32.Vec3(0, 2*hh+headsz, 0) head := obj.NewDynamicSkin(sc, name+"_head", physics.Box, "tan", mass*.1, math32.Vec3(headsz, headsz, headsz), headPos, rot) diff --git a/physics/params.go b/physics/params.go index f97b5295..929e4d96 100644 --- a/physics/params.go +++ b/physics/params.go @@ -69,6 +69,10 @@ type PhysicsParams struct { // runaway numerical overflow. MaxForce float32 `default:"1e5"` + // MaxDelta is the maximum computed change in position magnitude, + // which prevents runaway numerical overflow. + MaxDelta float32 `default:"2"` + // MaxGeomIter is number of iterations to perform in shape-based // geometry collision computations MaxGeomIter int32 `default:"10"` @@ -107,8 +111,6 @@ type PhysicsParams struct { // to examine. BodyCollidePairsN int32 `edit:"-"` - pad int32 - // Gravity is the gravity acceleration function Gravity slvec.Vector3 } @@ -133,6 +135,7 @@ func (pr *PhysicsParams) Defaults() { pr.AngularDamping = 0 pr.SoftRelax = 0.9 pr.MaxForce = 1.0e5 + pr.MaxDelta = 2 pr.MaxGeomIter = 10 } diff --git a/physics/shaders/CollisionBroad.wgsl b/physics/shaders/CollisionBroad.wgsl index 9ce7b23d..81ece103 100644 --- a/physics/shaders/CollisionBroad.wgsl +++ b/physics/shaders/CollisionBroad.wgsl @@ -364,6 +364,7 @@ struct PhysicsParams { AngularDamping: f32, // 0 def SoftRelax: f32, MaxForce: f32, + MaxDelta: f32, MaxGeomIter: i32, ContactsMax: i32, Cur: i32, @@ -376,7 +377,6 @@ struct PhysicsParams { JointDoFsN: i32, BodyJointsMax: i32, BodyCollidePairsN: i32, - pad: i32, Gravity: vec4, } diff --git a/physics/shaders/CollisionNarrow.wgsl b/physics/shaders/CollisionNarrow.wgsl index 90f29f7e..08c1815d 100644 --- a/physics/shaders/CollisionNarrow.wgsl +++ b/physics/shaders/CollisionNarrow.wgsl @@ -453,6 +453,7 @@ struct PhysicsParams { AngularDamping: f32, // 0 def SoftRelax: f32, MaxForce: f32, + MaxDelta: f32, MaxGeomIter: i32, ContactsMax: i32, Cur: i32, @@ -465,7 +466,6 @@ struct PhysicsParams { JointDoFsN: i32, BodyJointsMax: i32, BodyCollidePairsN: i32, - pad: i32, Gravity: vec4, } diff --git a/physics/shaders/DynamicsCurToNext.wgsl b/physics/shaders/DynamicsCurToNext.wgsl index a2293a23..4698d13e 100644 --- a/physics/shaders/DynamicsCurToNext.wgsl +++ b/physics/shaders/DynamicsCurToNext.wgsl @@ -247,6 +247,7 @@ struct PhysicsParams { AngularDamping: f32, // 0 def SoftRelax: f32, MaxForce: f32, + MaxDelta: f32, MaxGeomIter: i32, ContactsMax: i32, Cur: i32, @@ -259,7 +260,6 @@ struct PhysicsParams { JointDoFsN: i32, BodyJointsMax: i32, BodyCollidePairsN: i32, - pad: i32, Gravity: vec4, } diff --git a/physics/shaders/ForcesFromJoints.wgsl b/physics/shaders/ForcesFromJoints.wgsl index 5866d557..a89ea6e1 100644 --- a/physics/shaders/ForcesFromJoints.wgsl +++ b/physics/shaders/ForcesFromJoints.wgsl @@ -277,6 +277,7 @@ struct PhysicsParams { AngularDamping: f32, // 0 def SoftRelax: f32, MaxForce: f32, + MaxDelta: f32, MaxGeomIter: i32, ContactsMax: i32, Cur: i32, @@ -289,7 +290,6 @@ struct PhysicsParams { JointDoFsN: i32, BodyJointsMax: i32, BodyCollidePairsN: i32, - pad: i32, Gravity: vec4, } diff --git a/physics/shaders/InitDynamics.wgsl b/physics/shaders/InitDynamics.wgsl index 61060e24..dfe34352 100644 --- a/physics/shaders/InitDynamics.wgsl +++ b/physics/shaders/InitDynamics.wgsl @@ -254,6 +254,7 @@ struct PhysicsParams { AngularDamping: f32, // 0 def SoftRelax: f32, MaxForce: f32, + MaxDelta: f32, MaxGeomIter: i32, ContactsMax: i32, Cur: i32, @@ -266,7 +267,6 @@ struct PhysicsParams { JointDoFsN: i32, BodyJointsMax: i32, BodyCollidePairsN: i32, - pad: i32, Gravity: vec4, } diff --git a/physics/shaders/StepBodyContactDeltas.wgsl b/physics/shaders/StepBodyContactDeltas.wgsl index 9b95ae41..dfa86b26 100644 --- a/physics/shaders/StepBodyContactDeltas.wgsl +++ b/physics/shaders/StepBodyContactDeltas.wgsl @@ -350,6 +350,7 @@ struct PhysicsParams { AngularDamping: f32, // 0 def SoftRelax: f32, MaxForce: f32, + MaxDelta: f32, MaxGeomIter: i32, ContactsMax: i32, Cur: i32, @@ -362,7 +363,6 @@ struct PhysicsParams { JointDoFsN: i32, BodyJointsMax: i32, BodyCollidePairsN: i32, - pad: i32, Gravity: vec4, } @@ -465,6 +465,8 @@ fn StepBodyDeltas(di: i32,bi: i32, contacts: bool, cWt: f32, linDel: vec3,a } var dp = linDel*(invMass * weight); var dq = angDel*(weight); + dp = LimitDelta(dp, params.MaxDelta); + dq = LimitDelta(dq, params.MaxDelta); var wb = MulQuatVectorInverse(q0, w0); var dwb = invInertia*(MulQuatVectorInverse(q0, dq)); var tb = Cross3(dwb, inertia*(wb+(dwb)))+(Cross3(wb, inertia*(dwb))); @@ -489,5 +491,11 @@ fn StepBodyDeltas(di: i32,bi: i32, contacts: bool, cWt: f32, linDel: vec3,a SetDynamicAngDelta(di, params.Next, w1); Params[0] = params; } +fn LimitDelta(v: vec3, lim: f32) -> vec3 { + var l = Length3(v); + if (l < lim) { + return v; + }return v*((lim / l)); +} //////// import: "step_joint.go" \ No newline at end of file diff --git a/physics/shaders/StepBodyContacts.wgsl b/physics/shaders/StepBodyContacts.wgsl index 9f1b9dcc..54e963c1 100644 --- a/physics/shaders/StepBodyContacts.wgsl +++ b/physics/shaders/StepBodyContacts.wgsl @@ -479,6 +479,7 @@ struct PhysicsParams { AngularDamping: f32, // 0 def SoftRelax: f32, MaxForce: f32, + MaxDelta: f32, MaxGeomIter: i32, ContactsMax: i32, Cur: i32, @@ -491,7 +492,6 @@ struct PhysicsParams { JointDoFsN: i32, BodyJointsMax: i32, BodyCollidePairsN: i32, - pad: i32, Gravity: vec4, } diff --git a/physics/shaders/StepIntegrateBodies.wgsl b/physics/shaders/StepIntegrateBodies.wgsl index 29d7dab4..de5411d7 100644 --- a/physics/shaders/StepIntegrateBodies.wgsl +++ b/physics/shaders/StepIntegrateBodies.wgsl @@ -277,6 +277,7 @@ struct PhysicsParams { AngularDamping: f32, // 0 def SoftRelax: f32, MaxForce: f32, + MaxDelta: f32, MaxGeomIter: i32, ContactsMax: i32, Cur: i32, @@ -289,7 +290,6 @@ struct PhysicsParams { JointDoFsN: i32, BodyJointsMax: i32, BodyCollidePairsN: i32, - pad: i32, Gravity: vec4, } diff --git a/physics/shaders/StepJointForces.wgsl b/physics/shaders/StepJointForces.wgsl index e3868120..3352c887 100644 --- a/physics/shaders/StepJointForces.wgsl +++ b/physics/shaders/StepJointForces.wgsl @@ -322,6 +322,7 @@ struct PhysicsParams { AngularDamping: f32, // 0 def SoftRelax: f32, MaxForce: f32, + MaxDelta: f32, MaxGeomIter: i32, ContactsMax: i32, Cur: i32, @@ -334,7 +335,6 @@ struct PhysicsParams { JointDoFsN: i32, BodyJointsMax: i32, BodyCollidePairsN: i32, - pad: i32, Gravity: vec4, } diff --git a/physics/shaders/StepSolveJoints.wgsl b/physics/shaders/StepSolveJoints.wgsl index f4fdfbd8..032b6dcd 100644 --- a/physics/shaders/StepSolveJoints.wgsl +++ b/physics/shaders/StepSolveJoints.wgsl @@ -361,6 +361,7 @@ struct PhysicsParams { AngularDamping: f32, // 0 def SoftRelax: f32, MaxForce: f32, + MaxDelta: f32, MaxGeomIter: i32, ContactsMax: i32, Cur: i32, @@ -373,7 +374,6 @@ struct PhysicsParams { JointDoFsN: i32, BodyJointsMax: i32, BodyCollidePairsN: i32, - pad: i32, Gravity: vec4, } @@ -569,6 +569,8 @@ fn StepBodyDeltas(di: i32,bi: i32, contacts: bool, cWt: f32, linDel: vec3,a } var dp = linDel*(invMass * weight); var dq = angDel*(weight); + dp = LimitDelta(dp, params.MaxDelta); + dq = LimitDelta(dq, params.MaxDelta); var wb = MulQuatVectorInverse(q0, w0); var dwb = invInertia*(MulQuatVectorInverse(q0, dq)); var tb = Cross3(dwb, inertia*(wb+(dwb)))+(Cross3(wb, inertia*(dwb))); @@ -593,6 +595,12 @@ fn StepBodyDeltas(di: i32,bi: i32, contacts: bool, cWt: f32, linDel: vec3,a SetDynamicAngDelta(di, params.Next, w1); Params[0] = params; } +fn LimitDelta(v: vec3, lim: f32) -> vec3 { + var l = Length3(v); + if (l < lim) { + return v; + }return v*((lim / l)); +} //////// import: "step_joint.go" fn StepSolveJoints(i: u32) { //gosl:kernel @@ -609,12 +617,11 @@ fn StepSolveJoints(i: u32) { //gosl:kernel if (jt == Free || !GetJointEnabled(ji)) { continue; } - StepSolveJointLinear(ji); - StepSolveJointAngular(ji); + StepSolveJoint(ji); } Params[0] = params; } -fn StepSolveJointLinear(ji: i32) { +fn StepSolveJoint(ji: i32) { var params = Params[0]; var jt = GetJointType(ji); var jPi = JointParentIndex(ji); @@ -791,68 +798,7 @@ fn StepSolveJointLinear(ji: i32) { } } SetJointLinLambda(ji, lambdaNext); - if (!parentFixed) { - StepBodyDeltas(jPi, jPbi, false, f32(f32(0)), linDeltaP, angDeltaP); - } - if (mInvC > 0) { - StepBodyDeltas(jCi, jCbi, false, f32(f32(0)), linDeltaC, angDeltaC); - } - Params[0] = params; -} -fn StepSolveJointAngular(ji: i32) { - var params = Params[0]; - var jPi = JointParentIndex(ji); - var jPbi = i32(-1); - var parentFixed = true; - if (jPi >= 0) { - jPbi = DynamicBody(jPi); - parentFixed = GetJointParentFixed(ji); - } - var jCi = JointChildIndex(ji); - var jCbi = DynamicBody(jCi); - var jLinearN = GetJointLinearDoFN(ji); var jAngularN = GetJointAngularDoFN(ji); - var jPR = JointPPos(ji); - var jPQ = JointPQuat(ji); - var xwPR = jPR; // world xform, parent, pos - var xwPQ = jPQ; // quat - var mInvP = f32(0.0); - var iInvP = mat3x3f(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0); - var posePR = jPR; - var posePQ = jPQ; - var wP: vec3; - if (jPi >= 0) { - posePR = DynamicPos(jPi, params.Next); // now using next - posePQ = DynamicQuat(jPi, params.Next); - MulSpatialTransforms(posePR, posePQ, jPR, jPQ, &xwPR, &xwPQ); - mInvP = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(jPbi), u32(BodyInvMass))]; - iInvP = BodyInvInertia(jPbi); - wP = DynamicAngDelta(jPi, params.Next); - if (mInvP == 0) { - parentFixed = true; - } - } - var poseCR = DynamicPos(jCi, params.Next); - var poseCQ = DynamicQuat(jCi, params.Next); - var jCR = JointCPos(ji); - var jCQ = JointCQuat(ji); - var xwCR = jCR; - var xwCQ = jCQ; - MulSpatialTransforms(poseCR, poseCQ, jCR, jCQ, &xwCR, &xwCQ); - var mInvC = Bodies[Index2D(TensorStrides[0], TensorStrides[1], u32(jCbi), u32(BodyInvMass))]; - var iInvC = BodyInvInertia(jCbi); - var wC = DynamicAngDelta(jCi, params.Next); - if (mInvP == 0.0 && mInvC == 0.0) { // connection between two immovable bodies - return; - } - var linDeltaP: vec3; - var angDeltaP: vec3; - var linDeltaC: vec3; - var angDeltaC: vec3; - var relPoseR = xwPR; - var relPoseQ = xwPQ; - SpatialTransformInverse(xwPR, xwPQ, &relPoseR, &relPoseQ); - MulSpatialTransforms(relPoseR, relPoseQ, xwCR, xwCQ, &relPoseR, &relPoseQ); var qP = xwPQ; var qC = xwCQ; if (QuatDot(qP, qC) < 0) { @@ -904,8 +850,8 @@ fn StepSolveJointAngular(ji: i32) { var axisTargetPosKeA: vec3; var axisTargetVelKdD: vec3; var axisTargetVelKdA: vec3; - var lambdaPrev = JointAngLambda(ji); - var lambdaNext = vec3(0, 0, 0); + lambdaPrev = JointAngLambda(ji); + lambdaNext = vec3(0, 0, 0); for (var dof=0; dof 0 { - StepBodyDeltas(jCi, jCbi, false, 0, linDeltaC, angDeltaC) - } -} - -// StepSolveJointAngular applies target positions to angular DoFs. -func StepSolveJointAngular(ji int32) { - params := GetParams(0) - jPi := JointParentIndex(ji) - jPbi := int32(-1) - parentFixed := true - if jPi >= 0 { - jPbi = DynamicBody(jPi) - parentFixed = GetJointParentFixed(ji) - } - jCi := JointChildIndex(ji) - jCbi := DynamicBody(jCi) + //////// Angular positions - jLinearN := GetJointLinearDoFN(ji) jAngularN := GetJointAngularDoFN(ji) - jPR := JointPPos(ji) - jPQ := JointPQuat(ji) - - xwPR := jPR // world xform, parent, pos - xwPQ := jPQ // quat - mInvP := float32(0.0) - iInvP := math32.Mat3(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0) - posePR := jPR - posePQ := jPQ - - var wP math32.Vector3 - - // parent transform and moment arm - if jPi >= 0 { - posePR = DynamicPos(jPi, params.Next) // now using next - posePQ = DynamicQuat(jPi, params.Next) - slmath.MulSpatialTransforms(posePR, posePQ, jPR, jPQ, &xwPR, &xwPQ) - mInvP = Bodies.Value(int(jPbi), int(BodyInvMass)) - iInvP = BodyInvInertia(jPbi) - wP = DynamicAngDelta(jPi, params.Next) - if mInvP == 0 { - parentFixed = true - } - } - - // child transform and moment arm - poseCR := DynamicPos(jCi, params.Next) - poseCQ := DynamicQuat(jCi, params.Next) - jCR := JointCPos(ji) - jCQ := JointCQuat(ji) - xwCR := jCR - xwCQ := jCQ - slmath.MulSpatialTransforms(poseCR, poseCQ, jCR, jCQ, &xwCR, &xwCQ) - mInvC := Bodies.Value(int(jCbi), int(BodyInvMass)) - iInvC := BodyInvInertia(jCbi) - wC := DynamicAngDelta(jCi, params.Next) - - if mInvP == 0.0 && mInvC == 0.0 { // connection between two immovable bodies - return - } - - // accumulate constraint deltas - var linDeltaP, angDeltaP, linDeltaC, angDeltaC math32.Vector3 - - relPoseR := xwPR - relPoseQ := xwPQ - slmath.SpatialTransformInverse(xwPR, xwPQ, &relPoseR, &relPoseQ) - slmath.MulSpatialTransforms(relPoseR, relPoseQ, xwCR, xwCQ, &relPoseR, &relPoseQ) - qP := xwPQ qC := xwCQ // make quats lie in same hemisphere @@ -484,8 +413,8 @@ func StepSolveJointAngular(ji int32) { var axisLimitsD, axisLimitsA math32.Vector3 var axisTargetPosKeD, axisTargetPosKeA math32.Vector3 var axisTargetVelKdD, axisTargetVelKdA math32.Vector3 - lambdaPrev := JointAngLambda(ji) - lambdaNext := math32.Vec3(0, 0, 0) + lambdaPrev = JointAngLambda(ji) + lambdaNext = math32.Vec3(0, 0, 0) for dof := range jAngularN { di := dof + jLinearN diff --git a/physics/step_joint.goal b/physics/step_joint.goal index 9297fd45..0fab6daa 100644 --- a/physics/step_joint.goal +++ b/physics/step_joint.goal @@ -128,14 +128,13 @@ func StepSolveJoints(i uint32) { //gosl:kernel if jt == Free || !GetJointEnabled(ji) { continue } - StepSolveJointLinear(ji) - StepSolveJointAngular(ji) + StepSolveJoint(ji) } } -// StepSolveJointLinear applies target positions to linear DoFs. +// StepSolveJoint applies target positions to linear DoFs. // Position is updated prior to computing angulars. -func StepSolveJointLinear(ji int32) { +func StepSolveJoint(ji int32) { params := GetParams(0) jt := GetJointType(ji) @@ -345,81 +344,11 @@ func StepSolveJointLinear(ji int32) { } } SetJointLinLambda(ji, lambdaNext) - // apply the linear deltas so the - if !parentFixed { - StepBodyDeltas(jPi, jPbi, false, 0, linDeltaP, angDeltaP) - } - if mInvC > 0 { - StepBodyDeltas(jCi, jCbi, false, 0, linDeltaC, angDeltaC) - } -} - -// StepSolveJointAngular applies target positions to angular DoFs. -func StepSolveJointAngular(ji int32) { - params := GetParams(0) - jPi := JointParentIndex(ji) - jPbi := int32(-1) - parentFixed := true - if jPi >= 0 { - jPbi = DynamicBody(jPi) - parentFixed = GetJointParentFixed(ji) - } - jCi := JointChildIndex(ji) - jCbi := DynamicBody(jCi) - - jLinearN := GetJointLinearDoFN(ji) + //////// Angular positions + jAngularN := GetJointAngularDoFN(ji) - jPR := JointPPos(ji) - jPQ := JointPQuat(ji) - - xwPR := jPR // world xform, parent, pos - xwPQ := jPQ // quat - mInvP := float32(0.0) - iInvP := math32.Mat3(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0) - posePR := jPR - posePQ := jPQ - - var wP math32.Vector3 - - // parent transform and moment arm - if jPi >= 0 { - posePR = DynamicPos(jPi, params.Next) // now using next - posePQ = DynamicQuat(jPi, params.Next) - slmath.MulSpatialTransforms(posePR, posePQ, jPR, jPQ, &xwPR, &xwPQ) - mInvP = Bodies[jPbi, BodyInvMass] - iInvP = BodyInvInertia(jPbi) - wP = DynamicAngDelta(jPi, params.Next) - if mInvP == 0 { - parentFixed = true - } - } - - // child transform and moment arm - poseCR := DynamicPos(jCi, params.Next) - poseCQ := DynamicQuat(jCi, params.Next) - jCR := JointCPos(ji) - jCQ := JointCQuat(ji) - xwCR := jCR - xwCQ := jCQ - slmath.MulSpatialTransforms(poseCR, poseCQ, jCR, jCQ, &xwCR, &xwCQ) - mInvC := Bodies[jCbi, BodyInvMass] - iInvC := BodyInvInertia(jCbi) - wC := DynamicAngDelta(jCi, params.Next) - - if mInvP == 0.0 && mInvC == 0.0 { // connection between two immovable bodies - return - } - - // accumulate constraint deltas - var linDeltaP, angDeltaP, linDeltaC, angDeltaC math32.Vector3 - - relPoseR := xwPR - relPoseQ := xwPQ - slmath.SpatialTransformInverse(xwPR, xwPQ, &relPoseR, &relPoseQ) - slmath.MulSpatialTransforms(relPoseR, relPoseQ, xwCR, xwCQ, &relPoseR, &relPoseQ) - qP := xwPQ qC := xwCQ // make quats lie in same hemisphere @@ -482,8 +411,8 @@ func StepSolveJointAngular(ji int32) { var axisLimitsD, axisLimitsA math32.Vector3 var axisTargetPosKeD, axisTargetPosKeA math32.Vector3 var axisTargetVelKdD, axisTargetVelKdA math32.Vector3 - lambdaPrev := JointAngLambda(ji) - lambdaNext := math32.Vec3(0,0,0) + lambdaPrev = JointAngLambda(ji) + lambdaNext = math32.Vec3(0,0,0) for dof := range jAngularN { di := dof + jLinearN diff --git a/physics/typegen.go b/physics/typegen.go index 2da4ab86..bf76df56 100644 --- a/physics/typegen.go +++ b/physics/typegen.go @@ -24,7 +24,7 @@ var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.JointDoFVars var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.Model", IDName: "model", Doc: "Model contains and manages all of the physics elements.", Directives: []types.Directive{{Tool: "go", Directive: "generate", Args: []string{"core", "generate", "-add-types", "-gosl"}}}, Fields: []types.Field{{Name: "GPU", Doc: "GPU determines whether to use GPU (else CPU)."}, {Name: "Params", Doc: "Params are global parameters."}, {Name: "CurrentWorld", Doc: "CurrentWorld is the [BodyWorld] value to use when creating new bodies.\nSet to -1 to create global elements that interact with everything,\nwhile 0 and positive numbers only interact amongst themselves."}, {Name: "CurrentObject", Doc: "CurrentObject is the Object to use when creating new joints.\nCall NewObject to increment."}, {Name: "CurrentObjectJoint", Doc: "CurrentObjectJoint is the Joint index in CurrentObject\nto use when creating new joints."}, {Name: "ReplicasStart", Doc: "ReplicasStart is the starting body index for replicated world bodies,\nwhich is needed for viewers to efficiently select a specific world to view.\nThis is the start of the World=0 first instance."}, {Name: "ReplicasN", Doc: "ReplicasN is the number of body elements within each set of\nreplicated world bodies, which is needed for viewers to efficiently select\na specific world to view."}, {Name: "Bodies", Doc: "Bodies are the rigid body elements (dynamic and static),\nspecifying the constant, non-dynamic properties,\nwhich is initial state for dynamics.\n[body][BodyVarsN]"}, {Name: "Objects", Doc: "Objects is a list of joint indexes for each object, where each object\ncontains all the joints interconnecting an overlapping set of bodies.\nThis is known as an articulation in other physics software.\nJoints must be added in parent -> child order within objects, as joints\nare updated in sequential order within object.\n[object][MaxObjectJoints+1]"}, {Name: "BodyJoints", Doc: "BodyJoints is a list of joint indexes for each dynamic body, for aggregating.\n[dyn body][parent, child][Params.BodyJointsMax]"}, {Name: "Joints", Doc: "Joints is a list of permanent joints connecting bodies,\nwhich do not change (no dynamic variables).\n[joint][JointVarsN]"}, {Name: "JointDoFs", Doc: "JointDoFs is a list of joint DoF parameters, allocated per joint.\n[dof][JointDoFVars]"}, {Name: "BodyCollidePairs", Doc: "BodyCollidePairs are pairs of Body indexes that could potentially collide\nbased on precomputed collision logic, using World, Group, and Joint indexes.\n[BodyCollidePairsN][2]"}, {Name: "Dynamics", Doc: "Dynamics are the dynamic rigid body elements: these actually move.\nThe first set of variables are for initial values, and the second current.\n[body][cur/next][DynamicVarsN]"}, {Name: "BroadContactsN", Doc: "BroadContactsN has number of points of broad contact\nbetween bodies. [1]"}, {Name: "BroadContacts", Doc: "BroadContacts are the results of broad-phase contact processing,\nestablishing possible points of contact between bodies.\n[ContactsMax][BroadContactVarsN]"}, {Name: "ContactsN", Doc: "ContactsN has number of points of narrow (final) contact\nbetween bodies. [1]"}, {Name: "Contacts", Doc: "Contacts are the results of narrow-phase contact processing,\nwhere only actual contacts with fully-specified values are present.\n[ContactsMax][ContactVarsN]"}, {Name: "JointControls", Doc: "JointControls are dynamic joint control inputs, per joint DoF\n(in correspondence with [JointDoFs]). This can be uploaded to the\nGPU at every step.\n[dof][JointControlVarsN]"}}}) -var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.PhysicsParams", IDName: "physics-params", Doc: "PhysicsParams are the physics parameters", Directives: []types.Directive{{Tool: "gosl", Directive: "start"}}, Fields: []types.Field{{Name: "Iterations", Doc: "Iterations is the number of integration iterations to perform\nwithin each solver step. Muller et al (2020) report that 1 is best."}, {Name: "Dt", Doc: "Dt is the integration stepsize.\nFor highly kinetic situations (e.g., rapidly moving bouncing balls)\n0.0001 is needed to ensure contact registration. Use SubSteps to\naccomplish a target effective read-out step size."}, {Name: "SubSteps", Doc: "SubSteps is the number of integration steps to take per Step()\nfunction call. These sub steps are taken without any sync to/from\nthe GPU and are therefore much faster."}, {Name: "ControlDt", Doc: "ControlDt is the stepsize for integrating joint control position values\n[JointTargetPos] over time, to avoid sudden strong changes in force."}, {Name: "ContactMargin", Doc: "Contact margin is the extra distance for broadphase collision\naround rigid bodies. This can make some joints potentially unstable if > 0"}, {Name: "ContactRelax", Doc: "ContactRelax is rigid contact relaxation constant.\nHigher values cause errros"}, {Name: "ContactWeighting", Doc: "Contact weighting: balances contact forces?"}, {Name: "Restitution", Doc: "Restitution takes into account bounciness of objects."}, {Name: "JointLinearRelax", Doc: "JointLinearRelax is joint linear relaxation constant."}, {Name: "JointAngularRelax", Doc: "JointAngularRelax is joint angular relaxation constant."}, {Name: "JointLinearComply", Doc: "JointLinearComply is joint linear compliance constant."}, {Name: "JointAngularComply", Doc: "JointAngularComply is joint angular compliance constant."}, {Name: "AngularDamping", Doc: "AngularDamping is damping of angular motion."}, {Name: "SoftRelax", Doc: "SoftRelax is soft-body relaxation constant."}, {Name: "MaxForce", Doc: "MaxForce is the maximum computed force value, which prevents\nrunaway numerical overflow."}, {Name: "MaxGeomIter", Doc: "MaxGeomIter is number of iterations to perform in shape-based\ngeometry collision computations"}, {Name: "ContactsMax", Doc: "Maximum number of contacts to process at any given point."}, {Name: "Cur", Doc: "Index for the current state (0 or 1, alternates with Next)."}, {Name: "Next", Doc: "Index for the next state (1 or 0, alternates with Cur)."}, {Name: "BodiesN", Doc: "BodiesN is number of rigid bodies."}, {Name: "DynamicsN", Doc: "DynamicsN is number of dynamics bodies."}, {Name: "ObjectsN", Doc: "ObjectsN is number of objects."}, {Name: "MaxObjectJoints", Doc: "MaxObjectJoints is max number of joints per object."}, {Name: "JointsN", Doc: "JointsN is number of joints."}, {Name: "JointDoFsN", Doc: "JointDoFsN is number of joint DoFs."}, {Name: "BodyJointsMax", Doc: "BodyJointsMax is max number of joints per body + 1 for actual n."}, {Name: "BodyCollidePairsN", Doc: "BodyCollidePairsN is the total number of pre-compiled collision pairs\nto examine."}, {Name: "pad"}, {Name: "Gravity", Doc: "Gravity is the gravity acceleration function"}}}) +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.PhysicsParams", IDName: "physics-params", Doc: "PhysicsParams are the physics parameters", Directives: []types.Directive{{Tool: "gosl", Directive: "start"}}, Fields: []types.Field{{Name: "Iterations", Doc: "Iterations is the number of integration iterations to perform\nwithin each solver step. Muller et al (2020) report that 1 is best."}, {Name: "Dt", Doc: "Dt is the integration stepsize.\nFor highly kinetic situations (e.g., rapidly moving bouncing balls)\n0.0001 is needed to ensure contact registration. Use SubSteps to\naccomplish a target effective read-out step size."}, {Name: "SubSteps", Doc: "SubSteps is the number of integration steps to take per Step()\nfunction call. These sub steps are taken without any sync to/from\nthe GPU and are therefore much faster."}, {Name: "ControlDt", Doc: "ControlDt is the stepsize for integrating joint control position values\n[JointTargetPos] over time, to avoid sudden strong changes in force."}, {Name: "ContactMargin", Doc: "Contact margin is the extra distance for broadphase collision\naround rigid bodies. This can make some joints potentially unstable if > 0"}, {Name: "ContactRelax", Doc: "ContactRelax is rigid contact relaxation constant.\nHigher values cause errros"}, {Name: "ContactWeighting", Doc: "Contact weighting: balances contact forces?"}, {Name: "Restitution", Doc: "Restitution takes into account bounciness of objects."}, {Name: "JointLinearRelax", Doc: "JointLinearRelax is joint linear relaxation constant."}, {Name: "JointAngularRelax", Doc: "JointAngularRelax is joint angular relaxation constant."}, {Name: "JointLinearComply", Doc: "JointLinearComply is joint linear compliance constant."}, {Name: "JointAngularComply", Doc: "JointAngularComply is joint angular compliance constant."}, {Name: "AngularDamping", Doc: "AngularDamping is damping of angular motion."}, {Name: "SoftRelax", Doc: "SoftRelax is soft-body relaxation constant."}, {Name: "MaxForce", Doc: "MaxForce is the maximum computed force value, which prevents\nrunaway numerical overflow."}, {Name: "MaxDelta", Doc: "MaxDelta is the maximum computed change in position magnitude,\nwhich prevents runaway numerical overflow."}, {Name: "MaxGeomIter", Doc: "MaxGeomIter is number of iterations to perform in shape-based\ngeometry collision computations"}, {Name: "ContactsMax", Doc: "Maximum number of contacts to process at any given point."}, {Name: "Cur", Doc: "Index for the current state (0 or 1, alternates with Next)."}, {Name: "Next", Doc: "Index for the next state (1 or 0, alternates with Cur)."}, {Name: "BodiesN", Doc: "BodiesN is number of rigid bodies."}, {Name: "DynamicsN", Doc: "DynamicsN is number of dynamics bodies."}, {Name: "ObjectsN", Doc: "ObjectsN is number of objects."}, {Name: "MaxObjectJoints", Doc: "MaxObjectJoints is max number of joints per object."}, {Name: "JointsN", Doc: "JointsN is number of joints."}, {Name: "JointDoFsN", Doc: "JointDoFsN is number of joint DoFs."}, {Name: "BodyJointsMax", Doc: "BodyJointsMax is max number of joints per body + 1 for actual n."}, {Name: "BodyCollidePairsN", Doc: "BodyCollidePairsN is the total number of pre-compiled collision pairs\nto examine."}, {Name: "Gravity", Doc: "Gravity is the gravity acceleration function"}}}) var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.GeomData", IDName: "geom-data", Doc: "GeomData contains all geometric data for narrow-phase collision.", Directives: []types.Directive{{Tool: "gosl", Directive: "start"}}, Fields: []types.Field{{Name: "BodyIdx"}, {Name: "Shape"}, {Name: "MinSize", Doc: "MinSize is the min of the Size dimensions."}, {Name: "Thick", Doc: "Thickness of shape."}, {Name: "Radius", Doc: "Radius is the effective radius for sphere-like elements (Sphere, Capsule, Cone)"}, {Name: "Size"}, {Name: "WbR", Doc: "World-to-Body transform\nPosition (R) (i.e., BodyPos)"}, {Name: "WbQ", Doc: "Quaternion (Q) (i.e., BodyQuat)"}, {Name: "BwR", Doc: "Body-to-World transform (inverse)\nPosition (R)"}, {Name: "BwQ", Doc: "Quaternion (Q)"}}}) From 33c2e274d6023ffa072aea4f06516e47dd2b0e2a Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Sun, 4 Jan 2026 10:10:51 -0800 Subject: [PATCH 75/97] physics: ControlDtThr and updated docs --- docs/content/physics.md | 5 +++-- physics/examples/virtroom/virtroom.go | 3 ++- physics/params.go | 14 ++++++++++++-- physics/shaders/CollisionBroad.wgsl | 4 ++++ physics/shaders/CollisionNarrow.wgsl | 4 ++++ physics/shaders/DynamicsCurToNext.wgsl | 4 ++++ physics/shaders/ForcesFromJoints.wgsl | 4 ++++ physics/shaders/InitDynamics.wgsl | 4 ++++ physics/shaders/StepBodyContactDeltas.wgsl | 4 ++++ physics/shaders/StepBodyContacts.wgsl | 4 ++++ physics/shaders/StepIntegrateBodies.wgsl | 4 ++++ physics/shaders/StepJointForces.wgsl | 4 ++++ physics/shaders/StepSolveJoints.wgsl | 4 ++++ physics/step.go | 8 +++++++- physics/step.goal | 8 +++++++- physics/typegen.go | 2 +- yaegilab/labsymbols/cogentcore_org-lab-physics.go | 4 ++-- 17 files changed, 74 insertions(+), 10 deletions(-) diff --git a/docs/content/physics.md b/docs/content/physics.md index 22fa1a92..b4e6a333 100644 --- a/docs/content/physics.md +++ b/docs/content/physics.md @@ -105,7 +105,7 @@ ed.SetConfigFunc(func() { obj := sc.NewDynamic(ml, "body", physics.Box, "blue", mass, hsz, math32.Vec3(0, hsz.Y, 0), math32.NewQuatIdentity()) ml.NewObject() - ji := sc.NewJointPrismatic(ml, nil, obj, math32.Vec3(-5, 0, 0), math32.Vec3(0, hsz.Y, 0), math32.Vec3(1, 0, 0)) + ji := sc.NewJointPrismatic(ml, nil, obj, math32.Vec3(-5, 0, 0), math32.Vec3(0, -hsz.Y, 0), math32.Vec3(1, 0, 0)) }) // variables to control @@ -169,9 +169,10 @@ ed.Styler(func(s *styles.Style) { ed.SetConfigFunc(func() { ml := ed.Model + physics.GetParams(0).ControlDt = 0.1 // much better behaved with this sc := ed.Scene hsz := math32.Vec3(0.5, 1.5, 0.2) - mass := float32(0.1) + mass := float32(1) obj := sc.NewDynamic(ml, "body", physics.Box, "blue", mass, hsz, math32.Vec3(0, hsz.Y, 0), math32.NewQuatIdentity()) ml.NewObject() diff --git a/physics/examples/virtroom/virtroom.go b/physics/examples/virtroom/virtroom.go index b67f0bf3..5ca770af 100644 --- a/physics/examples/virtroom/virtroom.go +++ b/physics/examples/virtroom/virtroom.go @@ -156,7 +156,8 @@ func (ev *Env) MakeWorld(sc *xyz.Scene) { ev.MakeEmer(ew, &ev.Emer, "emer") // ev.Physics.Builder.ReplicateWorld(1, 8, 2) ev.Physics.Build() - // params := physics.GetParams(0) + params := physics.GetParams(0) + params.ControlDt = 1 // params.Gravity.Y = 0 // params.MaxForce = 1.0e3 // params.AngularDamping = 0.5 diff --git a/physics/params.go b/physics/params.go index 929e4d96..3acdd50d 100644 --- a/physics/params.go +++ b/physics/params.go @@ -31,7 +31,14 @@ type PhysicsParams struct { // ControlDt is the stepsize for integrating joint control position values // [JointTargetPos] over time, to avoid sudden strong changes in force. - ControlDt float32 `default:"0.1"` + // For higher-DoF joints (e.g., Ball), this can be important for stability, + // but it can also result in under-shoot of the target position. + ControlDt float32 `default:"1,0.1"` + + // ControlDtThr is the threshold on the control delta above which + // ControlDt is used. ControlDt is most important for large changes, + // and can result in under-shoot if engaged for small changes. + ControlDtThr float32 `default:"1"` // Contact margin is the extra distance for broadphase collision // around rigid bodies. This can make some joints potentially unstable if > 0 @@ -111,6 +118,8 @@ type PhysicsParams struct { // to examine. BodyCollidePairsN int32 `edit:"-"` + pad, pad1, pad2 int32 + // Gravity is the gravity acceleration function Gravity slvec.Vector3 } @@ -119,7 +128,8 @@ func (pr *PhysicsParams) Defaults() { pr.Iterations = 1 pr.Dt = 0.0001 pr.SubSteps = 10 - pr.ControlDt = 0.1 + pr.ControlDt = 1 + pr.ControlDtThr = 1 pr.Gravity.Set(0, -9.81, 0) pr.ContactMargin = 0 diff --git a/physics/shaders/CollisionBroad.wgsl b/physics/shaders/CollisionBroad.wgsl index 81ece103..2337a6b0 100644 --- a/physics/shaders/CollisionBroad.wgsl +++ b/physics/shaders/CollisionBroad.wgsl @@ -353,6 +353,7 @@ struct PhysicsParams { Dt: f32, SubSteps: i32, ControlDt: f32, + ControlDtThr: f32, ContactMargin: f32, ContactRelax: f32, // 0.8 def ContactWeighting: i32, // true @@ -377,6 +378,9 @@ struct PhysicsParams { JointDoFsN: i32, BodyJointsMax: i32, BodyCollidePairsN: i32, + pad: i32, + pad1: i32, + pad2: i32, Gravity: vec4, } diff --git a/physics/shaders/CollisionNarrow.wgsl b/physics/shaders/CollisionNarrow.wgsl index 08c1815d..c66a7b30 100644 --- a/physics/shaders/CollisionNarrow.wgsl +++ b/physics/shaders/CollisionNarrow.wgsl @@ -442,6 +442,7 @@ struct PhysicsParams { Dt: f32, SubSteps: i32, ControlDt: f32, + ControlDtThr: f32, ContactMargin: f32, ContactRelax: f32, // 0.8 def ContactWeighting: i32, // true @@ -466,6 +467,9 @@ struct PhysicsParams { JointDoFsN: i32, BodyJointsMax: i32, BodyCollidePairsN: i32, + pad: i32, + pad1: i32, + pad2: i32, Gravity: vec4, } diff --git a/physics/shaders/DynamicsCurToNext.wgsl b/physics/shaders/DynamicsCurToNext.wgsl index 4698d13e..2fe0ec37 100644 --- a/physics/shaders/DynamicsCurToNext.wgsl +++ b/physics/shaders/DynamicsCurToNext.wgsl @@ -236,6 +236,7 @@ struct PhysicsParams { Dt: f32, SubSteps: i32, ControlDt: f32, + ControlDtThr: f32, ContactMargin: f32, ContactRelax: f32, // 0.8 def ContactWeighting: i32, // true @@ -260,6 +261,9 @@ struct PhysicsParams { JointDoFsN: i32, BodyJointsMax: i32, BodyCollidePairsN: i32, + pad: i32, + pad1: i32, + pad2: i32, Gravity: vec4, } diff --git a/physics/shaders/ForcesFromJoints.wgsl b/physics/shaders/ForcesFromJoints.wgsl index a89ea6e1..e18d444b 100644 --- a/physics/shaders/ForcesFromJoints.wgsl +++ b/physics/shaders/ForcesFromJoints.wgsl @@ -266,6 +266,7 @@ struct PhysicsParams { Dt: f32, SubSteps: i32, ControlDt: f32, + ControlDtThr: f32, ContactMargin: f32, ContactRelax: f32, // 0.8 def ContactWeighting: i32, // true @@ -290,6 +291,9 @@ struct PhysicsParams { JointDoFsN: i32, BodyJointsMax: i32, BodyCollidePairsN: i32, + pad: i32, + pad1: i32, + pad2: i32, Gravity: vec4, } diff --git a/physics/shaders/InitDynamics.wgsl b/physics/shaders/InitDynamics.wgsl index dfe34352..684cb70c 100644 --- a/physics/shaders/InitDynamics.wgsl +++ b/physics/shaders/InitDynamics.wgsl @@ -243,6 +243,7 @@ struct PhysicsParams { Dt: f32, SubSteps: i32, ControlDt: f32, + ControlDtThr: f32, ContactMargin: f32, ContactRelax: f32, // 0.8 def ContactWeighting: i32, // true @@ -267,6 +268,9 @@ struct PhysicsParams { JointDoFsN: i32, BodyJointsMax: i32, BodyCollidePairsN: i32, + pad: i32, + pad1: i32, + pad2: i32, Gravity: vec4, } diff --git a/physics/shaders/StepBodyContactDeltas.wgsl b/physics/shaders/StepBodyContactDeltas.wgsl index dfa86b26..a17eca7b 100644 --- a/physics/shaders/StepBodyContactDeltas.wgsl +++ b/physics/shaders/StepBodyContactDeltas.wgsl @@ -339,6 +339,7 @@ struct PhysicsParams { Dt: f32, SubSteps: i32, ControlDt: f32, + ControlDtThr: f32, ContactMargin: f32, ContactRelax: f32, // 0.8 def ContactWeighting: i32, // true @@ -363,6 +364,9 @@ struct PhysicsParams { JointDoFsN: i32, BodyJointsMax: i32, BodyCollidePairsN: i32, + pad: i32, + pad1: i32, + pad2: i32, Gravity: vec4, } diff --git a/physics/shaders/StepBodyContacts.wgsl b/physics/shaders/StepBodyContacts.wgsl index 54e963c1..0f51f56a 100644 --- a/physics/shaders/StepBodyContacts.wgsl +++ b/physics/shaders/StepBodyContacts.wgsl @@ -468,6 +468,7 @@ struct PhysicsParams { Dt: f32, SubSteps: i32, ControlDt: f32, + ControlDtThr: f32, ContactMargin: f32, ContactRelax: f32, // 0.8 def ContactWeighting: i32, // true @@ -492,6 +493,9 @@ struct PhysicsParams { JointDoFsN: i32, BodyJointsMax: i32, BodyCollidePairsN: i32, + pad: i32, + pad1: i32, + pad2: i32, Gravity: vec4, } diff --git a/physics/shaders/StepIntegrateBodies.wgsl b/physics/shaders/StepIntegrateBodies.wgsl index de5411d7..3c9200de 100644 --- a/physics/shaders/StepIntegrateBodies.wgsl +++ b/physics/shaders/StepIntegrateBodies.wgsl @@ -266,6 +266,7 @@ struct PhysicsParams { Dt: f32, SubSteps: i32, ControlDt: f32, + ControlDtThr: f32, ContactMargin: f32, ContactRelax: f32, // 0.8 def ContactWeighting: i32, // true @@ -290,6 +291,9 @@ struct PhysicsParams { JointDoFsN: i32, BodyJointsMax: i32, BodyCollidePairsN: i32, + pad: i32, + pad1: i32, + pad2: i32, Gravity: vec4, } diff --git a/physics/shaders/StepJointForces.wgsl b/physics/shaders/StepJointForces.wgsl index 3352c887..94a5944d 100644 --- a/physics/shaders/StepJointForces.wgsl +++ b/physics/shaders/StepJointForces.wgsl @@ -311,6 +311,7 @@ struct PhysicsParams { Dt: f32, SubSteps: i32, ControlDt: f32, + ControlDtThr: f32, ContactMargin: f32, ContactRelax: f32, // 0.8 def ContactWeighting: i32, // true @@ -335,6 +336,9 @@ struct PhysicsParams { JointDoFsN: i32, BodyJointsMax: i32, BodyCollidePairsN: i32, + pad: i32, + pad1: i32, + pad2: i32, Gravity: vec4, } diff --git a/physics/shaders/StepSolveJoints.wgsl b/physics/shaders/StepSolveJoints.wgsl index 032b6dcd..800a5b18 100644 --- a/physics/shaders/StepSolveJoints.wgsl +++ b/physics/shaders/StepSolveJoints.wgsl @@ -350,6 +350,7 @@ struct PhysicsParams { Dt: f32, SubSteps: i32, ControlDt: f32, + ControlDtThr: f32, ContactMargin: f32, ContactRelax: f32, // 0.8 def ContactWeighting: i32, // true @@ -374,6 +375,9 @@ struct PhysicsParams { JointDoFsN: i32, BodyJointsMax: i32, BodyCollidePairsN: i32, + pad: i32, + pad1: i32, + pad2: i32, Gravity: vec4, } diff --git a/physics/step.go b/physics/step.go index 1edf59cf..504064ac 100644 --- a/physics/step.go +++ b/physics/step.go @@ -9,6 +9,8 @@ package physics +import "cogentcore.org/core/math32" + //gosl:start //gosl:import "cogentcore.org/lab/gosl/slmath" @@ -37,7 +39,11 @@ func StepInit(i uint32) { //gosl:kernel read-write:Params for j := range params.JointDoFsN { tpos := JointControls.Value(int(j), int(JointTargetPos)) tcur := JointControls.Value(int(j), int(JointTargetPosCur)) - tcur += params.ControlDt * (tpos - tcur) + if math32.Abs(tpos-tcur) < params.ControlDtThr { + tcur = tpos + } else { + tcur += params.ControlDt * (tpos - tcur) + } JointControls.Set(tcur, int(j), int(JointTargetPosCur)) } } diff --git a/physics/step.goal b/physics/step.goal index 6bffcfd1..1878cb7e 100644 --- a/physics/step.goal +++ b/physics/step.goal @@ -7,6 +7,8 @@ package physics +import "cogentcore.org/core/math32" + //gosl:start //gosl:import "cogentcore.org/lab/gosl/slmath" @@ -35,7 +37,11 @@ func StepInit(i uint32) { //gosl:kernel read-write:Params for j := range params.JointDoFsN { tpos := JointControls[j, JointTargetPos] tcur := JointControls[j, JointTargetPosCur] - tcur += params.ControlDt * (tpos - tcur) + if math32.Abs(tpos-tcur) < params.ControlDtThr { + tcur = tpos + } else { + tcur += params.ControlDt * (tpos - tcur) + } JointControls[j, JointTargetPosCur] = tcur } } diff --git a/physics/typegen.go b/physics/typegen.go index bf76df56..fc93f6d0 100644 --- a/physics/typegen.go +++ b/physics/typegen.go @@ -24,7 +24,7 @@ var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.JointDoFVars var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.Model", IDName: "model", Doc: "Model contains and manages all of the physics elements.", Directives: []types.Directive{{Tool: "go", Directive: "generate", Args: []string{"core", "generate", "-add-types", "-gosl"}}}, Fields: []types.Field{{Name: "GPU", Doc: "GPU determines whether to use GPU (else CPU)."}, {Name: "Params", Doc: "Params are global parameters."}, {Name: "CurrentWorld", Doc: "CurrentWorld is the [BodyWorld] value to use when creating new bodies.\nSet to -1 to create global elements that interact with everything,\nwhile 0 and positive numbers only interact amongst themselves."}, {Name: "CurrentObject", Doc: "CurrentObject is the Object to use when creating new joints.\nCall NewObject to increment."}, {Name: "CurrentObjectJoint", Doc: "CurrentObjectJoint is the Joint index in CurrentObject\nto use when creating new joints."}, {Name: "ReplicasStart", Doc: "ReplicasStart is the starting body index for replicated world bodies,\nwhich is needed for viewers to efficiently select a specific world to view.\nThis is the start of the World=0 first instance."}, {Name: "ReplicasN", Doc: "ReplicasN is the number of body elements within each set of\nreplicated world bodies, which is needed for viewers to efficiently select\na specific world to view."}, {Name: "Bodies", Doc: "Bodies are the rigid body elements (dynamic and static),\nspecifying the constant, non-dynamic properties,\nwhich is initial state for dynamics.\n[body][BodyVarsN]"}, {Name: "Objects", Doc: "Objects is a list of joint indexes for each object, where each object\ncontains all the joints interconnecting an overlapping set of bodies.\nThis is known as an articulation in other physics software.\nJoints must be added in parent -> child order within objects, as joints\nare updated in sequential order within object.\n[object][MaxObjectJoints+1]"}, {Name: "BodyJoints", Doc: "BodyJoints is a list of joint indexes for each dynamic body, for aggregating.\n[dyn body][parent, child][Params.BodyJointsMax]"}, {Name: "Joints", Doc: "Joints is a list of permanent joints connecting bodies,\nwhich do not change (no dynamic variables).\n[joint][JointVarsN]"}, {Name: "JointDoFs", Doc: "JointDoFs is a list of joint DoF parameters, allocated per joint.\n[dof][JointDoFVars]"}, {Name: "BodyCollidePairs", Doc: "BodyCollidePairs are pairs of Body indexes that could potentially collide\nbased on precomputed collision logic, using World, Group, and Joint indexes.\n[BodyCollidePairsN][2]"}, {Name: "Dynamics", Doc: "Dynamics are the dynamic rigid body elements: these actually move.\nThe first set of variables are for initial values, and the second current.\n[body][cur/next][DynamicVarsN]"}, {Name: "BroadContactsN", Doc: "BroadContactsN has number of points of broad contact\nbetween bodies. [1]"}, {Name: "BroadContacts", Doc: "BroadContacts are the results of broad-phase contact processing,\nestablishing possible points of contact between bodies.\n[ContactsMax][BroadContactVarsN]"}, {Name: "ContactsN", Doc: "ContactsN has number of points of narrow (final) contact\nbetween bodies. [1]"}, {Name: "Contacts", Doc: "Contacts are the results of narrow-phase contact processing,\nwhere only actual contacts with fully-specified values are present.\n[ContactsMax][ContactVarsN]"}, {Name: "JointControls", Doc: "JointControls are dynamic joint control inputs, per joint DoF\n(in correspondence with [JointDoFs]). This can be uploaded to the\nGPU at every step.\n[dof][JointControlVarsN]"}}}) -var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.PhysicsParams", IDName: "physics-params", Doc: "PhysicsParams are the physics parameters", Directives: []types.Directive{{Tool: "gosl", Directive: "start"}}, Fields: []types.Field{{Name: "Iterations", Doc: "Iterations is the number of integration iterations to perform\nwithin each solver step. Muller et al (2020) report that 1 is best."}, {Name: "Dt", Doc: "Dt is the integration stepsize.\nFor highly kinetic situations (e.g., rapidly moving bouncing balls)\n0.0001 is needed to ensure contact registration. Use SubSteps to\naccomplish a target effective read-out step size."}, {Name: "SubSteps", Doc: "SubSteps is the number of integration steps to take per Step()\nfunction call. These sub steps are taken without any sync to/from\nthe GPU and are therefore much faster."}, {Name: "ControlDt", Doc: "ControlDt is the stepsize for integrating joint control position values\n[JointTargetPos] over time, to avoid sudden strong changes in force."}, {Name: "ContactMargin", Doc: "Contact margin is the extra distance for broadphase collision\naround rigid bodies. This can make some joints potentially unstable if > 0"}, {Name: "ContactRelax", Doc: "ContactRelax is rigid contact relaxation constant.\nHigher values cause errros"}, {Name: "ContactWeighting", Doc: "Contact weighting: balances contact forces?"}, {Name: "Restitution", Doc: "Restitution takes into account bounciness of objects."}, {Name: "JointLinearRelax", Doc: "JointLinearRelax is joint linear relaxation constant."}, {Name: "JointAngularRelax", Doc: "JointAngularRelax is joint angular relaxation constant."}, {Name: "JointLinearComply", Doc: "JointLinearComply is joint linear compliance constant."}, {Name: "JointAngularComply", Doc: "JointAngularComply is joint angular compliance constant."}, {Name: "AngularDamping", Doc: "AngularDamping is damping of angular motion."}, {Name: "SoftRelax", Doc: "SoftRelax is soft-body relaxation constant."}, {Name: "MaxForce", Doc: "MaxForce is the maximum computed force value, which prevents\nrunaway numerical overflow."}, {Name: "MaxDelta", Doc: "MaxDelta is the maximum computed change in position magnitude,\nwhich prevents runaway numerical overflow."}, {Name: "MaxGeomIter", Doc: "MaxGeomIter is number of iterations to perform in shape-based\ngeometry collision computations"}, {Name: "ContactsMax", Doc: "Maximum number of contacts to process at any given point."}, {Name: "Cur", Doc: "Index for the current state (0 or 1, alternates with Next)."}, {Name: "Next", Doc: "Index for the next state (1 or 0, alternates with Cur)."}, {Name: "BodiesN", Doc: "BodiesN is number of rigid bodies."}, {Name: "DynamicsN", Doc: "DynamicsN is number of dynamics bodies."}, {Name: "ObjectsN", Doc: "ObjectsN is number of objects."}, {Name: "MaxObjectJoints", Doc: "MaxObjectJoints is max number of joints per object."}, {Name: "JointsN", Doc: "JointsN is number of joints."}, {Name: "JointDoFsN", Doc: "JointDoFsN is number of joint DoFs."}, {Name: "BodyJointsMax", Doc: "BodyJointsMax is max number of joints per body + 1 for actual n."}, {Name: "BodyCollidePairsN", Doc: "BodyCollidePairsN is the total number of pre-compiled collision pairs\nto examine."}, {Name: "Gravity", Doc: "Gravity is the gravity acceleration function"}}}) +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.PhysicsParams", IDName: "physics-params", Doc: "PhysicsParams are the physics parameters", Directives: []types.Directive{{Tool: "gosl", Directive: "start"}}, Fields: []types.Field{{Name: "Iterations", Doc: "Iterations is the number of integration iterations to perform\nwithin each solver step. Muller et al (2020) report that 1 is best."}, {Name: "Dt", Doc: "Dt is the integration stepsize.\nFor highly kinetic situations (e.g., rapidly moving bouncing balls)\n0.0001 is needed to ensure contact registration. Use SubSteps to\naccomplish a target effective read-out step size."}, {Name: "SubSteps", Doc: "SubSteps is the number of integration steps to take per Step()\nfunction call. These sub steps are taken without any sync to/from\nthe GPU and are therefore much faster."}, {Name: "ControlDt", Doc: "ControlDt is the stepsize for integrating joint control position values\n[JointTargetPos] over time, to avoid sudden strong changes in force."}, {Name: "ControlDtThr", Doc: "ControlDtThr is the threshold on the control delta above which\nControlDt is used. ControlDt is most important for large changes,\nand can result in under-shoot if engaged for small chages."}, {Name: "ContactMargin", Doc: "Contact margin is the extra distance for broadphase collision\naround rigid bodies. This can make some joints potentially unstable if > 0"}, {Name: "ContactRelax", Doc: "ContactRelax is rigid contact relaxation constant.\nHigher values cause errros"}, {Name: "ContactWeighting", Doc: "Contact weighting: balances contact forces?"}, {Name: "Restitution", Doc: "Restitution takes into account bounciness of objects."}, {Name: "JointLinearRelax", Doc: "JointLinearRelax is joint linear relaxation constant."}, {Name: "JointAngularRelax", Doc: "JointAngularRelax is joint angular relaxation constant."}, {Name: "JointLinearComply", Doc: "JointLinearComply is joint linear compliance constant."}, {Name: "JointAngularComply", Doc: "JointAngularComply is joint angular compliance constant."}, {Name: "AngularDamping", Doc: "AngularDamping is damping of angular motion."}, {Name: "SoftRelax", Doc: "SoftRelax is soft-body relaxation constant."}, {Name: "MaxForce", Doc: "MaxForce is the maximum computed force value, which prevents\nrunaway numerical overflow."}, {Name: "MaxDelta", Doc: "MaxDelta is the maximum computed change in position magnitude,\nwhich prevents runaway numerical overflow."}, {Name: "MaxGeomIter", Doc: "MaxGeomIter is number of iterations to perform in shape-based\ngeometry collision computations"}, {Name: "ContactsMax", Doc: "Maximum number of contacts to process at any given point."}, {Name: "Cur", Doc: "Index for the current state (0 or 1, alternates with Next)."}, {Name: "Next", Doc: "Index for the next state (1 or 0, alternates with Cur)."}, {Name: "BodiesN", Doc: "BodiesN is number of rigid bodies."}, {Name: "DynamicsN", Doc: "DynamicsN is number of dynamics bodies."}, {Name: "ObjectsN", Doc: "ObjectsN is number of objects."}, {Name: "MaxObjectJoints", Doc: "MaxObjectJoints is max number of joints per object."}, {Name: "JointsN", Doc: "JointsN is number of joints."}, {Name: "JointDoFsN", Doc: "JointDoFsN is number of joint DoFs."}, {Name: "BodyJointsMax", Doc: "BodyJointsMax is max number of joints per body + 1 for actual n."}, {Name: "BodyCollidePairsN", Doc: "BodyCollidePairsN is the total number of pre-compiled collision pairs\nto examine."}, {Name: "pad"}, {Name: "pad1"}, {Name: "pad2"}, {Name: "Gravity", Doc: "Gravity is the gravity acceleration function"}}}) var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.GeomData", IDName: "geom-data", Doc: "GeomData contains all geometric data for narrow-phase collision.", Directives: []types.Directive{{Tool: "gosl", Directive: "start"}}, Fields: []types.Field{{Name: "BodyIdx"}, {Name: "Shape"}, {Name: "MinSize", Doc: "MinSize is the min of the Size dimensions."}, {Name: "Thick", Doc: "Thickness of shape."}, {Name: "Radius", Doc: "Radius is the effective radius for sphere-like elements (Sphere, Capsule, Cone)"}, {Name: "Size"}, {Name: "WbR", Doc: "World-to-Body transform\nPosition (R) (i.e., BodyPos)"}, {Name: "WbQ", Doc: "Quaternion (Q) (i.e., BodyQuat)"}, {Name: "BwR", Doc: "Body-to-World transform (inverse)\nPosition (R)"}, {Name: "BwQ", Doc: "Quaternion (Q)"}}}) diff --git a/yaegilab/labsymbols/cogentcore_org-lab-physics.go b/yaegilab/labsymbols/cogentcore_org-lab-physics.go index 77add5ea..03b53fa5 100644 --- a/yaegilab/labsymbols/cogentcore_org-lab-physics.go +++ b/yaegilab/labsymbols/cogentcore_org-lab-physics.go @@ -332,6 +332,7 @@ func init() { "JointVarsValues": reflect.ValueOf(physics.JointVarsValues), "Joints": reflect.ValueOf(&physics.Joints).Elem(), "JointsVar": reflect.ValueOf(physics.JointsVar), + "LimitDelta": reflect.ValueOf(physics.LimitDelta), "NewGeomData": reflect.ValueOf(physics.NewGeomData), "NewModel": reflect.ValueOf(physics.NewModel), "Objects": reflect.ValueOf(&physics.Objects).Elem(), @@ -471,8 +472,7 @@ func init() { "StepInit": reflect.ValueOf(physics.StepInit), "StepIntegrateBodies": reflect.ValueOf(physics.StepIntegrateBodies), "StepJointForces": reflect.ValueOf(physics.StepJointForces), - "StepSolveJointAngular": reflect.ValueOf(physics.StepSolveJointAngular), - "StepSolveJointLinear": reflect.ValueOf(physics.StepSolveJointLinear), + "StepSolveJoint": reflect.ValueOf(physics.StepSolveJoint), "StepSolveJoints": reflect.ValueOf(physics.StepSolveJoints), "StepsToMsec": reflect.ValueOf(physics.StepsToMsec), "SyncFromGPU": reflect.ValueOf(physics.SyncFromGPU), From 061a0c203753d1a12d96f066212308e70945a4be Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Sun, 4 Jan 2026 11:13:17 -0800 Subject: [PATCH 76/97] physics: JointNoLinearRotation flag fixes PlaneXZ completely -- fully 100% functional. Now about that neck joint.. --- physics/enumgen.go | 10 +- physics/examples/virtroom/virtroom.go | 4 +- physics/joint.go | 28 ++++-- physics/joint.goal | 30 ++++-- physics/shaders/CollisionBroad.wgsl | 87 ++++++++-------- physics/shaders/CollisionNarrow.wgsl | 87 ++++++++-------- physics/shaders/DynamicsCurToNext.wgsl | 87 ++++++++-------- physics/shaders/ForcesFromJoints.wgsl | 87 ++++++++-------- physics/shaders/InitDynamics.wgsl | 87 ++++++++-------- physics/shaders/StepBodyContactDeltas.wgsl | 87 ++++++++-------- physics/shaders/StepBodyContacts.wgsl | 87 ++++++++-------- physics/shaders/StepIntegrateBodies.wgsl | 87 ++++++++-------- physics/shaders/StepJointForces.wgsl | 87 ++++++++-------- physics/shaders/StepSolveJoints.wgsl | 109 ++++++++++++--------- physics/step_joint.go | 26 ++--- physics/step_joint.goal | 26 ++--- physics/typegen.go | 2 +- 17 files changed, 538 insertions(+), 480 deletions(-) diff --git a/physics/enumgen.go b/physics/enumgen.go index 49908db4..052368b1 100644 --- a/physics/enumgen.go +++ b/physics/enumgen.go @@ -284,20 +284,20 @@ func (i *JointTypes) UnmarshalText(text []byte) error { return enums.UnmarshalText(i, text, "JointTypes") } -var _JointVarsValues = []JointVars{0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44} +var _JointVarsValues = []JointVars{0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45} // JointVarsN is the highest valid value for type JointVars, plus one. // //gosl:start -const JointVarsN JointVars = 45 +const JointVarsN JointVars = 46 //gosl:end -var _JointVarsValueMap = map[string]JointVars{`JointType`: 0, `JointEnabled`: 1, `JointParentFixed`: 2, `JointParent`: 3, `JointChild`: 4, `JointPPosX`: 5, `JointPPosY`: 6, `JointPPosZ`: 7, `JointPQuatX`: 8, `JointPQuatY`: 9, `JointPQuatZ`: 10, `JointPQuatW`: 11, `JointCPosX`: 12, `JointCPosY`: 13, `JointCPosZ`: 14, `JointCQuatX`: 15, `JointCQuatY`: 16, `JointCQuatZ`: 17, `JointCQuatW`: 18, `JointLinearDoFN`: 19, `JointAngularDoFN`: 20, `JointDoF1`: 21, `JointDoF2`: 22, `JointDoF3`: 23, `JointDoF4`: 24, `JointDoF5`: 25, `JointDoF6`: 26, `JointPForceX`: 27, `JointPForceY`: 28, `JointPForceZ`: 29, `JointPTorqueX`: 30, `JointPTorqueY`: 31, `JointPTorqueZ`: 32, `JointCForceX`: 33, `JointCForceY`: 34, `JointCForceZ`: 35, `JointCTorqueX`: 36, `JointCTorqueY`: 37, `JointCTorqueZ`: 38, `JointLinLambdaX`: 39, `JointLinLambdaY`: 40, `JointLinLambdaZ`: 41, `JointAngLambdaX`: 42, `JointAngLambdaY`: 43, `JointAngLambdaZ`: 44} +var _JointVarsValueMap = map[string]JointVars{`JointType`: 0, `JointEnabled`: 1, `JointParentFixed`: 2, `JointNoLinearRotation`: 3, `JointParent`: 4, `JointChild`: 5, `JointPPosX`: 6, `JointPPosY`: 7, `JointPPosZ`: 8, `JointPQuatX`: 9, `JointPQuatY`: 10, `JointPQuatZ`: 11, `JointPQuatW`: 12, `JointCPosX`: 13, `JointCPosY`: 14, `JointCPosZ`: 15, `JointCQuatX`: 16, `JointCQuatY`: 17, `JointCQuatZ`: 18, `JointCQuatW`: 19, `JointLinearDoFN`: 20, `JointAngularDoFN`: 21, `JointDoF1`: 22, `JointDoF2`: 23, `JointDoF3`: 24, `JointDoF4`: 25, `JointDoF5`: 26, `JointDoF6`: 27, `JointPForceX`: 28, `JointPForceY`: 29, `JointPForceZ`: 30, `JointPTorqueX`: 31, `JointPTorqueY`: 32, `JointPTorqueZ`: 33, `JointCForceX`: 34, `JointCForceY`: 35, `JointCForceZ`: 36, `JointCTorqueX`: 37, `JointCTorqueY`: 38, `JointCTorqueZ`: 39, `JointLinLambdaX`: 40, `JointLinLambdaY`: 41, `JointLinLambdaZ`: 42, `JointAngLambdaX`: 43, `JointAngLambdaY`: 44, `JointAngLambdaZ`: 45} -var _JointVarsDescMap = map[JointVars]string{0: `JointType (as an int32 from bits).`, 1: `JointEnabled allows joints to be dynamically enabled.`, 2: `JointParentFixed means that the parent is NOT updated based on the forces and positions for this joint. This can make dynamics cleaner when full accuracy is not necessary.`, 3: `JointParent is the dynamic body index for parent body. Can be -1 for a fixed parent for absolute anchor.`, 4: `JointChild is the dynamic body index for child body.`, 5: `relative position of joint, in parent frame. This is prior to parent body rotation.`, 6: ``, 7: ``, 8: `relative orientation of joint, in parent frame. This is prior to parent body rotation.`, 9: ``, 10: ``, 11: ``, 12: `relative position of joint, in child frame. This is prior to child body rotation.`, 13: ``, 14: ``, 15: `relative orientation of joint, in child frame. This is prior to parent body rotation.`, 16: ``, 17: ``, 18: ``, 19: `JointLinearDoFN is the number of linear degrees-of-freedom for the joint.`, 20: `JointAngularDoFN is the number of angular degrees-of-freedom for the joint.`, 21: `indexes in JointDoFs for each DoF`, 22: ``, 23: ``, 24: `angular starts here for Free, Distance, D6`, 25: ``, 26: ``, 27: `Computed parent joint force value.`, 28: ``, 29: ``, 30: `Computed parent joint torque value.`, 31: ``, 32: ``, 33: `Computed child joint force value.`, 34: ``, 35: ``, 36: `Computed child joint torque value.`, 37: ``, 38: ``, 39: `Computed linear lambdas.`, 40: ``, 41: ``, 42: `Computed angular lambdas.`, 43: ``, 44: ``} +var _JointVarsDescMap = map[JointVars]string{0: `JointType (as an int32 from bits).`, 1: `JointEnabled allows joints to be dynamically enabled.`, 2: `JointParentFixed means that the parent is NOT updated based on the forces and positions for this joint. This can make dynamics cleaner when full accuracy is not necessary.`, 3: `JointNoLinearRotation ignores the rotational (angular) effects of linear joint position constraints (i.e., Coriolis and centrifugal forces) which can otherwise interfere with rotational position constraints in joints with both linear and angular DoFs (e.g., [PlaneXZ], for which this is on by default).`, 4: `JointParent is the dynamic body index for parent body. Can be -1 for a fixed parent for absolute anchor.`, 5: `JointChild is the dynamic body index for child body.`, 6: `relative position of joint, in parent frame. This is prior to parent body rotation.`, 7: ``, 8: ``, 9: `relative orientation of joint, in parent frame. This is prior to parent body rotation.`, 10: ``, 11: ``, 12: ``, 13: `relative position of joint, in child frame. This is prior to child body rotation.`, 14: ``, 15: ``, 16: `relative orientation of joint, in child frame. This is prior to parent body rotation.`, 17: ``, 18: ``, 19: ``, 20: `JointLinearDoFN is the number of linear degrees-of-freedom for the joint.`, 21: `JointAngularDoFN is the number of angular degrees-of-freedom for the joint.`, 22: `indexes in JointDoFs for each DoF`, 23: ``, 24: ``, 25: `angular starts here for Free, Distance, D6`, 26: ``, 27: ``, 28: `Computed parent joint force value.`, 29: ``, 30: ``, 31: `Computed parent joint torque value.`, 32: ``, 33: ``, 34: `Computed child joint force value.`, 35: ``, 36: ``, 37: `Computed child joint torque value.`, 38: ``, 39: ``, 40: `Computed linear lambdas.`, 41: ``, 42: ``, 43: `Computed angular lambdas.`, 44: ``, 45: ``} -var _JointVarsMap = map[JointVars]string{0: `JointType`, 1: `JointEnabled`, 2: `JointParentFixed`, 3: `JointParent`, 4: `JointChild`, 5: `JointPPosX`, 6: `JointPPosY`, 7: `JointPPosZ`, 8: `JointPQuatX`, 9: `JointPQuatY`, 10: `JointPQuatZ`, 11: `JointPQuatW`, 12: `JointCPosX`, 13: `JointCPosY`, 14: `JointCPosZ`, 15: `JointCQuatX`, 16: `JointCQuatY`, 17: `JointCQuatZ`, 18: `JointCQuatW`, 19: `JointLinearDoFN`, 20: `JointAngularDoFN`, 21: `JointDoF1`, 22: `JointDoF2`, 23: `JointDoF3`, 24: `JointDoF4`, 25: `JointDoF5`, 26: `JointDoF6`, 27: `JointPForceX`, 28: `JointPForceY`, 29: `JointPForceZ`, 30: `JointPTorqueX`, 31: `JointPTorqueY`, 32: `JointPTorqueZ`, 33: `JointCForceX`, 34: `JointCForceY`, 35: `JointCForceZ`, 36: `JointCTorqueX`, 37: `JointCTorqueY`, 38: `JointCTorqueZ`, 39: `JointLinLambdaX`, 40: `JointLinLambdaY`, 41: `JointLinLambdaZ`, 42: `JointAngLambdaX`, 43: `JointAngLambdaY`, 44: `JointAngLambdaZ`} +var _JointVarsMap = map[JointVars]string{0: `JointType`, 1: `JointEnabled`, 2: `JointParentFixed`, 3: `JointNoLinearRotation`, 4: `JointParent`, 5: `JointChild`, 6: `JointPPosX`, 7: `JointPPosY`, 8: `JointPPosZ`, 9: `JointPQuatX`, 10: `JointPQuatY`, 11: `JointPQuatZ`, 12: `JointPQuatW`, 13: `JointCPosX`, 14: `JointCPosY`, 15: `JointCPosZ`, 16: `JointCQuatX`, 17: `JointCQuatY`, 18: `JointCQuatZ`, 19: `JointCQuatW`, 20: `JointLinearDoFN`, 21: `JointAngularDoFN`, 22: `JointDoF1`, 23: `JointDoF2`, 24: `JointDoF3`, 25: `JointDoF4`, 26: `JointDoF5`, 27: `JointDoF6`, 28: `JointPForceX`, 29: `JointPForceY`, 30: `JointPForceZ`, 31: `JointPTorqueX`, 32: `JointPTorqueY`, 33: `JointPTorqueZ`, 34: `JointCForceX`, 35: `JointCForceY`, 36: `JointCForceZ`, 37: `JointCTorqueX`, 38: `JointCTorqueY`, 39: `JointCTorqueZ`, 40: `JointLinLambdaX`, 41: `JointLinLambdaY`, 42: `JointLinLambdaZ`, 43: `JointAngLambdaX`, 44: `JointAngLambdaY`, 45: `JointAngLambdaZ`} // String returns the string representation of this JointVars value. func (i JointVars) String() string { return enums.String(i, _JointVarsMap) } diff --git a/physics/examples/virtroom/virtroom.go b/physics/examples/virtroom/virtroom.go index 5ca770af..4ffd2bc9 100644 --- a/physics/examples/virtroom/virtroom.go +++ b/physics/examples/virtroom/virtroom.go @@ -156,8 +156,8 @@ func (ev *Env) MakeWorld(sc *xyz.Scene) { ev.MakeEmer(ew, &ev.Emer, "emer") // ev.Physics.Builder.ReplicateWorld(1, 8, 2) ev.Physics.Build() - params := physics.GetParams(0) - params.ControlDt = 1 + // params := physics.GetParams(0) + // params.ControlDt = 1 // params.Gravity.Y = 0 // params.MaxForce = 1.0e3 // params.AngularDamping = 0.5 diff --git a/physics/joint.go b/physics/joint.go index 4fc03c00..ce9d25dc 100644 --- a/physics/joint.go +++ b/physics/joint.go @@ -65,6 +65,13 @@ const ( // cleaner when full accuracy is not necessary. JointParentFixed + // JointNoLinearRotation ignores the rotational (angular) effects of + // linear joint position constraints (i.e., Coriolis and centrifugal forces) + // which can otherwise interfere with rotational position constraints in + // joints with both linear and angular DoFs + // (e.g., [PlaneXZ], for which this is on by default). + JointNoLinearRotation + // JointParent is the dynamic body index for parent body. // Can be -1 for a fixed parent for absolute anchor. JointParent @@ -179,6 +186,19 @@ func SetJointParentFixed(idx int32, enabled bool) { Joints.Set(math.Float32frombits(je), int(idx), int(JointParentFixed)) } +func GetJointNoLinearRotation(idx int32) bool { + je := math.Float32bits(Joints.Value(int(idx), int(JointNoLinearRotation))) + return je != 0 +} + +func SetJointNoLinearRotation(idx int32, enabled bool) { + je := uint32(0) + if enabled { + je = 1 + } + Joints.Set(math.Float32frombits(je), int(idx), int(JointNoLinearRotation)) +} + func SetJointParent(idx, bodyIdx int32) { Joints.Set(math.Float32frombits(uint32(bodyIdx)), int(idx), int(JointParent)) } @@ -392,7 +412,6 @@ func (ml *Model) NewJointFixed(parent, child int32, ppos, cpos math32.Vector3) i // ppos, cpos are the relative positions from the parent, child. // Sets relative rotation matricies to identity by default. // axis is the axis of articulation for the joint. -// Use [SetJointDoF] to set the remaining DoF parameters. func (ml *Model) NewJointPrismatic(parent, child int32, ppos, cpos, axis math32.Vector3) int32 { idx := ml.newJoint(Prismatic, parent, child, ppos, cpos) SetJointLinearDoFN(idx, 1) @@ -406,7 +425,6 @@ func (ml *Model) NewJointPrismatic(parent, child int32, ppos, cpos, axis math32. // ppos, cpos are the relative positions from the parent, child. // Sets relative rotation matricies to identity by default. // axis is the axis of articulation for the joint. -// Use [SetJointDoF] to set the remaining DoF parameters. func (ml *Model) NewJointRevolute(parent, child int32, ppos, cpos, axis math32.Vector3) int32 { idx := ml.newJoint(Revolute, parent, child, ppos, cpos) SetJointAngularDoFN(idx, 1) @@ -419,7 +437,6 @@ func (ml *Model) NewJointRevolute(parent, child int32, ppos, cpos, axis math32.V // Use -1 for parent to add a world-anchored joint. // ppos, cpos are the relative positions from the parent, child. // Sets relative rotation matricies to identity by default. -// Use [SetJointDoF] to set the remaining DoF parameters. func (ml *Model) NewJointBall(parent, child int32, ppos, cpos math32.Vector3) int32 { idx := ml.newJoint(Ball, parent, child, ppos, cpos) SetJointAngularDoFN(idx, 3) @@ -434,16 +451,16 @@ func (ml *Model) NewJointBall(parent, child int32, ppos, cpos math32.Vector3) in // NewJointPlaneXZ adds a new 3 DoF Planar motion joint suitable for // controlling the motion of a body on the standard X-Z plane (Y = up). // The two linear DoF control position in X, Z, and 3rd angular -// controls rotation in Y axis. +// controls rotation in Y axis. Sets [JointNoLinearRotation] // Use -1 for parent to add a world-anchored joint (typical). // ppos, cpos are the relative positions from the parent, child. // Sets relative rotation matricies to identity by default. -// Use [SetJointDoF] to set the remaining DoF parameters. func (ml *Model) NewJointPlaneXZ(parent, child int32, ppos, cpos math32.Vector3) int32 { idx := ml.NewJointD6(parent, child, ppos, cpos, 2, 1) ml.newJointDoF(idx, 0, math32.Vec3(1, 0, 0)) ml.newJointDoF(idx, 1, math32.Vec3(0, 0, 1)) ml.newJointDoF(idx, 2, math32.Vec3(0, 1, 0)) + SetJointNoLinearRotation(idx, true) return idx } @@ -453,7 +470,6 @@ func (ml *Model) NewJointPlaneXZ(parent, child int32, ppos, cpos math32.Vector3) // Use -1 for parent to add a world-anchored joint. // ppos, cpos are the relative positions from the parent, child. // Sets relative rotation matricies to identity by default. -// Use [SetJointDoF] to set the remaining DoF parameters. func (ml *Model) NewJointDistance(parent, child int32, ppos, cpos math32.Vector3, minDist, maxDist float32) int32 { idx := ml.newJoint(Distance, parent, child, ppos, cpos) SetJointLinearDoFN(idx, 3) diff --git a/physics/joint.goal b/physics/joint.goal index bef0aa28..acfda4d0 100644 --- a/physics/joint.goal +++ b/physics/joint.goal @@ -63,6 +63,13 @@ const ( // cleaner when full accuracy is not necessary. JointParentFixed + // JointNoLinearRotation ignores the rotational (angular) effects of + // linear joint position constraints (i.e., Coriolis and centrifugal forces) + // which can otherwise interfere with rotational position constraints in + // joints with both linear and angular DoFs + // (e.g., [PlaneXZ], for which this is on by default). + JointNoLinearRotation + // JointParent is the dynamic body index for parent body. // Can be -1 for a fixed parent for absolute anchor. JointParent @@ -177,6 +184,19 @@ func SetJointParentFixed(idx int32, enabled bool) { Joints[idx, JointParentFixed] = math.Float32frombits(je) } +func GetJointNoLinearRotation(idx int32) bool { + je := math.Float32bits(Joints[idx, JointNoLinearRotation]) + return je != 0 +} + +func SetJointNoLinearRotation(idx int32, enabled bool) { + je := uint32(0) + if enabled { + je = 1 + } + Joints[idx, JointNoLinearRotation] = math.Float32frombits(je) +} + func SetJointParent(idx, bodyIdx int32) { Joints[idx, JointParent] = math.Float32frombits(uint32(bodyIdx)) } @@ -360,8 +380,6 @@ func SetJointDoF(idx, dof int32, vr JointDoFVars, value float32) { JointDoFs[JointDoFIndex(idx, dof), vr] = value } - - //gosl:end func (ml *Model) JointDefaults(idx int32) { @@ -392,7 +410,6 @@ func (ml *Model) NewJointFixed(parent, child int32, ppos, cpos math32.Vector3) i // ppos, cpos are the relative positions from the parent, child. // Sets relative rotation matricies to identity by default. // axis is the axis of articulation for the joint. -// Use [SetJointDoF] to set the remaining DoF parameters. func (ml *Model) NewJointPrismatic(parent, child int32, ppos, cpos, axis math32.Vector3) int32 { idx := ml.newJoint(Prismatic, parent, child, ppos, cpos) SetJointLinearDoFN(idx, 1) @@ -406,7 +423,6 @@ func (ml *Model) NewJointPrismatic(parent, child int32, ppos, cpos, axis math32. // ppos, cpos are the relative positions from the parent, child. // Sets relative rotation matricies to identity by default. // axis is the axis of articulation for the joint. -// Use [SetJointDoF] to set the remaining DoF parameters. func (ml *Model) NewJointRevolute(parent, child int32, ppos, cpos, axis math32.Vector3) int32 { idx := ml.newJoint(Revolute, parent, child, ppos, cpos) SetJointAngularDoFN(idx, 1) @@ -419,7 +435,6 @@ func (ml *Model) NewJointRevolute(parent, child int32, ppos, cpos, axis math32.V // Use -1 for parent to add a world-anchored joint. // ppos, cpos are the relative positions from the parent, child. // Sets relative rotation matricies to identity by default. -// Use [SetJointDoF] to set the remaining DoF parameters. func (ml *Model) NewJointBall(parent, child int32, ppos, cpos math32.Vector3) int32 { idx := ml.newJoint(Ball, parent, child, ppos, cpos) SetJointAngularDoFN(idx, 3) @@ -434,16 +449,16 @@ func (ml *Model) NewJointBall(parent, child int32, ppos, cpos math32.Vector3) in // NewJointPlaneXZ adds a new 3 DoF Planar motion joint suitable for // controlling the motion of a body on the standard X-Z plane (Y = up). // The two linear DoF control position in X, Z, and 3rd angular -// controls rotation in Y axis. +// controls rotation in Y axis. Sets [JointNoLinearRotation] // Use -1 for parent to add a world-anchored joint (typical). // ppos, cpos are the relative positions from the parent, child. // Sets relative rotation matricies to identity by default. -// Use [SetJointDoF] to set the remaining DoF parameters. func (ml *Model) NewJointPlaneXZ(parent, child int32, ppos, cpos math32.Vector3) int32 { idx := ml.NewJointD6(parent, child, ppos, cpos, 2, 1) ml.newJointDoF(idx, 0, math32.Vec3(1,0,0)) ml.newJointDoF(idx, 1, math32.Vec3(0,0,1)) ml.newJointDoF(idx, 2, math32.Vec3(0,1,0)) + SetJointNoLinearRotation(idx, true) return idx } @@ -453,7 +468,6 @@ func (ml *Model) NewJointPlaneXZ(parent, child int32, ppos, cpos math32.Vector3) // Use -1 for parent to add a world-anchored joint. // ppos, cpos are the relative positions from the parent, child. // Sets relative rotation matricies to identity by default. -// Use [SetJointDoF] to set the remaining DoF parameters. func (ml *Model) NewJointDistance(parent, child int32, ppos, cpos math32.Vector3, minDist, maxDist float32) int32 { idx := ml.newJoint(Distance, parent, child, ppos, cpos) SetJointLinearDoFN(idx, 3) diff --git a/physics/shaders/CollisionBroad.wgsl b/physics/shaders/CollisionBroad.wgsl index 2337a6b0..89423ea9 100644 --- a/physics/shaders/CollisionBroad.wgsl +++ b/physics/shaders/CollisionBroad.wgsl @@ -279,7 +279,7 @@ const JointControlVarsN: JointControlVars = 6; const DynamicVarsN: DynamicVars = 33; const GPUVarsN: GPUVars = 13; const JointTypesN: JointTypes = 8; -const JointVarsN: JointVars = 45; +const JointVarsN: JointVars = 46; const JointDoFVarsN: JointDoFVars = 5; const ShapesN: Shapes = 6; @@ -298,48 +298,49 @@ alias JointVars = i32; //enums:enum const JointType: JointVars = 0; const JointEnabled: JointVars = 1; const JointParentFixed: JointVars = 2; -const JointParent: JointVars = 3; -const JointChild: JointVars = 4; -const JointPPosX: JointVars = 5; -const JointPPosY: JointVars = 6; -const JointPPosZ: JointVars = 7; -const JointPQuatX: JointVars = 8; -const JointPQuatY: JointVars = 9; -const JointPQuatZ: JointVars = 10; -const JointPQuatW: JointVars = 11; -const JointCPosX: JointVars = 12; -const JointCPosY: JointVars = 13; -const JointCPosZ: JointVars = 14; -const JointCQuatX: JointVars = 15; -const JointCQuatY: JointVars = 16; -const JointCQuatZ: JointVars = 17; -const JointCQuatW: JointVars = 18; -const JointLinearDoFN: JointVars = 19; -const JointAngularDoFN: JointVars = 20; -const JointDoF1: JointVars = 21; -const JointDoF2: JointVars = 22; -const JointDoF3: JointVars = 23; -const JointDoF4: JointVars = 24; -const JointDoF5: JointVars = 25; -const JointDoF6: JointVars = 26; -const JointPForceX: JointVars = 27; -const JointPForceY: JointVars = 28; -const JointPForceZ: JointVars = 29; -const JointPTorqueX: JointVars = 30; -const JointPTorqueY: JointVars = 31; -const JointPTorqueZ: JointVars = 32; -const JointCForceX: JointVars = 33; -const JointCForceY: JointVars = 34; -const JointCForceZ: JointVars = 35; -const JointCTorqueX: JointVars = 36; -const JointCTorqueY: JointVars = 37; -const JointCTorqueZ: JointVars = 38; -const JointLinLambdaX: JointVars = 39; -const JointLinLambdaY: JointVars = 40; -const JointLinLambdaZ: JointVars = 41; -const JointAngLambdaX: JointVars = 42; -const JointAngLambdaY: JointVars = 43; -const JointAngLambdaZ: JointVars = 44; +const JointNoLinearRotation: JointVars = 3; +const JointParent: JointVars = 4; +const JointChild: JointVars = 5; +const JointPPosX: JointVars = 6; +const JointPPosY: JointVars = 7; +const JointPPosZ: JointVars = 8; +const JointPQuatX: JointVars = 9; +const JointPQuatY: JointVars = 10; +const JointPQuatZ: JointVars = 11; +const JointPQuatW: JointVars = 12; +const JointCPosX: JointVars = 13; +const JointCPosY: JointVars = 14; +const JointCPosZ: JointVars = 15; +const JointCQuatX: JointVars = 16; +const JointCQuatY: JointVars = 17; +const JointCQuatZ: JointVars = 18; +const JointCQuatW: JointVars = 19; +const JointLinearDoFN: JointVars = 20; +const JointAngularDoFN: JointVars = 21; +const JointDoF1: JointVars = 22; +const JointDoF2: JointVars = 23; +const JointDoF3: JointVars = 24; +const JointDoF4: JointVars = 25; +const JointDoF5: JointVars = 26; +const JointDoF6: JointVars = 27; +const JointPForceX: JointVars = 28; +const JointPForceY: JointVars = 29; +const JointPForceZ: JointVars = 30; +const JointPTorqueX: JointVars = 31; +const JointPTorqueY: JointVars = 32; +const JointPTorqueZ: JointVars = 33; +const JointCForceX: JointVars = 34; +const JointCForceY: JointVars = 35; +const JointCForceZ: JointVars = 36; +const JointCTorqueX: JointVars = 37; +const JointCTorqueY: JointVars = 38; +const JointCTorqueZ: JointVars = 39; +const JointLinLambdaX: JointVars = 40; +const JointLinLambdaY: JointVars = 41; +const JointLinLambdaZ: JointVars = 42; +const JointAngLambdaX: JointVars = 43; +const JointAngLambdaY: JointVars = 44; +const JointAngLambdaZ: JointVars = 45; alias JointDoFVars = i32; //enums:enum const JointAxisX: JointDoFVars = 0; const JointAxisY: JointDoFVars = 1; diff --git a/physics/shaders/CollisionNarrow.wgsl b/physics/shaders/CollisionNarrow.wgsl index c66a7b30..eeb8b398 100644 --- a/physics/shaders/CollisionNarrow.wgsl +++ b/physics/shaders/CollisionNarrow.wgsl @@ -368,7 +368,7 @@ const JointControlVarsN: JointControlVars = 6; const DynamicVarsN: DynamicVars = 33; const GPUVarsN: GPUVars = 13; const JointTypesN: JointTypes = 8; -const JointVarsN: JointVars = 45; +const JointVarsN: JointVars = 46; const JointDoFVarsN: JointDoFVars = 5; const ShapesN: Shapes = 6; @@ -387,48 +387,49 @@ alias JointVars = i32; //enums:enum const JointType: JointVars = 0; const JointEnabled: JointVars = 1; const JointParentFixed: JointVars = 2; -const JointParent: JointVars = 3; -const JointChild: JointVars = 4; -const JointPPosX: JointVars = 5; -const JointPPosY: JointVars = 6; -const JointPPosZ: JointVars = 7; -const JointPQuatX: JointVars = 8; -const JointPQuatY: JointVars = 9; -const JointPQuatZ: JointVars = 10; -const JointPQuatW: JointVars = 11; -const JointCPosX: JointVars = 12; -const JointCPosY: JointVars = 13; -const JointCPosZ: JointVars = 14; -const JointCQuatX: JointVars = 15; -const JointCQuatY: JointVars = 16; -const JointCQuatZ: JointVars = 17; -const JointCQuatW: JointVars = 18; -const JointLinearDoFN: JointVars = 19; -const JointAngularDoFN: JointVars = 20; -const JointDoF1: JointVars = 21; -const JointDoF2: JointVars = 22; -const JointDoF3: JointVars = 23; -const JointDoF4: JointVars = 24; -const JointDoF5: JointVars = 25; -const JointDoF6: JointVars = 26; -const JointPForceX: JointVars = 27; -const JointPForceY: JointVars = 28; -const JointPForceZ: JointVars = 29; -const JointPTorqueX: JointVars = 30; -const JointPTorqueY: JointVars = 31; -const JointPTorqueZ: JointVars = 32; -const JointCForceX: JointVars = 33; -const JointCForceY: JointVars = 34; -const JointCForceZ: JointVars = 35; -const JointCTorqueX: JointVars = 36; -const JointCTorqueY: JointVars = 37; -const JointCTorqueZ: JointVars = 38; -const JointLinLambdaX: JointVars = 39; -const JointLinLambdaY: JointVars = 40; -const JointLinLambdaZ: JointVars = 41; -const JointAngLambdaX: JointVars = 42; -const JointAngLambdaY: JointVars = 43; -const JointAngLambdaZ: JointVars = 44; +const JointNoLinearRotation: JointVars = 3; +const JointParent: JointVars = 4; +const JointChild: JointVars = 5; +const JointPPosX: JointVars = 6; +const JointPPosY: JointVars = 7; +const JointPPosZ: JointVars = 8; +const JointPQuatX: JointVars = 9; +const JointPQuatY: JointVars = 10; +const JointPQuatZ: JointVars = 11; +const JointPQuatW: JointVars = 12; +const JointCPosX: JointVars = 13; +const JointCPosY: JointVars = 14; +const JointCPosZ: JointVars = 15; +const JointCQuatX: JointVars = 16; +const JointCQuatY: JointVars = 17; +const JointCQuatZ: JointVars = 18; +const JointCQuatW: JointVars = 19; +const JointLinearDoFN: JointVars = 20; +const JointAngularDoFN: JointVars = 21; +const JointDoF1: JointVars = 22; +const JointDoF2: JointVars = 23; +const JointDoF3: JointVars = 24; +const JointDoF4: JointVars = 25; +const JointDoF5: JointVars = 26; +const JointDoF6: JointVars = 27; +const JointPForceX: JointVars = 28; +const JointPForceY: JointVars = 29; +const JointPForceZ: JointVars = 30; +const JointPTorqueX: JointVars = 31; +const JointPTorqueY: JointVars = 32; +const JointPTorqueZ: JointVars = 33; +const JointCForceX: JointVars = 34; +const JointCForceY: JointVars = 35; +const JointCForceZ: JointVars = 36; +const JointCTorqueX: JointVars = 37; +const JointCTorqueY: JointVars = 38; +const JointCTorqueZ: JointVars = 39; +const JointLinLambdaX: JointVars = 40; +const JointLinLambdaY: JointVars = 41; +const JointLinLambdaZ: JointVars = 42; +const JointAngLambdaX: JointVars = 43; +const JointAngLambdaY: JointVars = 44; +const JointAngLambdaZ: JointVars = 45; alias JointDoFVars = i32; //enums:enum const JointAxisX: JointDoFVars = 0; const JointAxisY: JointDoFVars = 1; diff --git a/physics/shaders/DynamicsCurToNext.wgsl b/physics/shaders/DynamicsCurToNext.wgsl index 2fe0ec37..64ee43b6 100644 --- a/physics/shaders/DynamicsCurToNext.wgsl +++ b/physics/shaders/DynamicsCurToNext.wgsl @@ -162,7 +162,7 @@ const JointControlVarsN: JointControlVars = 6; const DynamicVarsN: DynamicVars = 33; const GPUVarsN: GPUVars = 13; const JointTypesN: JointTypes = 8; -const JointVarsN: JointVars = 45; +const JointVarsN: JointVars = 46; const JointDoFVarsN: JointDoFVars = 5; const ShapesN: Shapes = 6; @@ -181,48 +181,49 @@ alias JointVars = i32; //enums:enum const JointType: JointVars = 0; const JointEnabled: JointVars = 1; const JointParentFixed: JointVars = 2; -const JointParent: JointVars = 3; -const JointChild: JointVars = 4; -const JointPPosX: JointVars = 5; -const JointPPosY: JointVars = 6; -const JointPPosZ: JointVars = 7; -const JointPQuatX: JointVars = 8; -const JointPQuatY: JointVars = 9; -const JointPQuatZ: JointVars = 10; -const JointPQuatW: JointVars = 11; -const JointCPosX: JointVars = 12; -const JointCPosY: JointVars = 13; -const JointCPosZ: JointVars = 14; -const JointCQuatX: JointVars = 15; -const JointCQuatY: JointVars = 16; -const JointCQuatZ: JointVars = 17; -const JointCQuatW: JointVars = 18; -const JointLinearDoFN: JointVars = 19; -const JointAngularDoFN: JointVars = 20; -const JointDoF1: JointVars = 21; -const JointDoF2: JointVars = 22; -const JointDoF3: JointVars = 23; -const JointDoF4: JointVars = 24; -const JointDoF5: JointVars = 25; -const JointDoF6: JointVars = 26; -const JointPForceX: JointVars = 27; -const JointPForceY: JointVars = 28; -const JointPForceZ: JointVars = 29; -const JointPTorqueX: JointVars = 30; -const JointPTorqueY: JointVars = 31; -const JointPTorqueZ: JointVars = 32; -const JointCForceX: JointVars = 33; -const JointCForceY: JointVars = 34; -const JointCForceZ: JointVars = 35; -const JointCTorqueX: JointVars = 36; -const JointCTorqueY: JointVars = 37; -const JointCTorqueZ: JointVars = 38; -const JointLinLambdaX: JointVars = 39; -const JointLinLambdaY: JointVars = 40; -const JointLinLambdaZ: JointVars = 41; -const JointAngLambdaX: JointVars = 42; -const JointAngLambdaY: JointVars = 43; -const JointAngLambdaZ: JointVars = 44; +const JointNoLinearRotation: JointVars = 3; +const JointParent: JointVars = 4; +const JointChild: JointVars = 5; +const JointPPosX: JointVars = 6; +const JointPPosY: JointVars = 7; +const JointPPosZ: JointVars = 8; +const JointPQuatX: JointVars = 9; +const JointPQuatY: JointVars = 10; +const JointPQuatZ: JointVars = 11; +const JointPQuatW: JointVars = 12; +const JointCPosX: JointVars = 13; +const JointCPosY: JointVars = 14; +const JointCPosZ: JointVars = 15; +const JointCQuatX: JointVars = 16; +const JointCQuatY: JointVars = 17; +const JointCQuatZ: JointVars = 18; +const JointCQuatW: JointVars = 19; +const JointLinearDoFN: JointVars = 20; +const JointAngularDoFN: JointVars = 21; +const JointDoF1: JointVars = 22; +const JointDoF2: JointVars = 23; +const JointDoF3: JointVars = 24; +const JointDoF4: JointVars = 25; +const JointDoF5: JointVars = 26; +const JointDoF6: JointVars = 27; +const JointPForceX: JointVars = 28; +const JointPForceY: JointVars = 29; +const JointPForceZ: JointVars = 30; +const JointPTorqueX: JointVars = 31; +const JointPTorqueY: JointVars = 32; +const JointPTorqueZ: JointVars = 33; +const JointCForceX: JointVars = 34; +const JointCForceY: JointVars = 35; +const JointCForceZ: JointVars = 36; +const JointCTorqueX: JointVars = 37; +const JointCTorqueY: JointVars = 38; +const JointCTorqueZ: JointVars = 39; +const JointLinLambdaX: JointVars = 40; +const JointLinLambdaY: JointVars = 41; +const JointLinLambdaZ: JointVars = 42; +const JointAngLambdaX: JointVars = 43; +const JointAngLambdaY: JointVars = 44; +const JointAngLambdaZ: JointVars = 45; alias JointDoFVars = i32; //enums:enum const JointAxisX: JointDoFVars = 0; const JointAxisY: JointDoFVars = 1; diff --git a/physics/shaders/ForcesFromJoints.wgsl b/physics/shaders/ForcesFromJoints.wgsl index e18d444b..45ccebfa 100644 --- a/physics/shaders/ForcesFromJoints.wgsl +++ b/physics/shaders/ForcesFromJoints.wgsl @@ -180,7 +180,7 @@ const JointControlVarsN: JointControlVars = 6; const DynamicVarsN: DynamicVars = 33; const GPUVarsN: GPUVars = 13; const JointTypesN: JointTypes = 8; -const JointVarsN: JointVars = 45; +const JointVarsN: JointVars = 46; const JointDoFVarsN: JointDoFVars = 5; const ShapesN: Shapes = 6; @@ -199,48 +199,49 @@ alias JointVars = i32; //enums:enum const JointType: JointVars = 0; const JointEnabled: JointVars = 1; const JointParentFixed: JointVars = 2; -const JointParent: JointVars = 3; -const JointChild: JointVars = 4; -const JointPPosX: JointVars = 5; -const JointPPosY: JointVars = 6; -const JointPPosZ: JointVars = 7; -const JointPQuatX: JointVars = 8; -const JointPQuatY: JointVars = 9; -const JointPQuatZ: JointVars = 10; -const JointPQuatW: JointVars = 11; -const JointCPosX: JointVars = 12; -const JointCPosY: JointVars = 13; -const JointCPosZ: JointVars = 14; -const JointCQuatX: JointVars = 15; -const JointCQuatY: JointVars = 16; -const JointCQuatZ: JointVars = 17; -const JointCQuatW: JointVars = 18; -const JointLinearDoFN: JointVars = 19; -const JointAngularDoFN: JointVars = 20; -const JointDoF1: JointVars = 21; -const JointDoF2: JointVars = 22; -const JointDoF3: JointVars = 23; -const JointDoF4: JointVars = 24; -const JointDoF5: JointVars = 25; -const JointDoF6: JointVars = 26; -const JointPForceX: JointVars = 27; -const JointPForceY: JointVars = 28; -const JointPForceZ: JointVars = 29; -const JointPTorqueX: JointVars = 30; -const JointPTorqueY: JointVars = 31; -const JointPTorqueZ: JointVars = 32; -const JointCForceX: JointVars = 33; -const JointCForceY: JointVars = 34; -const JointCForceZ: JointVars = 35; -const JointCTorqueX: JointVars = 36; -const JointCTorqueY: JointVars = 37; -const JointCTorqueZ: JointVars = 38; -const JointLinLambdaX: JointVars = 39; -const JointLinLambdaY: JointVars = 40; -const JointLinLambdaZ: JointVars = 41; -const JointAngLambdaX: JointVars = 42; -const JointAngLambdaY: JointVars = 43; -const JointAngLambdaZ: JointVars = 44; +const JointNoLinearRotation: JointVars = 3; +const JointParent: JointVars = 4; +const JointChild: JointVars = 5; +const JointPPosX: JointVars = 6; +const JointPPosY: JointVars = 7; +const JointPPosZ: JointVars = 8; +const JointPQuatX: JointVars = 9; +const JointPQuatY: JointVars = 10; +const JointPQuatZ: JointVars = 11; +const JointPQuatW: JointVars = 12; +const JointCPosX: JointVars = 13; +const JointCPosY: JointVars = 14; +const JointCPosZ: JointVars = 15; +const JointCQuatX: JointVars = 16; +const JointCQuatY: JointVars = 17; +const JointCQuatZ: JointVars = 18; +const JointCQuatW: JointVars = 19; +const JointLinearDoFN: JointVars = 20; +const JointAngularDoFN: JointVars = 21; +const JointDoF1: JointVars = 22; +const JointDoF2: JointVars = 23; +const JointDoF3: JointVars = 24; +const JointDoF4: JointVars = 25; +const JointDoF5: JointVars = 26; +const JointDoF6: JointVars = 27; +const JointPForceX: JointVars = 28; +const JointPForceY: JointVars = 29; +const JointPForceZ: JointVars = 30; +const JointPTorqueX: JointVars = 31; +const JointPTorqueY: JointVars = 32; +const JointPTorqueZ: JointVars = 33; +const JointCForceX: JointVars = 34; +const JointCForceY: JointVars = 35; +const JointCForceZ: JointVars = 36; +const JointCTorqueX: JointVars = 37; +const JointCTorqueY: JointVars = 38; +const JointCTorqueZ: JointVars = 39; +const JointLinLambdaX: JointVars = 40; +const JointLinLambdaY: JointVars = 41; +const JointLinLambdaZ: JointVars = 42; +const JointAngLambdaX: JointVars = 43; +const JointAngLambdaY: JointVars = 44; +const JointAngLambdaZ: JointVars = 45; fn JointPForce(idx: i32) -> vec3 { return vec3(Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointPForceX))], Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointPForceY))], Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointPForceZ))]); } diff --git a/physics/shaders/InitDynamics.wgsl b/physics/shaders/InitDynamics.wgsl index 684cb70c..28143271 100644 --- a/physics/shaders/InitDynamics.wgsl +++ b/physics/shaders/InitDynamics.wgsl @@ -169,7 +169,7 @@ const JointControlVarsN: JointControlVars = 6; const DynamicVarsN: DynamicVars = 33; const GPUVarsN: GPUVars = 13; const JointTypesN: JointTypes = 8; -const JointVarsN: JointVars = 45; +const JointVarsN: JointVars = 46; const JointDoFVarsN: JointDoFVars = 5; const ShapesN: Shapes = 6; @@ -188,48 +188,49 @@ alias JointVars = i32; //enums:enum const JointType: JointVars = 0; const JointEnabled: JointVars = 1; const JointParentFixed: JointVars = 2; -const JointParent: JointVars = 3; -const JointChild: JointVars = 4; -const JointPPosX: JointVars = 5; -const JointPPosY: JointVars = 6; -const JointPPosZ: JointVars = 7; -const JointPQuatX: JointVars = 8; -const JointPQuatY: JointVars = 9; -const JointPQuatZ: JointVars = 10; -const JointPQuatW: JointVars = 11; -const JointCPosX: JointVars = 12; -const JointCPosY: JointVars = 13; -const JointCPosZ: JointVars = 14; -const JointCQuatX: JointVars = 15; -const JointCQuatY: JointVars = 16; -const JointCQuatZ: JointVars = 17; -const JointCQuatW: JointVars = 18; -const JointLinearDoFN: JointVars = 19; -const JointAngularDoFN: JointVars = 20; -const JointDoF1: JointVars = 21; -const JointDoF2: JointVars = 22; -const JointDoF3: JointVars = 23; -const JointDoF4: JointVars = 24; -const JointDoF5: JointVars = 25; -const JointDoF6: JointVars = 26; -const JointPForceX: JointVars = 27; -const JointPForceY: JointVars = 28; -const JointPForceZ: JointVars = 29; -const JointPTorqueX: JointVars = 30; -const JointPTorqueY: JointVars = 31; -const JointPTorqueZ: JointVars = 32; -const JointCForceX: JointVars = 33; -const JointCForceY: JointVars = 34; -const JointCForceZ: JointVars = 35; -const JointCTorqueX: JointVars = 36; -const JointCTorqueY: JointVars = 37; -const JointCTorqueZ: JointVars = 38; -const JointLinLambdaX: JointVars = 39; -const JointLinLambdaY: JointVars = 40; -const JointLinLambdaZ: JointVars = 41; -const JointAngLambdaX: JointVars = 42; -const JointAngLambdaY: JointVars = 43; -const JointAngLambdaZ: JointVars = 44; +const JointNoLinearRotation: JointVars = 3; +const JointParent: JointVars = 4; +const JointChild: JointVars = 5; +const JointPPosX: JointVars = 6; +const JointPPosY: JointVars = 7; +const JointPPosZ: JointVars = 8; +const JointPQuatX: JointVars = 9; +const JointPQuatY: JointVars = 10; +const JointPQuatZ: JointVars = 11; +const JointPQuatW: JointVars = 12; +const JointCPosX: JointVars = 13; +const JointCPosY: JointVars = 14; +const JointCPosZ: JointVars = 15; +const JointCQuatX: JointVars = 16; +const JointCQuatY: JointVars = 17; +const JointCQuatZ: JointVars = 18; +const JointCQuatW: JointVars = 19; +const JointLinearDoFN: JointVars = 20; +const JointAngularDoFN: JointVars = 21; +const JointDoF1: JointVars = 22; +const JointDoF2: JointVars = 23; +const JointDoF3: JointVars = 24; +const JointDoF4: JointVars = 25; +const JointDoF5: JointVars = 26; +const JointDoF6: JointVars = 27; +const JointPForceX: JointVars = 28; +const JointPForceY: JointVars = 29; +const JointPForceZ: JointVars = 30; +const JointPTorqueX: JointVars = 31; +const JointPTorqueY: JointVars = 32; +const JointPTorqueZ: JointVars = 33; +const JointCForceX: JointVars = 34; +const JointCForceY: JointVars = 35; +const JointCForceZ: JointVars = 36; +const JointCTorqueX: JointVars = 37; +const JointCTorqueY: JointVars = 38; +const JointCTorqueZ: JointVars = 39; +const JointLinLambdaX: JointVars = 40; +const JointLinLambdaY: JointVars = 41; +const JointLinLambdaZ: JointVars = 42; +const JointAngLambdaX: JointVars = 43; +const JointAngLambdaY: JointVars = 44; +const JointAngLambdaZ: JointVars = 45; alias JointDoFVars = i32; //enums:enum const JointAxisX: JointDoFVars = 0; const JointAxisY: JointDoFVars = 1; diff --git a/physics/shaders/StepBodyContactDeltas.wgsl b/physics/shaders/StepBodyContactDeltas.wgsl index a17eca7b..306e2790 100644 --- a/physics/shaders/StepBodyContactDeltas.wgsl +++ b/physics/shaders/StepBodyContactDeltas.wgsl @@ -265,7 +265,7 @@ const JointControlVarsN: JointControlVars = 6; const DynamicVarsN: DynamicVars = 33; const GPUVarsN: GPUVars = 13; const JointTypesN: JointTypes = 8; -const JointVarsN: JointVars = 45; +const JointVarsN: JointVars = 46; const JointDoFVarsN: JointDoFVars = 5; const ShapesN: Shapes = 6; @@ -284,48 +284,49 @@ alias JointVars = i32; //enums:enum const JointType: JointVars = 0; const JointEnabled: JointVars = 1; const JointParentFixed: JointVars = 2; -const JointParent: JointVars = 3; -const JointChild: JointVars = 4; -const JointPPosX: JointVars = 5; -const JointPPosY: JointVars = 6; -const JointPPosZ: JointVars = 7; -const JointPQuatX: JointVars = 8; -const JointPQuatY: JointVars = 9; -const JointPQuatZ: JointVars = 10; -const JointPQuatW: JointVars = 11; -const JointCPosX: JointVars = 12; -const JointCPosY: JointVars = 13; -const JointCPosZ: JointVars = 14; -const JointCQuatX: JointVars = 15; -const JointCQuatY: JointVars = 16; -const JointCQuatZ: JointVars = 17; -const JointCQuatW: JointVars = 18; -const JointLinearDoFN: JointVars = 19; -const JointAngularDoFN: JointVars = 20; -const JointDoF1: JointVars = 21; -const JointDoF2: JointVars = 22; -const JointDoF3: JointVars = 23; -const JointDoF4: JointVars = 24; -const JointDoF5: JointVars = 25; -const JointDoF6: JointVars = 26; -const JointPForceX: JointVars = 27; -const JointPForceY: JointVars = 28; -const JointPForceZ: JointVars = 29; -const JointPTorqueX: JointVars = 30; -const JointPTorqueY: JointVars = 31; -const JointPTorqueZ: JointVars = 32; -const JointCForceX: JointVars = 33; -const JointCForceY: JointVars = 34; -const JointCForceZ: JointVars = 35; -const JointCTorqueX: JointVars = 36; -const JointCTorqueY: JointVars = 37; -const JointCTorqueZ: JointVars = 38; -const JointLinLambdaX: JointVars = 39; -const JointLinLambdaY: JointVars = 40; -const JointLinLambdaZ: JointVars = 41; -const JointAngLambdaX: JointVars = 42; -const JointAngLambdaY: JointVars = 43; -const JointAngLambdaZ: JointVars = 44; +const JointNoLinearRotation: JointVars = 3; +const JointParent: JointVars = 4; +const JointChild: JointVars = 5; +const JointPPosX: JointVars = 6; +const JointPPosY: JointVars = 7; +const JointPPosZ: JointVars = 8; +const JointPQuatX: JointVars = 9; +const JointPQuatY: JointVars = 10; +const JointPQuatZ: JointVars = 11; +const JointPQuatW: JointVars = 12; +const JointCPosX: JointVars = 13; +const JointCPosY: JointVars = 14; +const JointCPosZ: JointVars = 15; +const JointCQuatX: JointVars = 16; +const JointCQuatY: JointVars = 17; +const JointCQuatZ: JointVars = 18; +const JointCQuatW: JointVars = 19; +const JointLinearDoFN: JointVars = 20; +const JointAngularDoFN: JointVars = 21; +const JointDoF1: JointVars = 22; +const JointDoF2: JointVars = 23; +const JointDoF3: JointVars = 24; +const JointDoF4: JointVars = 25; +const JointDoF5: JointVars = 26; +const JointDoF6: JointVars = 27; +const JointPForceX: JointVars = 28; +const JointPForceY: JointVars = 29; +const JointPForceZ: JointVars = 30; +const JointPTorqueX: JointVars = 31; +const JointPTorqueY: JointVars = 32; +const JointPTorqueZ: JointVars = 33; +const JointCForceX: JointVars = 34; +const JointCForceY: JointVars = 35; +const JointCForceZ: JointVars = 36; +const JointCTorqueX: JointVars = 37; +const JointCTorqueY: JointVars = 38; +const JointCTorqueZ: JointVars = 39; +const JointLinLambdaX: JointVars = 40; +const JointLinLambdaY: JointVars = 41; +const JointLinLambdaZ: JointVars = 42; +const JointAngLambdaX: JointVars = 43; +const JointAngLambdaY: JointVars = 44; +const JointAngLambdaZ: JointVars = 45; alias JointDoFVars = i32; //enums:enum const JointAxisX: JointDoFVars = 0; const JointAxisY: JointDoFVars = 1; diff --git a/physics/shaders/StepBodyContacts.wgsl b/physics/shaders/StepBodyContacts.wgsl index 0f51f56a..9186f72e 100644 --- a/physics/shaders/StepBodyContacts.wgsl +++ b/physics/shaders/StepBodyContacts.wgsl @@ -394,7 +394,7 @@ const JointControlVarsN: JointControlVars = 6; const DynamicVarsN: DynamicVars = 33; const GPUVarsN: GPUVars = 13; const JointTypesN: JointTypes = 8; -const JointVarsN: JointVars = 45; +const JointVarsN: JointVars = 46; const JointDoFVarsN: JointDoFVars = 5; const ShapesN: Shapes = 6; @@ -413,48 +413,49 @@ alias JointVars = i32; //enums:enum const JointType: JointVars = 0; const JointEnabled: JointVars = 1; const JointParentFixed: JointVars = 2; -const JointParent: JointVars = 3; -const JointChild: JointVars = 4; -const JointPPosX: JointVars = 5; -const JointPPosY: JointVars = 6; -const JointPPosZ: JointVars = 7; -const JointPQuatX: JointVars = 8; -const JointPQuatY: JointVars = 9; -const JointPQuatZ: JointVars = 10; -const JointPQuatW: JointVars = 11; -const JointCPosX: JointVars = 12; -const JointCPosY: JointVars = 13; -const JointCPosZ: JointVars = 14; -const JointCQuatX: JointVars = 15; -const JointCQuatY: JointVars = 16; -const JointCQuatZ: JointVars = 17; -const JointCQuatW: JointVars = 18; -const JointLinearDoFN: JointVars = 19; -const JointAngularDoFN: JointVars = 20; -const JointDoF1: JointVars = 21; -const JointDoF2: JointVars = 22; -const JointDoF3: JointVars = 23; -const JointDoF4: JointVars = 24; -const JointDoF5: JointVars = 25; -const JointDoF6: JointVars = 26; -const JointPForceX: JointVars = 27; -const JointPForceY: JointVars = 28; -const JointPForceZ: JointVars = 29; -const JointPTorqueX: JointVars = 30; -const JointPTorqueY: JointVars = 31; -const JointPTorqueZ: JointVars = 32; -const JointCForceX: JointVars = 33; -const JointCForceY: JointVars = 34; -const JointCForceZ: JointVars = 35; -const JointCTorqueX: JointVars = 36; -const JointCTorqueY: JointVars = 37; -const JointCTorqueZ: JointVars = 38; -const JointLinLambdaX: JointVars = 39; -const JointLinLambdaY: JointVars = 40; -const JointLinLambdaZ: JointVars = 41; -const JointAngLambdaX: JointVars = 42; -const JointAngLambdaY: JointVars = 43; -const JointAngLambdaZ: JointVars = 44; +const JointNoLinearRotation: JointVars = 3; +const JointParent: JointVars = 4; +const JointChild: JointVars = 5; +const JointPPosX: JointVars = 6; +const JointPPosY: JointVars = 7; +const JointPPosZ: JointVars = 8; +const JointPQuatX: JointVars = 9; +const JointPQuatY: JointVars = 10; +const JointPQuatZ: JointVars = 11; +const JointPQuatW: JointVars = 12; +const JointCPosX: JointVars = 13; +const JointCPosY: JointVars = 14; +const JointCPosZ: JointVars = 15; +const JointCQuatX: JointVars = 16; +const JointCQuatY: JointVars = 17; +const JointCQuatZ: JointVars = 18; +const JointCQuatW: JointVars = 19; +const JointLinearDoFN: JointVars = 20; +const JointAngularDoFN: JointVars = 21; +const JointDoF1: JointVars = 22; +const JointDoF2: JointVars = 23; +const JointDoF3: JointVars = 24; +const JointDoF4: JointVars = 25; +const JointDoF5: JointVars = 26; +const JointDoF6: JointVars = 27; +const JointPForceX: JointVars = 28; +const JointPForceY: JointVars = 29; +const JointPForceZ: JointVars = 30; +const JointPTorqueX: JointVars = 31; +const JointPTorqueY: JointVars = 32; +const JointPTorqueZ: JointVars = 33; +const JointCForceX: JointVars = 34; +const JointCForceY: JointVars = 35; +const JointCForceZ: JointVars = 36; +const JointCTorqueX: JointVars = 37; +const JointCTorqueY: JointVars = 38; +const JointCTorqueZ: JointVars = 39; +const JointLinLambdaX: JointVars = 40; +const JointLinLambdaY: JointVars = 41; +const JointLinLambdaZ: JointVars = 42; +const JointAngLambdaX: JointVars = 43; +const JointAngLambdaY: JointVars = 44; +const JointAngLambdaZ: JointVars = 45; alias JointDoFVars = i32; //enums:enum const JointAxisX: JointDoFVars = 0; const JointAxisY: JointDoFVars = 1; diff --git a/physics/shaders/StepIntegrateBodies.wgsl b/physics/shaders/StepIntegrateBodies.wgsl index 3c9200de..8b4f4d99 100644 --- a/physics/shaders/StepIntegrateBodies.wgsl +++ b/physics/shaders/StepIntegrateBodies.wgsl @@ -192,7 +192,7 @@ const JointControlVarsN: JointControlVars = 6; const DynamicVarsN: DynamicVars = 33; const GPUVarsN: GPUVars = 13; const JointTypesN: JointTypes = 8; -const JointVarsN: JointVars = 45; +const JointVarsN: JointVars = 46; const JointDoFVarsN: JointDoFVars = 5; const ShapesN: Shapes = 6; @@ -211,48 +211,49 @@ alias JointVars = i32; //enums:enum const JointType: JointVars = 0; const JointEnabled: JointVars = 1; const JointParentFixed: JointVars = 2; -const JointParent: JointVars = 3; -const JointChild: JointVars = 4; -const JointPPosX: JointVars = 5; -const JointPPosY: JointVars = 6; -const JointPPosZ: JointVars = 7; -const JointPQuatX: JointVars = 8; -const JointPQuatY: JointVars = 9; -const JointPQuatZ: JointVars = 10; -const JointPQuatW: JointVars = 11; -const JointCPosX: JointVars = 12; -const JointCPosY: JointVars = 13; -const JointCPosZ: JointVars = 14; -const JointCQuatX: JointVars = 15; -const JointCQuatY: JointVars = 16; -const JointCQuatZ: JointVars = 17; -const JointCQuatW: JointVars = 18; -const JointLinearDoFN: JointVars = 19; -const JointAngularDoFN: JointVars = 20; -const JointDoF1: JointVars = 21; -const JointDoF2: JointVars = 22; -const JointDoF3: JointVars = 23; -const JointDoF4: JointVars = 24; -const JointDoF5: JointVars = 25; -const JointDoF6: JointVars = 26; -const JointPForceX: JointVars = 27; -const JointPForceY: JointVars = 28; -const JointPForceZ: JointVars = 29; -const JointPTorqueX: JointVars = 30; -const JointPTorqueY: JointVars = 31; -const JointPTorqueZ: JointVars = 32; -const JointCForceX: JointVars = 33; -const JointCForceY: JointVars = 34; -const JointCForceZ: JointVars = 35; -const JointCTorqueX: JointVars = 36; -const JointCTorqueY: JointVars = 37; -const JointCTorqueZ: JointVars = 38; -const JointLinLambdaX: JointVars = 39; -const JointLinLambdaY: JointVars = 40; -const JointLinLambdaZ: JointVars = 41; -const JointAngLambdaX: JointVars = 42; -const JointAngLambdaY: JointVars = 43; -const JointAngLambdaZ: JointVars = 44; +const JointNoLinearRotation: JointVars = 3; +const JointParent: JointVars = 4; +const JointChild: JointVars = 5; +const JointPPosX: JointVars = 6; +const JointPPosY: JointVars = 7; +const JointPPosZ: JointVars = 8; +const JointPQuatX: JointVars = 9; +const JointPQuatY: JointVars = 10; +const JointPQuatZ: JointVars = 11; +const JointPQuatW: JointVars = 12; +const JointCPosX: JointVars = 13; +const JointCPosY: JointVars = 14; +const JointCPosZ: JointVars = 15; +const JointCQuatX: JointVars = 16; +const JointCQuatY: JointVars = 17; +const JointCQuatZ: JointVars = 18; +const JointCQuatW: JointVars = 19; +const JointLinearDoFN: JointVars = 20; +const JointAngularDoFN: JointVars = 21; +const JointDoF1: JointVars = 22; +const JointDoF2: JointVars = 23; +const JointDoF3: JointVars = 24; +const JointDoF4: JointVars = 25; +const JointDoF5: JointVars = 26; +const JointDoF6: JointVars = 27; +const JointPForceX: JointVars = 28; +const JointPForceY: JointVars = 29; +const JointPForceZ: JointVars = 30; +const JointPTorqueX: JointVars = 31; +const JointPTorqueY: JointVars = 32; +const JointPTorqueZ: JointVars = 33; +const JointCForceX: JointVars = 34; +const JointCForceY: JointVars = 35; +const JointCForceZ: JointVars = 36; +const JointCTorqueX: JointVars = 37; +const JointCTorqueY: JointVars = 38; +const JointCTorqueZ: JointVars = 39; +const JointLinLambdaX: JointVars = 40; +const JointLinLambdaY: JointVars = 41; +const JointLinLambdaZ: JointVars = 42; +const JointAngLambdaX: JointVars = 43; +const JointAngLambdaY: JointVars = 44; +const JointAngLambdaZ: JointVars = 45; alias JointDoFVars = i32; //enums:enum const JointAxisX: JointDoFVars = 0; const JointAxisY: JointDoFVars = 1; diff --git a/physics/shaders/StepJointForces.wgsl b/physics/shaders/StepJointForces.wgsl index 94a5944d..aeebf7cb 100644 --- a/physics/shaders/StepJointForces.wgsl +++ b/physics/shaders/StepJointForces.wgsl @@ -183,7 +183,7 @@ const JointControlVarsN: JointControlVars = 6; const DynamicVarsN: DynamicVars = 33; const GPUVarsN: GPUVars = 13; const JointTypesN: JointTypes = 8; -const JointVarsN: JointVars = 45; +const JointVarsN: JointVars = 46; const JointDoFVarsN: JointDoFVars = 5; const ShapesN: Shapes = 6; @@ -202,48 +202,49 @@ alias JointVars = i32; //enums:enum const JointType: JointVars = 0; const JointEnabled: JointVars = 1; const JointParentFixed: JointVars = 2; -const JointParent: JointVars = 3; -const JointChild: JointVars = 4; -const JointPPosX: JointVars = 5; -const JointPPosY: JointVars = 6; -const JointPPosZ: JointVars = 7; -const JointPQuatX: JointVars = 8; -const JointPQuatY: JointVars = 9; -const JointPQuatZ: JointVars = 10; -const JointPQuatW: JointVars = 11; -const JointCPosX: JointVars = 12; -const JointCPosY: JointVars = 13; -const JointCPosZ: JointVars = 14; -const JointCQuatX: JointVars = 15; -const JointCQuatY: JointVars = 16; -const JointCQuatZ: JointVars = 17; -const JointCQuatW: JointVars = 18; -const JointLinearDoFN: JointVars = 19; -const JointAngularDoFN: JointVars = 20; -const JointDoF1: JointVars = 21; -const JointDoF2: JointVars = 22; -const JointDoF3: JointVars = 23; -const JointDoF4: JointVars = 24; -const JointDoF5: JointVars = 25; -const JointDoF6: JointVars = 26; -const JointPForceX: JointVars = 27; -const JointPForceY: JointVars = 28; -const JointPForceZ: JointVars = 29; -const JointPTorqueX: JointVars = 30; -const JointPTorqueY: JointVars = 31; -const JointPTorqueZ: JointVars = 32; -const JointCForceX: JointVars = 33; -const JointCForceY: JointVars = 34; -const JointCForceZ: JointVars = 35; -const JointCTorqueX: JointVars = 36; -const JointCTorqueY: JointVars = 37; -const JointCTorqueZ: JointVars = 38; -const JointLinLambdaX: JointVars = 39; -const JointLinLambdaY: JointVars = 40; -const JointLinLambdaZ: JointVars = 41; -const JointAngLambdaX: JointVars = 42; -const JointAngLambdaY: JointVars = 43; -const JointAngLambdaZ: JointVars = 44; +const JointNoLinearRotation: JointVars = 3; +const JointParent: JointVars = 4; +const JointChild: JointVars = 5; +const JointPPosX: JointVars = 6; +const JointPPosY: JointVars = 7; +const JointPPosZ: JointVars = 8; +const JointPQuatX: JointVars = 9; +const JointPQuatY: JointVars = 10; +const JointPQuatZ: JointVars = 11; +const JointPQuatW: JointVars = 12; +const JointCPosX: JointVars = 13; +const JointCPosY: JointVars = 14; +const JointCPosZ: JointVars = 15; +const JointCQuatX: JointVars = 16; +const JointCQuatY: JointVars = 17; +const JointCQuatZ: JointVars = 18; +const JointCQuatW: JointVars = 19; +const JointLinearDoFN: JointVars = 20; +const JointAngularDoFN: JointVars = 21; +const JointDoF1: JointVars = 22; +const JointDoF2: JointVars = 23; +const JointDoF3: JointVars = 24; +const JointDoF4: JointVars = 25; +const JointDoF5: JointVars = 26; +const JointDoF6: JointVars = 27; +const JointPForceX: JointVars = 28; +const JointPForceY: JointVars = 29; +const JointPForceZ: JointVars = 30; +const JointPTorqueX: JointVars = 31; +const JointPTorqueY: JointVars = 32; +const JointPTorqueZ: JointVars = 33; +const JointCForceX: JointVars = 34; +const JointCForceY: JointVars = 35; +const JointCForceZ: JointVars = 36; +const JointCTorqueX: JointVars = 37; +const JointCTorqueY: JointVars = 38; +const JointCTorqueZ: JointVars = 39; +const JointLinLambdaX: JointVars = 40; +const JointLinLambdaY: JointVars = 41; +const JointLinLambdaZ: JointVars = 42; +const JointAngLambdaX: JointVars = 43; +const JointAngLambdaY: JointVars = 44; +const JointAngLambdaZ: JointVars = 45; fn GetJointType(idx: i32) -> JointTypes { return JointTypes(bitcast(Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointType))])); } diff --git a/physics/shaders/StepSolveJoints.wgsl b/physics/shaders/StepSolveJoints.wgsl index 800a5b18..097f5c46 100644 --- a/physics/shaders/StepSolveJoints.wgsl +++ b/physics/shaders/StepSolveJoints.wgsl @@ -213,7 +213,7 @@ const JointControlVarsN: JointControlVars = 6; const DynamicVarsN: DynamicVars = 33; const GPUVarsN: GPUVars = 13; const JointTypesN: JointTypes = 8; -const JointVarsN: JointVars = 45; +const JointVarsN: JointVars = 46; const JointDoFVarsN: JointDoFVars = 5; const ShapesN: Shapes = 6; @@ -232,48 +232,49 @@ alias JointVars = i32; //enums:enum const JointType: JointVars = 0; const JointEnabled: JointVars = 1; const JointParentFixed: JointVars = 2; -const JointParent: JointVars = 3; -const JointChild: JointVars = 4; -const JointPPosX: JointVars = 5; -const JointPPosY: JointVars = 6; -const JointPPosZ: JointVars = 7; -const JointPQuatX: JointVars = 8; -const JointPQuatY: JointVars = 9; -const JointPQuatZ: JointVars = 10; -const JointPQuatW: JointVars = 11; -const JointCPosX: JointVars = 12; -const JointCPosY: JointVars = 13; -const JointCPosZ: JointVars = 14; -const JointCQuatX: JointVars = 15; -const JointCQuatY: JointVars = 16; -const JointCQuatZ: JointVars = 17; -const JointCQuatW: JointVars = 18; -const JointLinearDoFN: JointVars = 19; -const JointAngularDoFN: JointVars = 20; -const JointDoF1: JointVars = 21; -const JointDoF2: JointVars = 22; -const JointDoF3: JointVars = 23; -const JointDoF4: JointVars = 24; -const JointDoF5: JointVars = 25; -const JointDoF6: JointVars = 26; -const JointPForceX: JointVars = 27; -const JointPForceY: JointVars = 28; -const JointPForceZ: JointVars = 29; -const JointPTorqueX: JointVars = 30; -const JointPTorqueY: JointVars = 31; -const JointPTorqueZ: JointVars = 32; -const JointCForceX: JointVars = 33; -const JointCForceY: JointVars = 34; -const JointCForceZ: JointVars = 35; -const JointCTorqueX: JointVars = 36; -const JointCTorqueY: JointVars = 37; -const JointCTorqueZ: JointVars = 38; -const JointLinLambdaX: JointVars = 39; -const JointLinLambdaY: JointVars = 40; -const JointLinLambdaZ: JointVars = 41; -const JointAngLambdaX: JointVars = 42; -const JointAngLambdaY: JointVars = 43; -const JointAngLambdaZ: JointVars = 44; +const JointNoLinearRotation: JointVars = 3; +const JointParent: JointVars = 4; +const JointChild: JointVars = 5; +const JointPPosX: JointVars = 6; +const JointPPosY: JointVars = 7; +const JointPPosZ: JointVars = 8; +const JointPQuatX: JointVars = 9; +const JointPQuatY: JointVars = 10; +const JointPQuatZ: JointVars = 11; +const JointPQuatW: JointVars = 12; +const JointCPosX: JointVars = 13; +const JointCPosY: JointVars = 14; +const JointCPosZ: JointVars = 15; +const JointCQuatX: JointVars = 16; +const JointCQuatY: JointVars = 17; +const JointCQuatZ: JointVars = 18; +const JointCQuatW: JointVars = 19; +const JointLinearDoFN: JointVars = 20; +const JointAngularDoFN: JointVars = 21; +const JointDoF1: JointVars = 22; +const JointDoF2: JointVars = 23; +const JointDoF3: JointVars = 24; +const JointDoF4: JointVars = 25; +const JointDoF5: JointVars = 26; +const JointDoF6: JointVars = 27; +const JointPForceX: JointVars = 28; +const JointPForceY: JointVars = 29; +const JointPForceZ: JointVars = 30; +const JointPTorqueX: JointVars = 31; +const JointPTorqueY: JointVars = 32; +const JointPTorqueZ: JointVars = 33; +const JointCForceX: JointVars = 34; +const JointCForceY: JointVars = 35; +const JointCForceZ: JointVars = 36; +const JointCTorqueX: JointVars = 37; +const JointCTorqueY: JointVars = 38; +const JointCTorqueZ: JointVars = 39; +const JointLinLambdaX: JointVars = 40; +const JointLinLambdaY: JointVars = 41; +const JointLinLambdaZ: JointVars = 42; +const JointAngLambdaX: JointVars = 43; +const JointAngLambdaY: JointVars = 44; +const JointAngLambdaZ: JointVars = 45; fn GetJointType(idx: i32) -> JointTypes { return JointTypes(bitcast(Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointType))])); } @@ -285,6 +286,10 @@ fn GetJointParentFixed(idx: i32) -> bool { var je = bitcast(Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointParentFixed))]); return je != 0; } +fn GetJointNoLinearRotation(idx: i32) -> bool { + var je = bitcast(Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointNoLinearRotation))]); +return je != 0; +} fn JointParentIndex(idx: i32) -> i32 { return i32(bitcast(Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointParent))])); } @@ -637,6 +642,7 @@ fn StepSolveJoint(ji: i32) { } var jCi = JointChildIndex(ji); var jCbi = DynamicBody(jCi); + var noLinearRot = GetJointNoLinearRotation(ji); var jLinearN = GetJointLinearDoFN(ji); var jPR = JointPPos(ji); var jPQ = JointPQuat(ji); @@ -723,9 +729,11 @@ fn StepSolveJoint(ji: i32) { var dLambda = PositionalCorrection(err, derr, posePQ, poseCQ, mInvP, mInvC, iInvP, iInvC, linearP, linearC, angularP, angularC, lambdaIn, compliance, kd, params.Dt); linDeltaP = linDeltaP+(linearP*(dLambda * params.JointLinearRelax)); - angDeltaP = angDeltaP+(angularP*(dLambda * params.JointAngularRelax)); linDeltaC = linDeltaC+(linearC*(dLambda * params.JointLinearRelax)); - angDeltaC = angDeltaC+(angularC*(dLambda * params.JointAngularRelax)); + if (!noLinearRot) { + angDeltaP = angDeltaP+(angularP*(dLambda * params.JointAngularRelax)); + angDeltaC = angDeltaC+(angularC*(dLambda * params.JointAngularRelax)); + } } } else { var axisLimitsD: vec3; @@ -790,13 +798,15 @@ fn StepSolveJoint(ji: i32) { } } if (abs(err) > 1e-9 || abs(derrRel) > 1e-9) { - var lambdaIn = Dim3(lambdaPrev, dim); + var lambdaIn = f32(0); var dLambda = PositionalCorrection(err, derrRel, posePQ, poseCQ, mInvP, mInvC, iInvP, iInvC, linearP, linearC, angularP, angularC, lambdaIn, compliance, damping, params.Dt); linDeltaP = linDeltaP+(linearP*(dLambda * params.JointLinearRelax)); linDeltaC = linDeltaC+(linearC*(dLambda * params.JointLinearRelax)); - angDeltaP = angDeltaP+(angularP*(dLambda * params.JointAngularRelax)); - angDeltaC = angDeltaC+(angularC*(dLambda * params.JointAngularRelax)); + if (!noLinearRot) { + angDeltaP = angDeltaP+(angularP*(dLambda * params.JointAngularRelax)); + angDeltaC = angDeltaC+(angularC*(dLambda * params.JointAngularRelax)); + } lambdaNext = SetDim3(lambdaNext, dim, dLambda); } } @@ -856,6 +866,7 @@ fn StepSolveJoint(ji: i32) { var axisTargetVelKdA: vec3; lambdaPrev = JointAngLambda(ji); lambdaNext = vec3(0, 0, 0); + _ = lambdaPrev; for (var dof=0; dof 1e-9 || math32.Abs(derrRel) > 1e-9 { - lambdaIn := slmath.Dim3(lambdaPrev, dim) - // lambdaIn := float32(0) + // lambdaIn := slmath.Dim3(lambdaPrev, dim) + lambdaIn := float32(0) dLambda := PositionalCorrection(err, derrRel, posePQ, poseCQ, mInvP, mInvC, iInvP, iInvC, linearP, linearC, angularP, angularC, lambdaIn, compliance, damping, params.Dt) linDeltaP = linDeltaP.Add(linearP.MulScalar(dLambda * params.JointLinearRelax)) linDeltaC = linDeltaC.Add(linearC.MulScalar(dLambda * params.JointLinearRelax)) - angDeltaP = angDeltaP.Add(angularP.MulScalar(dLambda * params.JointAngularRelax)) - angDeltaC = angDeltaC.Add(angularC.MulScalar(dLambda * params.JointAngularRelax)) + if !noLinearRot { + angDeltaP = angDeltaP.Add(angularP.MulScalar(dLambda * params.JointAngularRelax)) + angDeltaC = angDeltaC.Add(angularC.MulScalar(dLambda * params.JointAngularRelax)) + } lambdaNext = slmath.SetDim3(lambdaNext, dim, dLambda) - // if angularP.Length() > 0.001 && dLambda > 0.001 { - // fmt.Println("lin:", angularP.Length(), dLambda, angularP, angDeltaP, dP, linearC, angularC.Length()) - // } } } } SetJointLinLambda(ji, lambdaNext) - //////// Angular positions + //////// Angular DoFs jAngularN := GetJointAngularDoFN(ji) @@ -415,6 +417,7 @@ func StepSolveJoint(ji int32) { var axisTargetVelKdD, axisTargetVelKdA math32.Vector3 lambdaPrev = JointAngLambda(ji) lambdaNext = math32.Vec3(0, 0, 0) + _ = lambdaPrev for dof := range jAngularN { di := dof + jLinearN @@ -486,7 +489,8 @@ func StepSolveJoint(ji int32) { // fmt.Println(targetPos, e, err) // } } - lambdaIn := slmath.Dim3(lambdaPrev, dim) + // lambdaIn := slmath.Dim3(lambdaPrev, dim) + lambdaIn := float32(0) dLambda := AngularCorrection(err, derrRel, posePQ, poseCQ, iInvP, iInvC, angularP, angularC, lambdaIn, compliance, damping, params.Dt) // note: no relaxation factors here: diff --git a/physics/step_joint.goal b/physics/step_joint.goal index 0fab6daa..1f96acaf 100644 --- a/physics/step_joint.goal +++ b/physics/step_joint.goal @@ -147,6 +147,7 @@ func StepSolveJoint(ji int32) { } jCi := JointChildIndex(ji) jCbi := DynamicBody(jCi) + noLinearRot := GetJointNoLinearRotation(ji) jLinearN := GetJointLinearDoFN(ji) // jAngularN := GetJointAngularDoFN(ji) @@ -248,9 +249,11 @@ func StepSolveJoint(ji int32) { dLambda := PositionalCorrection(err, derr, posePQ, poseCQ, mInvP, mInvC, iInvP, iInvC, linearP, linearC, angularP, angularC, lambdaIn, compliance, kd, params.Dt) linDeltaP = linDeltaP.Add(linearP.MulScalar(dLambda * params.JointLinearRelax)) - angDeltaP = angDeltaP.Add(angularP.MulScalar(dLambda * params.JointAngularRelax)) linDeltaC = linDeltaC.Add(linearC.MulScalar(dLambda * params.JointLinearRelax)) - angDeltaC = angDeltaC.Add(angularC.MulScalar(dLambda * params.JointAngularRelax)) + if !noLinearRot { + angDeltaP = angDeltaP.Add(angularP.MulScalar(dLambda * params.JointAngularRelax)) + angDeltaC = angDeltaC.Add(angularC.MulScalar(dLambda * params.JointAngularRelax)) + } } } else { // all joints impose linear constraints! @@ -326,26 +329,25 @@ func StepSolveJoint(ji int32) { } } if math32.Abs(err) > 1e-9 || math32.Abs(derrRel) > 1e-9 { - lambdaIn := slmath.Dim3(lambdaPrev, dim) - // lambdaIn := float32(0) + // lambdaIn := slmath.Dim3(lambdaPrev, dim) + lambdaIn := float32(0) dLambda := PositionalCorrection(err, derrRel, posePQ, poseCQ, mInvP, mInvC, iInvP, iInvC, linearP, linearC, angularP, angularC, lambdaIn, compliance, damping, params.Dt) linDeltaP = linDeltaP.Add(linearP.MulScalar(dLambda * params.JointLinearRelax)) linDeltaC = linDeltaC.Add(linearC.MulScalar(dLambda * params.JointLinearRelax)) - angDeltaP = angDeltaP.Add(angularP.MulScalar(dLambda * params.JointAngularRelax)) - angDeltaC = angDeltaC.Add(angularC.MulScalar(dLambda * params.JointAngularRelax)) + if !noLinearRot { + angDeltaP = angDeltaP.Add(angularP.MulScalar(dLambda * params.JointAngularRelax)) + angDeltaC = angDeltaC.Add(angularC.MulScalar(dLambda * params.JointAngularRelax)) + } lambdaNext = slmath.SetDim3(lambdaNext, dim, dLambda) - // if angularP.Length() > 0.001 && dLambda > 0.001 { - // fmt.Println("lin:", angularP.Length(), dLambda, angularP, angDeltaP, dP, linearC, angularC.Length()) - // } } } } SetJointLinLambda(ji, lambdaNext) - //////// Angular positions + //////// Angular DoFs jAngularN := GetJointAngularDoFN(ji) @@ -413,6 +415,7 @@ func StepSolveJoint(ji int32) { var axisTargetVelKdD, axisTargetVelKdA math32.Vector3 lambdaPrev = JointAngLambda(ji) lambdaNext = math32.Vec3(0,0,0) + _ = lambdaPrev for dof := range jAngularN { di := dof + jLinearN @@ -484,7 +487,8 @@ func StepSolveJoint(ji int32) { // fmt.Println(targetPos, e, err) // } } - lambdaIn := slmath.Dim3(lambdaPrev, dim) + // lambdaIn := slmath.Dim3(lambdaPrev, dim) + lambdaIn := float32(0) dLambda := AngularCorrection(err, derrRel, posePQ, poseCQ, iInvP, iInvC, angularP, angularC, lambdaIn, compliance, damping, params.Dt) // note: no relaxation factors here: diff --git a/physics/typegen.go b/physics/typegen.go index fc93f6d0..49ed7691 100644 --- a/physics/typegen.go +++ b/physics/typegen.go @@ -24,7 +24,7 @@ var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.JointDoFVars var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.Model", IDName: "model", Doc: "Model contains and manages all of the physics elements.", Directives: []types.Directive{{Tool: "go", Directive: "generate", Args: []string{"core", "generate", "-add-types", "-gosl"}}}, Fields: []types.Field{{Name: "GPU", Doc: "GPU determines whether to use GPU (else CPU)."}, {Name: "Params", Doc: "Params are global parameters."}, {Name: "CurrentWorld", Doc: "CurrentWorld is the [BodyWorld] value to use when creating new bodies.\nSet to -1 to create global elements that interact with everything,\nwhile 0 and positive numbers only interact amongst themselves."}, {Name: "CurrentObject", Doc: "CurrentObject is the Object to use when creating new joints.\nCall NewObject to increment."}, {Name: "CurrentObjectJoint", Doc: "CurrentObjectJoint is the Joint index in CurrentObject\nto use when creating new joints."}, {Name: "ReplicasStart", Doc: "ReplicasStart is the starting body index for replicated world bodies,\nwhich is needed for viewers to efficiently select a specific world to view.\nThis is the start of the World=0 first instance."}, {Name: "ReplicasN", Doc: "ReplicasN is the number of body elements within each set of\nreplicated world bodies, which is needed for viewers to efficiently select\na specific world to view."}, {Name: "Bodies", Doc: "Bodies are the rigid body elements (dynamic and static),\nspecifying the constant, non-dynamic properties,\nwhich is initial state for dynamics.\n[body][BodyVarsN]"}, {Name: "Objects", Doc: "Objects is a list of joint indexes for each object, where each object\ncontains all the joints interconnecting an overlapping set of bodies.\nThis is known as an articulation in other physics software.\nJoints must be added in parent -> child order within objects, as joints\nare updated in sequential order within object.\n[object][MaxObjectJoints+1]"}, {Name: "BodyJoints", Doc: "BodyJoints is a list of joint indexes for each dynamic body, for aggregating.\n[dyn body][parent, child][Params.BodyJointsMax]"}, {Name: "Joints", Doc: "Joints is a list of permanent joints connecting bodies,\nwhich do not change (no dynamic variables).\n[joint][JointVarsN]"}, {Name: "JointDoFs", Doc: "JointDoFs is a list of joint DoF parameters, allocated per joint.\n[dof][JointDoFVars]"}, {Name: "BodyCollidePairs", Doc: "BodyCollidePairs are pairs of Body indexes that could potentially collide\nbased on precomputed collision logic, using World, Group, and Joint indexes.\n[BodyCollidePairsN][2]"}, {Name: "Dynamics", Doc: "Dynamics are the dynamic rigid body elements: these actually move.\nThe first set of variables are for initial values, and the second current.\n[body][cur/next][DynamicVarsN]"}, {Name: "BroadContactsN", Doc: "BroadContactsN has number of points of broad contact\nbetween bodies. [1]"}, {Name: "BroadContacts", Doc: "BroadContacts are the results of broad-phase contact processing,\nestablishing possible points of contact between bodies.\n[ContactsMax][BroadContactVarsN]"}, {Name: "ContactsN", Doc: "ContactsN has number of points of narrow (final) contact\nbetween bodies. [1]"}, {Name: "Contacts", Doc: "Contacts are the results of narrow-phase contact processing,\nwhere only actual contacts with fully-specified values are present.\n[ContactsMax][ContactVarsN]"}, {Name: "JointControls", Doc: "JointControls are dynamic joint control inputs, per joint DoF\n(in correspondence with [JointDoFs]). This can be uploaded to the\nGPU at every step.\n[dof][JointControlVarsN]"}}}) -var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.PhysicsParams", IDName: "physics-params", Doc: "PhysicsParams are the physics parameters", Directives: []types.Directive{{Tool: "gosl", Directive: "start"}}, Fields: []types.Field{{Name: "Iterations", Doc: "Iterations is the number of integration iterations to perform\nwithin each solver step. Muller et al (2020) report that 1 is best."}, {Name: "Dt", Doc: "Dt is the integration stepsize.\nFor highly kinetic situations (e.g., rapidly moving bouncing balls)\n0.0001 is needed to ensure contact registration. Use SubSteps to\naccomplish a target effective read-out step size."}, {Name: "SubSteps", Doc: "SubSteps is the number of integration steps to take per Step()\nfunction call. These sub steps are taken without any sync to/from\nthe GPU and are therefore much faster."}, {Name: "ControlDt", Doc: "ControlDt is the stepsize for integrating joint control position values\n[JointTargetPos] over time, to avoid sudden strong changes in force."}, {Name: "ControlDtThr", Doc: "ControlDtThr is the threshold on the control delta above which\nControlDt is used. ControlDt is most important for large changes,\nand can result in under-shoot if engaged for small chages."}, {Name: "ContactMargin", Doc: "Contact margin is the extra distance for broadphase collision\naround rigid bodies. This can make some joints potentially unstable if > 0"}, {Name: "ContactRelax", Doc: "ContactRelax is rigid contact relaxation constant.\nHigher values cause errros"}, {Name: "ContactWeighting", Doc: "Contact weighting: balances contact forces?"}, {Name: "Restitution", Doc: "Restitution takes into account bounciness of objects."}, {Name: "JointLinearRelax", Doc: "JointLinearRelax is joint linear relaxation constant."}, {Name: "JointAngularRelax", Doc: "JointAngularRelax is joint angular relaxation constant."}, {Name: "JointLinearComply", Doc: "JointLinearComply is joint linear compliance constant."}, {Name: "JointAngularComply", Doc: "JointAngularComply is joint angular compliance constant."}, {Name: "AngularDamping", Doc: "AngularDamping is damping of angular motion."}, {Name: "SoftRelax", Doc: "SoftRelax is soft-body relaxation constant."}, {Name: "MaxForce", Doc: "MaxForce is the maximum computed force value, which prevents\nrunaway numerical overflow."}, {Name: "MaxDelta", Doc: "MaxDelta is the maximum computed change in position magnitude,\nwhich prevents runaway numerical overflow."}, {Name: "MaxGeomIter", Doc: "MaxGeomIter is number of iterations to perform in shape-based\ngeometry collision computations"}, {Name: "ContactsMax", Doc: "Maximum number of contacts to process at any given point."}, {Name: "Cur", Doc: "Index for the current state (0 or 1, alternates with Next)."}, {Name: "Next", Doc: "Index for the next state (1 or 0, alternates with Cur)."}, {Name: "BodiesN", Doc: "BodiesN is number of rigid bodies."}, {Name: "DynamicsN", Doc: "DynamicsN is number of dynamics bodies."}, {Name: "ObjectsN", Doc: "ObjectsN is number of objects."}, {Name: "MaxObjectJoints", Doc: "MaxObjectJoints is max number of joints per object."}, {Name: "JointsN", Doc: "JointsN is number of joints."}, {Name: "JointDoFsN", Doc: "JointDoFsN is number of joint DoFs."}, {Name: "BodyJointsMax", Doc: "BodyJointsMax is max number of joints per body + 1 for actual n."}, {Name: "BodyCollidePairsN", Doc: "BodyCollidePairsN is the total number of pre-compiled collision pairs\nto examine."}, {Name: "pad"}, {Name: "pad1"}, {Name: "pad2"}, {Name: "Gravity", Doc: "Gravity is the gravity acceleration function"}}}) +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.PhysicsParams", IDName: "physics-params", Doc: "PhysicsParams are the physics parameters", Directives: []types.Directive{{Tool: "gosl", Directive: "start"}}, Fields: []types.Field{{Name: "Iterations", Doc: "Iterations is the number of integration iterations to perform\nwithin each solver step. Muller et al (2020) report that 1 is best."}, {Name: "Dt", Doc: "Dt is the integration stepsize.\nFor highly kinetic situations (e.g., rapidly moving bouncing balls)\n0.0001 is needed to ensure contact registration. Use SubSteps to\naccomplish a target effective read-out step size."}, {Name: "SubSteps", Doc: "SubSteps is the number of integration steps to take per Step()\nfunction call. These sub steps are taken without any sync to/from\nthe GPU and are therefore much faster."}, {Name: "ControlDt", Doc: "ControlDt is the stepsize for integrating joint control position values\n[JointTargetPos] over time, to avoid sudden strong changes in force.\nFor higher-DoF joints (e.g., Ball), this can be important for stability,\nbut it can also result in under-shoot of the target position."}, {Name: "ControlDtThr", Doc: "ControlDtThr is the threshold on the control delta above which\nControlDt is used. ControlDt is most important for large changes,\nand can result in under-shoot if engaged for small changes."}, {Name: "ContactMargin", Doc: "Contact margin is the extra distance for broadphase collision\naround rigid bodies. This can make some joints potentially unstable if > 0"}, {Name: "ContactRelax", Doc: "ContactRelax is rigid contact relaxation constant.\nHigher values cause errros"}, {Name: "ContactWeighting", Doc: "Contact weighting: balances contact forces?"}, {Name: "Restitution", Doc: "Restitution takes into account bounciness of objects."}, {Name: "JointLinearRelax", Doc: "JointLinearRelax is joint linear relaxation constant."}, {Name: "JointAngularRelax", Doc: "JointAngularRelax is joint angular relaxation constant."}, {Name: "JointLinearComply", Doc: "JointLinearComply is joint linear compliance constant."}, {Name: "JointAngularComply", Doc: "JointAngularComply is joint angular compliance constant."}, {Name: "AngularDamping", Doc: "AngularDamping is damping of angular motion."}, {Name: "SoftRelax", Doc: "SoftRelax is soft-body relaxation constant."}, {Name: "MaxForce", Doc: "MaxForce is the maximum computed force value, which prevents\nrunaway numerical overflow."}, {Name: "MaxDelta", Doc: "MaxDelta is the maximum computed change in position magnitude,\nwhich prevents runaway numerical overflow."}, {Name: "MaxGeomIter", Doc: "MaxGeomIter is number of iterations to perform in shape-based\ngeometry collision computations"}, {Name: "ContactsMax", Doc: "Maximum number of contacts to process at any given point."}, {Name: "Cur", Doc: "Index for the current state (0 or 1, alternates with Next)."}, {Name: "Next", Doc: "Index for the next state (1 or 0, alternates with Cur)."}, {Name: "BodiesN", Doc: "BodiesN is number of rigid bodies."}, {Name: "DynamicsN", Doc: "DynamicsN is number of dynamics bodies."}, {Name: "ObjectsN", Doc: "ObjectsN is number of objects."}, {Name: "MaxObjectJoints", Doc: "MaxObjectJoints is max number of joints per object."}, {Name: "JointsN", Doc: "JointsN is number of joints."}, {Name: "JointDoFsN", Doc: "JointDoFsN is number of joint DoFs."}, {Name: "BodyJointsMax", Doc: "BodyJointsMax is max number of joints per body + 1 for actual n."}, {Name: "BodyCollidePairsN", Doc: "BodyCollidePairsN is the total number of pre-compiled collision pairs\nto examine."}, {Name: "pad"}, {Name: "pad1"}, {Name: "pad2"}, {Name: "Gravity", Doc: "Gravity is the gravity acceleration function"}}}) var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.GeomData", IDName: "geom-data", Doc: "GeomData contains all geometric data for narrow-phase collision.", Directives: []types.Directive{{Tool: "gosl", Directive: "start"}}, Fields: []types.Field{{Name: "BodyIdx"}, {Name: "Shape"}, {Name: "MinSize", Doc: "MinSize is the min of the Size dimensions."}, {Name: "Thick", Doc: "Thickness of shape."}, {Name: "Radius", Doc: "Radius is the effective radius for sphere-like elements (Sphere, Capsule, Cone)"}, {Name: "Size"}, {Name: "WbR", Doc: "World-to-Body transform\nPosition (R) (i.e., BodyPos)"}, {Name: "WbQ", Doc: "Quaternion (Q) (i.e., BodyQuat)"}, {Name: "BwR", Doc: "Body-to-World transform (inverse)\nPosition (R)"}, {Name: "BwQ", Doc: "Quaternion (Q)"}}}) From 507399b953a0359366ac0de39a7b1fcaf5f1c625 Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Sun, 4 Jan 2026 12:53:19 -0800 Subject: [PATCH 77/97] physics: builder uses pointer objects in slices -- otherwise pointers change and can't be cached. usual lesson: just always use the pointers here -- value types are only for really basic values. --- physics/builder/body.go | 9 +++- physics/builder/builder.go | 30 +++++------ physics/builder/joint.go | 71 ++++++++++++++++++--------- physics/builder/object.go | 22 +++++---- physics/builder/typegen.go | 24 ++++++--- physics/builder/world.go | 11 +++-- physics/examples/balls/balls.go | 1 + physics/examples/virtroom/virtroom.go | 1 + physics/joint.go | 1 + physics/joint.goal | 1 + 10 files changed, 107 insertions(+), 64 deletions(-) diff --git a/physics/builder/body.go b/physics/builder/body.go index 9cb9386a..04ab0074 100644 --- a/physics/builder/body.go +++ b/physics/builder/body.go @@ -85,11 +85,11 @@ type Body struct { // Use this for Static elements; NewDynamic for dynamic elements. func (ob *Object) NewBody(shape physics.Shapes, hsize, pos math32.Vector3, rot math32.Quat) *Body { idx := len(ob.Bodies) - bd := Body{ObjectIndex: idx, Shape: shape, HSize: hsize} + bd := &Body{ObjectIndex: idx, Shape: shape, HSize: hsize} bd.Pose.Pos = pos bd.Pose.Quat = rot ob.Bodies = append(ob.Bodies, bd) - return &(ob.Bodies[idx]) + return ob.Bodies[idx] } // NewDynamic adds a new dynamic body with given parameters. @@ -129,6 +129,11 @@ func (ob *Object) NewDynamicSkin(sc *phyxyz.Scene, name string, shape physics.Sh return bd } +func (bd *Body) Copy(sb *Body) { + *bd = *sb + bd.Skin = nil // skins are unique +} + /////// Physics functions func (bd *Body) NewPhysicsBody(ml *physics.Model, world int) { diff --git a/physics/builder/builder.go b/physics/builder/builder.go index 6de77281..4e23b6bd 100644 --- a/physics/builder/builder.go +++ b/physics/builder/builder.go @@ -16,7 +16,7 @@ import ( // organized into worlds that are independently updated. type Builder struct { // Worlds are the independent world elements. - Worlds []World + Worlds []*World // ReplicasStart is the starting Worlds index for replicated world bodies. // Set by ReplicateWorld, and used to set corresponding value in Model. @@ -37,15 +37,15 @@ func (bl *Builder) Reset() { } func (bl *Builder) World(idx int) *World { - return &bl.Worlds[idx] + return bl.Worlds[idx] } // NewGlobalWorld creates a new world with World index = -1, // which are globals that collide with all worlds. func (bl *Builder) NewGlobalWorld() *World { idx := len(bl.Worlds) - bl.Worlds = append(bl.Worlds, World{World: -1}) - return &bl.Worlds[idx] + bl.Worlds = append(bl.Worlds, &World{World: -1}) + return bl.Worlds[idx] } // NewWorld creates a new standard (non-global) world, with @@ -56,8 +56,8 @@ func (bl *Builder) NewWorld() *World { if idx > 0 { wn = bl.Worlds[idx-1].World + 1 } - bl.Worlds = append(bl.Worlds, World{World: wn}) - return &bl.Worlds[idx] + bl.Worlds = append(bl.Worlds, &World{World: wn}) + return bl.Worlds[idx] } // Build builds a physics model, with optional [phyxyz.Scene] for @@ -65,14 +65,11 @@ func (bl *Builder) NewWorld() *World { func (bl *Builder) Build(ml *physics.Model, sc *phyxyz.Scene) { repSt := int32(0) repN := int32(0) - for wi := range bl.Worlds { - wl := bl.World(wi) + for wi, wl := range bl.Worlds { // fmt.Println("\n######## World:", wl.World) - for oi := range wl.Objects { - ob := wl.Object(oi) + for _, ob := range wl.Objects { // fmt.Println("\n\t#### Object") - for bbi := range ob.Bodies { - bd := ob.Body(bbi) + for bbi, bd := range ob.Bodies { bd.NewPhysicsBody(ml, wl.World) if bl.ReplicasN > 0 && wi == bl.ReplicasStart { repN++ @@ -85,8 +82,7 @@ func (bl *Builder) Build(ml *physics.Model, sc *phyxyz.Scene) { continue } ml.NewObject() - for bji := range ob.Joints { - jd := ob.Joint(bji) + for _, jd := range ob.Joints { jd.NewPhysicsJoint(ml, ob) } } @@ -101,10 +97,8 @@ func (bl *Builder) Build(ml *physics.Model, sc *phyxyz.Scene) { // This does not call InitState in physics, because that depends on // whether the Sccene is being used. func (bl *Builder) InitState() { - for wi := range bl.Worlds { - wl := bl.World(wi) - for oi := range wl.Objects { - ob := wl.Object(oi) + for _, wl := range bl.Worlds { + for _, ob := range wl.Objects { ob.InitState() } } diff --git a/physics/builder/joint.go b/physics/builder/joint.go index 748f0971..ce3528f6 100644 --- a/physics/builder/joint.go +++ b/physics/builder/joint.go @@ -33,6 +33,13 @@ type Joint struct { // ParentFixed does not update the parent side of the joint. ParentFixed bool + // NoLinearRotation ignores the rotational (angular) effects of + // linear joint position constraints (i.e., Coriolis and centrifugal forces) + // which can otherwise interfere with rotational position constraints in + // joints with both linear and angular DoFs + // (e.g., [PlaneXZ], for which this is on by default). + NoLinearRotation bool + // LinearDoFN is the number of linear degrees of freedom (3 max). LinearDoFN int @@ -40,7 +47,7 @@ type Joint struct { AngularDoFN int // DoFs are the degrees-of-freedom for this joint. - DoFs []DoF + DoFs []*DoF // JointIndex is the index of this joint in [physics.Joints] when built. JointIndex int32 @@ -103,7 +110,20 @@ func (df *DoF) InitState() { } func (jd *Joint) DoF(idx int) *DoF { - return &jd.DoFs[idx] + return jd.DoFs[idx] +} + +func (jd *Joint) Copy(sj *Joint) { + *jd = *sj + jd.DoFs = make([]*DoF, len(sj.DoFs)) + for i := range jd.DoFs { + jd.DoFs[i] = &DoF{} + jd.DoF(i).Copy(sj.DoF(i)) + } +} + +func (df *DoF) Copy(sd *DoF) { + *df = *sd } // newJoint adds a new joint of given type. @@ -113,7 +133,7 @@ func (ob *Object) newJoint(typ physics.JointTypes, parent, child *Body, ppos, cp pidx = parent.ObjectIndex } idx := len(ob.Joints) - ob.Joints = append(ob.Joints, Joint{Parent: pidx, Child: child.ObjectIndex, Type: typ, LinearDoFN: linDoF, AngularDoFN: angDoF}) + ob.Joints = append(ob.Joints, &Joint{Parent: pidx, Child: child.ObjectIndex, Type: typ, LinearDoFN: linDoF, AngularDoFN: angDoF}) jd := ob.Joint(idx) jd.PPose.Pos = ppos jd.PPose.Quat = math32.NewQuatIdentity() @@ -121,9 +141,10 @@ func (ob *Object) newJoint(typ physics.JointTypes, parent, child *Body, ppos, cp jd.CPose.Quat = math32.NewQuatIdentity() ndof := linDoF + angDoF if ndof > 0 { - jd.DoFs = make([]DoF, linDoF+angDoF) + jd.DoFs = make([]*DoF, linDoF+angDoF) for i := range ndof { - dof := jd.DoF(i) + dof := &DoF{} + jd.DoFs[i] = dof dof.Defaults() } } @@ -137,7 +158,9 @@ func (ob *Object) newJoint(typ physics.JointTypes, parent, child *Body, ppos, cp // to these positions as well). // Sets relative rotation matricies to identity by default. func (ob *Object) NewJointFixed(parent, child *Body, ppos, cpos math32.Vector3) *Joint { - return ob.newJoint(physics.Fixed, parent, child, ppos, cpos, 0, 0) + jd := ob.newJoint(physics.Fixed, parent, child, ppos, cpos, 0, 0) + jd.NoLinearRotation = true + return jd } // NewJointPrismatic adds a new Prismatic (slider) joint as a child @@ -149,9 +172,9 @@ func (ob *Object) NewJointFixed(parent, child *Body, ppos, cpos math32.Vector3) // axis is the axis of articulation for the joint. // Use [SetJointDoF] to set the remaining DoF parameters. func (ob *Object) NewJointPrismatic(parent, child *Body, ppos, cpos, axis math32.Vector3) *Joint { - jt := ob.newJoint(physics.Prismatic, parent, child, ppos, cpos, 1, 0) - jt.DoFs[0].Axis = axis - return jt + jd := ob.newJoint(physics.Prismatic, parent, child, ppos, cpos, 1, 0) + jd.DoFs[0].Axis = axis + return jd } // NewJointRevolute adds a new Revolute (hinge, axel) joint as a child @@ -163,9 +186,9 @@ func (ob *Object) NewJointPrismatic(parent, child *Body, ppos, cpos, axis math32 // axis is the axis of articulation for the joint. // Use [SetJointDoF] to set the remaining DoF parameters. func (ob *Object) NewJointRevolute(parent, child *Body, ppos, cpos, axis math32.Vector3) *Joint { - jt := ob.newJoint(physics.Revolute, parent, child, ppos, cpos, 0, 1) - jt.DoFs[0].Axis = axis - return jt + jd := ob.newJoint(physics.Revolute, parent, child, ppos, cpos, 0, 1) + jd.DoFs[0].Axis = axis + return jd } // NewJointBall adds a new Ball joint (3 angular DoF) as a child @@ -176,8 +199,8 @@ func (ob *Object) NewJointRevolute(parent, child *Body, ppos, cpos, axis math32. // Sets relative rotation matricies to identity by default. // Use [SetJointDoF] to set the remaining DoF parameters. func (ob *Object) NewJointBall(parent, child *Body, ppos, cpos math32.Vector3) *Joint { - jt := ob.newJoint(physics.Ball, parent, child, ppos, cpos, 0, 3) - return jt + jd := ob.newJoint(physics.Ball, parent, child, ppos, cpos, 0, 3) + return jd } // NewJointDistance adds a new Distance joint (6 DoF), @@ -189,10 +212,10 @@ func (ob *Object) NewJointBall(parent, child *Body, ppos, cpos math32.Vector3) * // Sets relative rotation matricies to identity by default. // Use [SetJointDoF] to set the remaining DoF parameters. func (ob *Object) NewJointDistance(parent, child *Body, ppos, cpos math32.Vector3, minDist, maxDist float32) *Joint { - jt := ob.newJoint(physics.Ball, parent, child, ppos, cpos, 3, 3) - jt.DoFs[0].Limit.Min = minDist - jt.DoFs[0].Limit.Max = maxDist - return jt + jd := ob.newJoint(physics.Ball, parent, child, ppos, cpos, 3, 3) + jd.DoFs[0].Limit.Min = minDist + jd.DoFs[0].Limit.Max = maxDist + return jd } // NewJointFree adds a new Free joint as a child @@ -203,8 +226,8 @@ func (ob *Object) NewJointDistance(parent, child *Body, ppos, cpos math32.Vector // Sets relative rotation matricies to identity by default. // Use [SetJointDoF] to set the remaining DoF parameters. func (ob *Object) NewJointFree(parent, child *Body, ppos, cpos math32.Vector3) *Joint { - jt := ob.newJoint(physics.Free, parent, child, ppos, cpos, 0, 0) - return jt + jd := ob.newJoint(physics.Free, parent, child, ppos, cpos, 0, 0) + return jd } // NewJointPlaneXZ adds a new 3 DoF Planar motion joint suitable for @@ -216,8 +239,9 @@ func (ob *Object) NewJointFree(parent, child *Body, ppos, cpos math32.Vector3) * // Sets relative rotation matricies to identity by default. // Use [SetJointDoF] to set the remaining DoF parameters. func (ob *Object) NewJointPlaneXZ(parent, child *Body, ppos, cpos math32.Vector3) *Joint { - jt := ob.newJoint(physics.PlaneXZ, parent, child, ppos, cpos, 2, 1) - return jt + jd := ob.newJoint(physics.PlaneXZ, parent, child, ppos, cpos, 2, 1) + jd.NoLinearRotation = true + return jd } // NewPhysicsJoint makes the physics joint for joint @@ -248,6 +272,7 @@ func (jd *Joint) NewPhysicsJoint(ml *physics.Model, ob *Object) int32 { ji = ml.NewJointPlaneXZ(pdi, cdi, jd.PPose.Pos, jd.CPose.Pos) } physics.SetJointParentFixed(ji, jd.ParentFixed) + physics.SetJointNoLinearRotation(ji, jd.NoLinearRotation) for i := range jd.LinearDoFN { d := jd.DoF(i) di := int32(i) @@ -267,7 +292,7 @@ func (jd *Joint) NewPhysicsJoint(ml *physics.Model, ob *Object) int32 { d.Axis = physics.JointAxis(ji, di) } jd.JointIndex = ji - // fmt.Println("\t\tjoint:", pdi, cdi, jd.Type) + // fmt.Printf("\tjoint: %p %d\n", jd, jd.JointIndex) // if pdi < 0 { // fmt.Println("\t\t\t", jd.PPose.Pos) // } diff --git a/physics/builder/object.go b/physics/builder/object.go index 028c0f40..febf87a0 100644 --- a/physics/builder/object.go +++ b/physics/builder/object.go @@ -15,32 +15,34 @@ import ( // for positioning elements; has no physical implications. type Object struct { // Bodies are the bodies in the object. - Bodies []Body + Bodies []*Body // Joints are joints connecting object bodies. // Joint indexes here refer strictly within bodies. - Joints []Joint + Joints []*Joint } func (ob *Object) Body(idx int) *Body { - return &ob.Bodies[idx] + return ob.Bodies[idx] } func (ob *Object) Joint(idx int) *Joint { - return &ob.Joints[idx] + return ob.Joints[idx] } // Copy copies all bodies and joints from given source world into this one. // (The objects will be identical after, regardless of current starting // condition). func (ob *Object) Copy(so *Object) { - ob.Bodies = make([]Body, len(so.Bodies)) - ob.Joints = make([]Joint, len(so.Joints)) - copy(ob.Bodies, so.Bodies) - copy(ob.Joints, so.Joints) + ob.Bodies = make([]*Body, len(so.Bodies)) + ob.Joints = make([]*Joint, len(so.Joints)) for i := range ob.Bodies { - bd := ob.Body(i) - bd.Skin = nil + ob.Bodies[i] = &Body{} + ob.Body(i).Copy(so.Body(i)) + } + for i := range ob.Joints { + ob.Joints[i] = &Joint{} + ob.Joint(i).Copy(so.Joint(i)) } } diff --git a/physics/builder/typegen.go b/physics/builder/typegen.go index 52a3a3ff..79ec1f0a 100644 --- a/physics/builder/typegen.go +++ b/physics/builder/typegen.go @@ -99,7 +99,7 @@ var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics/builder.Buil // SetWorlds sets the [Builder.Worlds]: // Worlds are the independent world elements. -func (t *Builder) SetWorlds(v ...World) *Builder { t.Worlds = v; return t } +func (t *Builder) SetWorlds(v ...*World) *Builder { t.Worlds = v; return t } // SetReplicasStart sets the [Builder.ReplicasStart]: // ReplicasStart is the starting Worlds index for replicated world bodies. @@ -111,7 +111,7 @@ func (t *Builder) SetReplicasStart(v int) *Builder { t.ReplicasStart = v; return // Set by ReplicateWorld, and used to set corresponding value in Model. func (t *Builder) SetReplicasN(v int) *Builder { t.ReplicasN = v; return t } -var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics/builder.Joint", IDName: "joint", Doc: "Joint describes a joint between two bodies.", Fields: []types.Field{{Name: "Parent", Doc: "Parent is index within an Object for parent body.\n-1 for world-anchored parent."}, {Name: "Child", Doc: "Parent is index within an Object for parent body."}, {Name: "Type", Doc: "Type is the type of the joint."}, {Name: "PPose", Doc: "PPose is the parent position and orientation of the joint\nin the parent's body-centered coordinates."}, {Name: "CPose", Doc: "CPose is the child position and orientation of the joint\nin the parent's body-centered coordinates."}, {Name: "LinearDoFN", Doc: "LinearDoFN is the number of linear degrees of freedom (3 max)."}, {Name: "AngularDoFN", Doc: "AngularDoFN is the number of linear degrees of freedom (3 max)."}, {Name: "DoFs", Doc: "DoFs are the degrees-of-freedom for this joint."}, {Name: "JointIndex", Doc: "JointIndex is the index of this joint in [physics.Joints] when built."}}}) +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics/builder.Joint", IDName: "joint", Doc: "Joint describes a joint between two bodies.", Fields: []types.Field{{Name: "Parent", Doc: "Parent is index within an Object for parent body.\n-1 for world-anchored parent."}, {Name: "Child", Doc: "Parent is index within an Object for parent body."}, {Name: "Type", Doc: "Type is the type of the joint."}, {Name: "PPose", Doc: "PPose is the parent position and orientation of the joint\nin the parent's body-centered coordinates."}, {Name: "CPose", Doc: "CPose is the child position and orientation of the joint\nin the parent's body-centered coordinates."}, {Name: "ParentFixed", Doc: "ParentFixed does not update the parent side of the joint."}, {Name: "NoLinearRotation", Doc: "NoLinearRotation ignores the rotational (angular) effects of\nlinear joint position constraints (i.e., Coriolis and centrifugal forces)\nwhich can otherwise interfere with rotational position constraints in\njoints with both linear and angular DoFs\n(e.g., [PlaneXZ], for which this is on by default)."}, {Name: "LinearDoFN", Doc: "LinearDoFN is the number of linear degrees of freedom (3 max)."}, {Name: "AngularDoFN", Doc: "AngularDoFN is the number of linear degrees of freedom (3 max)."}, {Name: "DoFs", Doc: "DoFs are the degrees-of-freedom for this joint."}, {Name: "JointIndex", Doc: "JointIndex is the index of this joint in [physics.Joints] when built."}}}) // SetParent sets the [Joint.Parent]: // Parent is index within an Object for parent body. @@ -136,6 +136,18 @@ func (t *Joint) SetPPose(v Pose) *Joint { t.PPose = v; return t } // in the parent's body-centered coordinates. func (t *Joint) SetCPose(v Pose) *Joint { t.CPose = v; return t } +// SetParentFixed sets the [Joint.ParentFixed]: +// ParentFixed does not update the parent side of the joint. +func (t *Joint) SetParentFixed(v bool) *Joint { t.ParentFixed = v; return t } + +// SetNoLinearRotation sets the [Joint.NoLinearRotation]: +// NoLinearRotation ignores the rotational (angular) effects of +// linear joint position constraints (i.e., Coriolis and centrifugal forces) +// which can otherwise interfere with rotational position constraints in +// joints with both linear and angular DoFs +// (e.g., [PlaneXZ], for which this is on by default). +func (t *Joint) SetNoLinearRotation(v bool) *Joint { t.NoLinearRotation = v; return t } + // SetLinearDoFN sets the [Joint.LinearDoFN]: // LinearDoFN is the number of linear degrees of freedom (3 max). func (t *Joint) SetLinearDoFN(v int) *Joint { t.LinearDoFN = v; return t } @@ -146,7 +158,7 @@ func (t *Joint) SetAngularDoFN(v int) *Joint { t.AngularDoFN = v; return t } // SetDoFs sets the [Joint.DoFs]: // DoFs are the degrees-of-freedom for this joint. -func (t *Joint) SetDoFs(v ...DoF) *Joint { t.DoFs = v; return t } +func (t *Joint) SetDoFs(v ...*DoF) *Joint { t.DoFs = v; return t } // SetJointIndex sets the [Joint.JointIndex]: // JointIndex is the index of this joint in [physics.Joints] when built. @@ -202,12 +214,12 @@ var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics/builder.Obje // SetBodies sets the [Object.Bodies]: // Bodies are the bodies in the object. -func (t *Object) SetBodies(v ...Body) *Object { t.Bodies = v; return t } +func (t *Object) SetBodies(v ...*Body) *Object { t.Bodies = v; return t } // SetJoints sets the [Object.Joints]: // Joints are joints connecting object bodies. // Joint indexes here refer strictly within bodies. -func (t *Object) SetJoints(v ...Joint) *Object { t.Joints = v; return t } +func (t *Object) SetJoints(v ...*Joint) *Object { t.Joints = v; return t } var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics/builder.Physics", IDName: "physics", Doc: "Physics provides a container and manager for the main physics elements:\n[Builder], [physics.Model], and [phyxyz.Scene]. This is helpful for\nmodels used within other apps (e.g., an AI simulation), whereas\n[phyxyz.Editor] provides a standalone GUI interface for testing models.", Fields: []types.Field{{Name: "Model", Doc: "Model has the physics Model."}, {Name: "Builder", Doc: "Builder for configuring the Model."}, {Name: "Scene", Doc: "Scene for visualizing the Model"}}}) @@ -245,4 +257,4 @@ func (t *World) SetWorld(v int) *World { t.World = v; return t } // Each object is a coherent collection of bodies, typically // connected by joints. This is an organizational convenience // for positioning elements; has no physical implications. -func (t *World) SetObjects(v ...Object) *World { t.Objects = v; return t } +func (t *World) SetObjects(v ...*Object) *World { t.Objects = v; return t } diff --git a/physics/builder/world.go b/physics/builder/world.go index 7b081f3e..2dfe82dd 100644 --- a/physics/builder/world.go +++ b/physics/builder/world.go @@ -19,25 +19,26 @@ type World struct { // Each object is a coherent collection of bodies, typically // connected by joints. This is an organizational convenience // for positioning elements; has no physical implications. - Objects []Object + Objects []*Object } func (wl *World) Object(idx int) *Object { - return &wl.Objects[idx] + return wl.Objects[idx] } func (wl *World) NewObject() *Object { idx := len(wl.Objects) - wl.Objects = append(wl.Objects, Object{}) - return &wl.Objects[idx] + wl.Objects = append(wl.Objects, &Object{}) + return wl.Objects[idx] } // Copy copies all objects from given source world into this one. // (The worlds will be identical after, regardless of current starting // condition). func (wl *World) Copy(ow *World) { - wl.Objects = make([]Object, len(ow.Objects)) + wl.Objects = make([]*Object, len(ow.Objects)) for i := range wl.Objects { + wl.Objects[i] = &Object{} wl.Object(i).Copy(ow.Object(i)) } } diff --git a/physics/examples/balls/balls.go b/physics/examples/balls/balls.go index e44ffed4..60b236d3 100644 --- a/physics/examples/balls/balls.go +++ b/physics/examples/balls/balls.go @@ -72,6 +72,7 @@ func main() { ed.SetConfigFunc(func() { ml := ed.Model + ml.Params[0].SubSteps = 100 sc := ed.Scene rot := math32.NewQuatIdentity() sc.NewBody(ml, "floor", physics.Plane, "#D0D0D080", math32.Vec3(0, 0, 0), diff --git a/physics/examples/virtroom/virtroom.go b/physics/examples/virtroom/virtroom.go index 4ffd2bc9..34982f38 100644 --- a/physics/examples/virtroom/virtroom.go +++ b/physics/examples/virtroom/virtroom.go @@ -340,6 +340,7 @@ func (ev *Env) MakeEmer(wl *builder.World, em *Emer, name string) { // em.Neck = obj.NewJointBall(emr, head, math32.Vec3(0, hh, 0), math32.Vec3(0, -headsz, 0)) em.Neck = obj.NewJointRevolute(emr, head, math32.Vec3(0, hh, 0), math32.Vec3(0, -headsz, 0), math32.Vec3(0, 1, 0)) em.Neck.ParentFixed = true + em.Neck.NoLinearRotation = true // obj.NewJointFixed(emr, head, math32.Vec3(0, hh, 0), math32.Vec3(0, -headsz, 0)) eyeoff := math32.Vec3(-headsz*.6, headsz*.1, -(headsz + eyesz*.3)) diff --git a/physics/joint.go b/physics/joint.go index ce9d25dc..63a03971 100644 --- a/physics/joint.go +++ b/physics/joint.go @@ -403,6 +403,7 @@ func (ml *Model) JointDoFDefaults(didx int32) { // Sets relative rotation matricies to identity by default. func (ml *Model) NewJointFixed(parent, child int32, ppos, cpos math32.Vector3) int32 { idx := ml.newJoint(Fixed, parent, child, ppos, cpos) + SetJointNoLinearRotation(idx, true) return idx } diff --git a/physics/joint.goal b/physics/joint.goal index acfda4d0..f79df4c9 100644 --- a/physics/joint.goal +++ b/physics/joint.goal @@ -401,6 +401,7 @@ func (ml *Model) JointDoFDefaults(didx int32) { // Sets relative rotation matricies to identity by default. func (ml *Model) NewJointFixed(parent, child int32, ppos, cpos math32.Vector3) int32 { idx := ml.newJoint(Fixed, parent, child, ppos, cpos) + SetJointNoLinearRotation(idx, true) return idx } From 27a1b84846f8acad4ea43eddfba235f1770099cc Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Sun, 4 Jan 2026 15:04:46 -0800 Subject: [PATCH 78/97] physics: virtroom using collisions -- all good! --- physics/builder/body.go | 4 +++ physics/builder/object.go | 47 +++++++++++++++++---------- physics/builder/world.go | 12 +++---- physics/examples/virtroom/virtroom.go | 43 ++++++++++++------------ physics/model.go | 3 ++ physics/model.goal | 3 ++ physics/shapegeom_test.go | 4 +-- physics/step.go | 3 ++ physics/step.goal | 3 ++ 9 files changed, 73 insertions(+), 49 deletions(-) diff --git a/physics/builder/body.go b/physics/builder/body.go index 04ab0074..e82f42d0 100644 --- a/physics/builder/body.go +++ b/physics/builder/body.go @@ -88,6 +88,7 @@ func (ob *Object) NewBody(shape physics.Shapes, hsize, pos math32.Vector3, rot m bd := &Body{ObjectIndex: idx, Shape: shape, HSize: hsize} bd.Pose.Pos = pos bd.Pose.Quat = rot + bd.Group = -1 // default static ob.Bodies = append(ob.Bodies, bd) return ob.Bodies[idx] } @@ -98,6 +99,7 @@ func (ob *Object) NewDynamic(shape physics.Shapes, mass float32, hsize, pos math bd := ob.NewBody(shape, hsize, pos, rot) bd.Dynamic = true bd.Mass = mass + bd.Group = 1 return bd } @@ -107,6 +109,7 @@ func (ob *Object) NewDynamic(shape physics.Shapes, mass float32, hsize, pos math // Use this for Static elements; NewDynamicSkin for dynamic elements. func (ob *Object) NewBodySkin(sc *phyxyz.Scene, name string, shape physics.Shapes, clr string, hsize, pos math32.Vector3, rot math32.Quat) *Body { bd := ob.NewBody(shape, hsize, pos, rot) + bd.Group = -1 // default static bd.NewSkin(sc, name, clr) return bd } @@ -126,6 +129,7 @@ func (ob *Object) NewDynamicSkin(sc *phyxyz.Scene, name string, shape physics.Sh bd := ob.NewBodySkin(sc, name, shape, clr, hsize, pos, rot) bd.Dynamic = true bd.Mass = mass + bd.Group = 1 return bd } diff --git a/physics/builder/object.go b/physics/builder/object.go index febf87a0..bd0fc246 100644 --- a/physics/builder/object.go +++ b/physics/builder/object.go @@ -5,6 +5,8 @@ package builder import ( + "slices" + "cogentcore.org/core/math32" "cogentcore.org/lab/physics/phyxyz" ) @@ -58,43 +60,53 @@ func (ob *Object) CopySkins(sc *phyxyz.Scene, so *Object) { // InitState initializes current state variables in the object. func (ob *Object) InitState() { - for i := range ob.Joints { - ob.Joint(i).InitState() + for _, jd := range ob.Joints { + jd.InitState() } } +// HasBodyIndex returns true if a body in the object has any of +// given body index(es). +func (ob *Object) HasBodyIndex(bodyIndex ...int32) bool { + for _, bd := range ob.Bodies { + if slices.Contains(bodyIndex, bd.BodyIndex) { + return true + } + } + return false +} + //////// Transforms // PoseToPhysics sets the current body poses to the physics current state. // For Dynamic bodies, sets dynamic state. Also updates world-anchored joints. func (ob *Object) PoseToPhysics() { - for i := range ob.Bodies { - ob.Body(i).PoseToPhysics() + for _, bd := range ob.Bodies { + bd.PoseToPhysics() } - for i := range ob.Joints { - ob.Joint(i).PoseToPhysics() + for _, jd := range ob.Joints { + jd.PoseToPhysics() } } // PoseFromPhysics gets the current body poses from the physics current state. // Also updates world-anchored joints. func (ob *Object) PoseFromPhysics() { - for i := range ob.Bodies { - ob.Body(i).PoseFromPhysics() + for _, bd := range ob.Bodies { + bd.PoseFromPhysics() } - for i := range ob.Joints { - ob.Joint(i).PoseFromPhysics() + for _, jd := range ob.Joints { + jd.PoseFromPhysics() } } // Move applies positional and rotational transforms to all bodies, // and world-anchored joints. func (ob *Object) Move(pos math32.Vector3) { - for i := range ob.Bodies { - ob.Body(i).Pose.Move(pos) + for _, bd := range ob.Bodies { + bd.Pose.Move(pos) } - for i := range ob.Joints { - jd := ob.Joint(i) + for _, jd := range ob.Joints { if jd.IsGlobal() { jd.PPose.Move(pos) } @@ -103,11 +115,10 @@ func (ob *Object) Move(pos math32.Vector3) { // RotateAround rotates around a given point func (ob *Object) RotateAround(rot math32.Quat, around math32.Vector3) { - for i := range ob.Bodies { - ob.Body(i).Pose.RotateAround(rot, around) + for _, bd := range ob.Bodies { + bd.Pose.RotateAround(rot, around) } - for i := range ob.Joints { - jd := ob.Joint(i) + for _, jd := range ob.Joints { if jd.IsGlobal() { jd.PPose.RotateAround(rot, around) } diff --git a/physics/builder/world.go b/physics/builder/world.go index 2dfe82dd..4f3e712a 100644 --- a/physics/builder/world.go +++ b/physics/builder/world.go @@ -46,22 +46,22 @@ func (wl *World) Copy(ow *World) { // CopySkins makes new skins for bodies in world, // based on those in source world, which must be a Copy. func (wl *World) CopySkins(sc *phyxyz.Scene, ow *World) { - for i := range wl.Objects { - wl.Object(i).CopySkins(sc, ow.Object(i)) + for i, ob := range wl.Objects { + ob.CopySkins(sc, ow.Object(i)) } } // Move moves all objects in world by given delta. func (wl *World) Move(delta math32.Vector3) { - for i := range wl.Objects { - wl.Object(i).Move(delta) + for _, ob := range wl.Objects { + ob.Move(delta) } } // PoseToPhysics sets the current body poses to the physics current state. // For Dynamic bodies, sets dynamic state. Also updates world-anchored joints. func (wl *World) PoseToPhysics() { - for i := range wl.Objects { - wl.Object(i).PoseToPhysics() + for _, ob := range wl.Objects { + ob.PoseToPhysics() } } diff --git a/physics/examples/virtroom/virtroom.go b/physics/examples/virtroom/virtroom.go index 34982f38..8b31559f 100644 --- a/physics/examples/virtroom/virtroom.go +++ b/physics/examples/virtroom/virtroom.go @@ -8,6 +8,7 @@ package main import ( "image" + "math/rand/v2" "os" "cogentcore.org/core/base/iox/imagex" @@ -143,6 +144,7 @@ func (ev *Env) MakeWorld(sc *xyz.Scene) { ev.Physics.Model = physics.NewModel() ev.Physics.Builder = builder.NewBuilder() ev.Physics.Model.GPU = false + ev.Physics.Model.GetContacts = true sc.Background = colors.Scheme.Select.Container xyz.NewAmbient(sc, "ambient", 0.3, xyz.DirectSun) @@ -215,27 +217,22 @@ func (ev *Env) UpdateView() { // ModelStep does one step of the physics model. func (ev *Env) ModelStep() { //types:add - // physics.ToGPU(physics.DynamicsVar) ev.Physics.Step(ev.ModelSteps) - // cts := pw.WorldCollide(physics.DynsTopGps) - // ev.Contacts = nil - // for _, cl := range cts { - // if len(cl) > 1 { - // for _, c := range cl { - // if c.A.AsTree().Name == "body" { - // ev.Contacts = cl - // } - // fmt.Printf("A: %v B: %v\n", c.A.AsTree().Name, c.B.AsTree().Name) - // } - // } - // } ev.Emer.Angry = false - // if len(ev.Contacts) > 1 { // turn around - // ev.EmerAngry = true - // fmt.Printf("hit wall: turn around!\n") - // rot := 100.0 + 90.0*rand.Float32() - // ev.Emer.Rel.RotateOnAxis(0, 1, 0, rot) - // } + ctN := physics.ContactsN.Value(0) + for ci := range ctN { + ca := physics.GetContactA(ci) + cb := physics.GetContactB(ci) + if ca == 0 || cb == 0 { // ignore the floor + continue + } + if ev.Emer.Obj.HasBodyIndex(ca, cb) { + ev.Emer.Angry = true + // fmt.Println("hit wall: turn around!") + rot := 100.0 + 90.0*rand.Float32() + ev.Emer.XZ.AddTargetAngle(2, rot, ev.Stiff) + } + } ev.GrabEyeImg() ev.UpdateView() } @@ -321,11 +318,11 @@ func (ev *Env) MakeEmer(wl *builder.World, em *Emer, name string) { // body := physics.NewCapsule(emr, "body", math32.Vec3(0, hh, 0), hh, hw) // body := physics.NewCylinder(emr, "body", math32.Vec3(0, hh, 0), hh, hw) em.XZ = obj.NewJointPlaneXZ(nil, emr, math32.Vec3(0, 0, 0), math32.Vec3(0, -hh, 0)) - emr.Group = 0 // no collide (temporary) + // emr.Group = 0 // no collide (temporary) headPos := math32.Vec3(0, 2*hh+headsz, 0) head := obj.NewDynamicSkin(sc, name+"_head", physics.Box, "tan", mass*.1, math32.Vec3(headsz, headsz, headsz), headPos, rot) - head.Group = 0 + // head.Group = 0 hdsk := head.Skin hdsk.InitSkin = func(sld *xyz.Solid) { hdsk.BoxInit(sld) @@ -345,13 +342,13 @@ func (ev *Env) MakeEmer(wl *builder.World, em *Emer, name string) { eyeoff := math32.Vec3(-headsz*.6, headsz*.1, -(headsz + eyesz*.3)) bd := obj.NewDynamicSkin(sc, name+"_eye-l", physics.Box, "green", mass*.001, math32.Vec3(eyesz, eyesz*.5, eyesz*.2), headPos.Add(eyeoff), rot) - bd.Group = 0 + // bd.Group = 0 ej := obj.NewJointFixed(head, bd, eyeoff, math32.Vec3(0, 0, -eyesz*.3)) ej.ParentFixed = true eyeoff = math32.Vec3(headsz*.6, headsz*.1, -(headsz + eyesz*.3)) em.EyeR = obj.NewDynamicSkin(sc, name+"_eye-r", physics.Box, "green", mass*.001, math32.Vec3(eyesz, eyesz*.5, eyesz*.2), headPos.Add(eyeoff), rot) - em.EyeR.Group = 0 + // em.EyeR.Group = 0 ej = obj.NewJointFixed(head, em.EyeR, eyeoff, math32.Vec3(0, 0, -eyesz*.3)) ej.ParentFixed = true } diff --git a/physics/model.go b/physics/model.go index 4cef353a..d3b3cfc3 100644 --- a/physics/model.go +++ b/physics/model.go @@ -21,6 +21,9 @@ type Model struct { // Params are global parameters. Params []PhysicsParams + // GetContacts will download Contacts from the GPU, if processing them on the CPU. + GetContacts bool + // CurrentWorld is the [BodyWorld] value to use when creating new bodies. // Set to -1 to create global elements that interact with everything, // while 0 and positive numbers only interact amongst themselves. diff --git a/physics/model.goal b/physics/model.goal index 02c45b42..1c38aac5 100644 --- a/physics/model.goal +++ b/physics/model.goal @@ -19,6 +19,9 @@ type Model struct { // Params are global parameters. Params []PhysicsParams + // GetContacts will download Contacts from the GPU, if processing them on the CPU. + GetContacts bool + // CurrentWorld is the [BodyWorld] value to use when creating new bodies. // Set to -1 to create global elements that interact with everything, // while 0 and positive numbers only interact amongst themselves. diff --git a/physics/shapegeom_test.go b/physics/shapegeom_test.go index faec5204..e7eb28ae 100644 --- a/physics/shapegeom_test.go +++ b/physics/shapegeom_test.go @@ -5,7 +5,6 @@ package physics import ( - "fmt" "testing" "cogentcore.org/core/math32" @@ -62,13 +61,14 @@ func TestSphereSphere(t *testing.T) { assert.InDelta(t, tc.dist, distActual, tol) assert.InDelta(t, 1.0, norm.Length(), tol) + assert.Equal(t, distActual < margin, actual) if !actual { continue } cpA := ctA.Add(offA) cpB := ctB.Add(offB) - fmt.Println(cpA, cpB) + // fmt.Println(cpA, cpB, tc.dist, actual) } // diff --git a/physics/step.go b/physics/step.go index 504064ac..26143eea 100644 --- a/physics/step.go +++ b/physics/step.go @@ -96,6 +96,9 @@ func (ml *Model) Step() { // StepGet runs one physics step and gets the given vars back // from the GPU. func (ml *Model) StepGet(vars ...GPUVars) { + if ml.GetContacts { + vars = append(vars, ContactsVar, ContactsNVar) + } params := GetParams(0) RunStepInit(1) ml.StepCollision() diff --git a/physics/step.goal b/physics/step.goal index 1878cb7e..47867246 100644 --- a/physics/step.goal +++ b/physics/step.goal @@ -94,6 +94,9 @@ func (ml *Model) Step() { // StepGet runs one physics step and gets the given vars back // from the GPU. func (ml *Model) StepGet(vars ...GPUVars) { + if ml.GetContacts { + vars = append(vars, ContactsVar, ContactsNVar) + } params := GetParams(0) RunStepInit(1) ml.StepCollision() From b3a59a1fa021eb3d9e80e839cc46c8090ace5950 Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Sun, 4 Jan 2026 16:39:42 -0800 Subject: [PATCH 79/97] physics: collision testing example -- fun! --- physics/examples/collision/collision.go | 77 +++++++++++++++++++++++++ physics/shapegeom_test.go | 4 +- 2 files changed, 79 insertions(+), 2 deletions(-) create mode 100644 physics/examples/collision/collision.go diff --git a/physics/examples/collision/collision.go b/physics/examples/collision/collision.go new file mode 100644 index 00000000..82b62dd9 --- /dev/null +++ b/physics/examples/collision/collision.go @@ -0,0 +1,77 @@ +// Copyright (c) 2025, Cogent Core. All rights reserved. +// Use of this source code is governed by a BSD-style +// license that can be found in the LICENSE file. + +package main + +//go:generate core generate -add-types + +import ( + "cogentcore.org/core/core" + "cogentcore.org/core/math32" + _ "cogentcore.org/lab/gosl/slbool/slboolcore" // include to get gui views + "cogentcore.org/lab/physics" + "cogentcore.org/lab/physics/phyxyz" +) + +// Collide has sim params +type Collide struct { + ShapeA physics.Shapes + ShapeB physics.Shapes + + SizeA float32 + SizeB float32 + + MassA float32 + MassB float32 + + ZposA float32 + ZposB float32 +} + +func (cl *Collide) Defaults() { + cl.ShapeA = physics.Sphere + cl.ShapeB = physics.Sphere + cl.SizeA = 0.5 + cl.SizeB = 0.5 + cl.MassA = 1 + cl.MassB = 1 +} + +func main() { + b := core.NewBody("collide").SetTitle("Physics Collide") + ed := phyxyz.NewEditor(b) + ed.CameraPos = math32.Vec3(0, 20, 20) + + cl := &Collide{} + cl.Defaults() + + ed.SetUserParams(cl) + + core.NewText(b).SetText("Pusher target position:") + pos := float32(3) + sld := core.NewSlider(b).SetMin(0).SetMax(5).SetStep(.1).SetEnforceStep(true) + core.Bind(&pos, sld) + + ed.SetConfigFunc(func() { + ml := ed.Model + ml.GPU = false + sc := ed.Scene + rot := math32.NewQuatIdentity() + sc.NewBody(ml, "floor", physics.Plane, "#D0D0D080", math32.Vec3(0, 0, 0), math32.Vec3(0, 0, 0), rot) + + sc.NewDynamic(ml, "A", cl.ShapeA, "blue", cl.MassA, math32.Vec3(cl.SizeA, cl.SizeA, cl.SizeA), math32.Vec3(-5, cl.SizeA, cl.ZposA), rot) + sc.NewDynamic(ml, "B", cl.ShapeB, "red", cl.MassB, math32.Vec3(cl.SizeB, cl.SizeB, cl.SizeB), math32.Vec3(5, cl.SizeB, cl.ZposB), rot) + + push := sc.NewDynamic(ml, "push", physics.Box, "grey", 1.0, math32.Vec3(.1, 2, 2), math32.Vec3(-8, 2, 0), rot) + ml.NewObject() + sc.NewJointPrismatic(ml, nil, push, math32.Vec3(-8, 0, 0), math32.Vec3(0, -2, 0), math32.Vec3(1, 0, 0)) + }) + + ed.SetControlFunc(func(timeStep int) { + physics.SetJointTargetPos(0, 0, pos, 100) + physics.SetJointTargetVel(0, 0, 0, 20) + }) + + b.RunMainWindow() +} diff --git a/physics/shapegeom_test.go b/physics/shapegeom_test.go index e7eb28ae..adb552cc 100644 --- a/physics/shapegeom_test.go +++ b/physics/shapegeom_test.go @@ -66,8 +66,8 @@ func TestSphereSphere(t *testing.T) { if !actual { continue } - cpA := ctA.Add(offA) - cpB := ctB.Add(offB) + // cpA := ctA.Add(offA) + // cpB := ctB.Add(offB) // fmt.Println(cpA, cpB, tc.dist, actual) } From 21386c3d43f6a3a2d9b2db72c2d41b77b8213deb Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Sun, 4 Jan 2026 17:25:32 -0800 Subject: [PATCH 80/97] physics: balls was exceeding contacts max: easy to always get ContactsN back and compare with max, and issue warning (for now). --- physics/contact.go | 6 ++++-- physics/contact.goal | 6 ++++-- physics/examples/balls/balls.go | 3 +++ physics/examples/collision/collision.go | 5 ++++- physics/step.go | 20 +++++++++++++------- physics/step.goal | 20 +++++++++++++------- 6 files changed, 41 insertions(+), 19 deletions(-) diff --git a/physics/contact.go b/physics/contact.go index ac0a5555..e1a07fe6 100644 --- a/physics/contact.go +++ b/physics/contact.go @@ -7,7 +7,7 @@ package physics import ( - // "fmt" + "fmt" "math" "sync/atomic" @@ -312,6 +312,7 @@ func CollisionBroad(i uint32) { //gosl:kernel nci := enci - (ncA + ncB) // starting index if nci >= params.ContactsMax { // shouldn't happen! + fmt.Println("over max!", nci, params.ContactsMax) return } AddBroadContacts(biA, biB, nci, ncA, ncB) @@ -812,7 +813,8 @@ func (ml *Model) SetMaxContacts() { // todo: this is a massive over-estimate, b/c there is no way everyone could be // colliding at once. Except.. if it is a very small model. if params.BodyCollidePairsN > 1000 { - n = int32(math32.Sqrt(float32(n))) + n = 4 * max(int32(math32.Sqrt(float32(n))), int32(Bodies.DimSize(0))) + // fmt.Println("> 1000", params.BodyCollidePairsN, n) } n = max(n, params.DynamicsN) params.ContactsMax = n diff --git a/physics/contact.goal b/physics/contact.goal index 24d0c366..5c47e027 100644 --- a/physics/contact.goal +++ b/physics/contact.goal @@ -5,7 +5,7 @@ package physics import ( - // "fmt" + "fmt" "math" "sync/atomic" @@ -310,6 +310,7 @@ func CollisionBroad(i uint32) { //gosl:kernel nci := enci - (ncA + ncB) // starting index if nci >= params.ContactsMax { // shouldn't happen! + fmt.Println("over max!", nci, params.ContactsMax) return } AddBroadContacts(biA, biB, nci, ncA, ncB) @@ -811,7 +812,8 @@ func (ml *Model) SetMaxContacts() { // todo: this is a massive over-estimate, b/c there is no way everyone could be // colliding at once. Except.. if it is a very small model. if params.BodyCollidePairsN > 1000 { - n = int32(math32.Sqrt(float32(n))) + n = 4 * max(int32(math32.Sqrt(float32(n))), int32(Bodies.DimSize(0))) + // fmt.Println("> 1000", params.BodyCollidePairsN, n) } n = max(n, params.DynamicsN) params.ContactsMax = n diff --git a/physics/examples/balls/balls.go b/physics/examples/balls/balls.go index 60b236d3..681f2c28 100644 --- a/physics/examples/balls/balls.go +++ b/physics/examples/balls/balls.go @@ -67,12 +67,15 @@ func main() { bs := &Balls{} bs.Defaults() + ed.CameraPos = math32.Vec3(0, bs.Width, bs.Width) ed.SetUserParams(bs) ed.SetConfigFunc(func() { ml := ed.Model ml.Params[0].SubSteps = 100 + ml.Params[0].Dt = 0.001 + // ml.GPU = false sc := ed.Scene rot := math32.NewQuatIdentity() sc.NewBody(ml, "floor", physics.Plane, "#D0D0D080", math32.Vec3(0, 0, 0), diff --git a/physics/examples/collision/collision.go b/physics/examples/collision/collision.go index 82b62dd9..5dd07d64 100644 --- a/physics/examples/collision/collision.go +++ b/physics/examples/collision/collision.go @@ -27,6 +27,8 @@ type Collide struct { ZposA float32 ZposB float32 + + PushMass float32 } func (cl *Collide) Defaults() { @@ -36,6 +38,7 @@ func (cl *Collide) Defaults() { cl.SizeB = 0.5 cl.MassA = 1 cl.MassB = 1 + cl.PushMass = 1 } func main() { @@ -63,7 +66,7 @@ func main() { sc.NewDynamic(ml, "A", cl.ShapeA, "blue", cl.MassA, math32.Vec3(cl.SizeA, cl.SizeA, cl.SizeA), math32.Vec3(-5, cl.SizeA, cl.ZposA), rot) sc.NewDynamic(ml, "B", cl.ShapeB, "red", cl.MassB, math32.Vec3(cl.SizeB, cl.SizeB, cl.SizeB), math32.Vec3(5, cl.SizeB, cl.ZposB), rot) - push := sc.NewDynamic(ml, "push", physics.Box, "grey", 1.0, math32.Vec3(.1, 2, 2), math32.Vec3(-8, 2, 0), rot) + push := sc.NewDynamic(ml, "push", physics.Box, "grey", cl.PushMass, math32.Vec3(.1, 2, 2), math32.Vec3(-8, 2, 0), rot) ml.NewObject() sc.NewJointPrismatic(ml, nil, push, math32.Vec3(-8, 0, 0), math32.Vec3(0, -2, 0), math32.Vec3(1, 0, 0)) }) diff --git a/physics/step.go b/physics/step.go index 26143eea..fb3a3705 100644 --- a/physics/step.go +++ b/physics/step.go @@ -9,7 +9,11 @@ package physics -import "cogentcore.org/core/math32" +import ( + "fmt" + + "cogentcore.org/core/math32" +) //gosl:start //gosl:import "cogentcore.org/lab/gosl/slmath" @@ -88,17 +92,19 @@ func (ml *Model) Step() { ml.StepGet() } } - ml.StepGet(ParamsVar, DynamicsVar) - // wl.StepGet(ParamsVar, DynamicsVar, ContactsNVar) - // fmt.Println("contacts:", ContactsN.Value(0), "max:", params.ContactsMax) + vars := []GPUVars{ParamsVar, DynamicsVar, ContactsNVar} + if ml.GetContacts { + vars = append(vars, ContactsVar) + } + ml.StepGet(vars...) + if ContactsN.Value(0) >= params.ContactsMax { + fmt.Println("Warning: over ContactsMax:", ContactsN.Value(0), "Max:", params.ContactsMax) + } } // StepGet runs one physics step and gets the given vars back // from the GPU. func (ml *Model) StepGet(vars ...GPUVars) { - if ml.GetContacts { - vars = append(vars, ContactsVar, ContactsNVar) - } params := GetParams(0) RunStepInit(1) ml.StepCollision() diff --git a/physics/step.goal b/physics/step.goal index 47867246..c537213f 100644 --- a/physics/step.goal +++ b/physics/step.goal @@ -7,7 +7,11 @@ package physics -import "cogentcore.org/core/math32" +import ( + "fmt" + + "cogentcore.org/core/math32" +) //gosl:start //gosl:import "cogentcore.org/lab/gosl/slmath" @@ -86,17 +90,19 @@ func (ml *Model) Step() { ml.StepGet() } } - ml.StepGet(ParamsVar, DynamicsVar) - // wl.StepGet(ParamsVar, DynamicsVar, ContactsNVar) - // fmt.Println("contacts:", ContactsN.Value(0), "max:", params.ContactsMax) + vars := []GPUVars{ParamsVar, DynamicsVar, ContactsNVar} + if ml.GetContacts { + vars = append(vars, ContactsVar) + } + ml.StepGet(vars...) + if ContactsN.Value(0) >= params.ContactsMax { + fmt.Println("Warning: over ContactsMax:", ContactsN.Value(0), "Max:", params.ContactsMax) + } } // StepGet runs one physics step and gets the given vars back // from the GPU. func (ml *Model) StepGet(vars ...GPUVars) { - if ml.GetContacts { - vars = append(vars, ContactsVar, ContactsNVar) - } params := GetParams(0) RunStepInit(1) ml.StepCollision() From 01aec1d3b6177e7bdbdb26c860b749c3c5d8bd25 Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Sun, 4 Jan 2026 20:03:08 -0800 Subject: [PATCH 81/97] physics: capsule-plane collision bugfix and test in place. capsule-sphere is next. --- physics/contact.go | 6 +- physics/contact.goal | 6 +- physics/enumgen.go | 2 +- physics/model.go | 3 + physics/model.goal | 3 + physics/phyxyz/skin.go | 2 +- physics/shaders/CollisionNarrow.wgsl | 18 +-- physics/shapecollide.go | 18 +-- physics/shapecollide.goal | 18 +-- physics/shapegeom_test.go | 196 ++++++++++++++------------- physics/shapes.go | 20 ++- physics/typegen.go | 2 +- 12 files changed, 158 insertions(+), 136 deletions(-) diff --git a/physics/contact.go b/physics/contact.go index e1a07fe6..04b9ae3a 100644 --- a/physics/contact.go +++ b/physics/contact.go @@ -7,7 +7,7 @@ package physics import ( - "fmt" + // "fmt" "math" "sync/atomic" @@ -312,7 +312,7 @@ func CollisionBroad(i uint32) { //gosl:kernel nci := enci - (ncA + ncB) // starting index if nci >= params.ContactsMax { // shouldn't happen! - fmt.Println("over max!", nci, params.ContactsMax) + // fmt.Println("over max!", nci, params.ContactsMax) return } AddBroadContacts(biA, biB, nci, ncA, ncB) @@ -816,7 +816,7 @@ func (ml *Model) SetMaxContacts() { n = 4 * max(int32(math32.Sqrt(float32(n))), int32(Bodies.DimSize(0))) // fmt.Println("> 1000", params.BodyCollidePairsN, n) } - n = max(n, params.DynamicsN) + n = max(n, 4*params.DynamicsN) params.ContactsMax = n ml.BroadContacts.SetShapeSizes(int(n), int(BroadContactVarsN)) ml.Contacts.SetShapeSizes(int(n), int(ContactVarsN)) diff --git a/physics/contact.goal b/physics/contact.goal index 5c47e027..514d8f92 100644 --- a/physics/contact.goal +++ b/physics/contact.goal @@ -5,7 +5,7 @@ package physics import ( - "fmt" + // "fmt" "math" "sync/atomic" @@ -310,7 +310,7 @@ func CollisionBroad(i uint32) { //gosl:kernel nci := enci - (ncA + ncB) // starting index if nci >= params.ContactsMax { // shouldn't happen! - fmt.Println("over max!", nci, params.ContactsMax) + // fmt.Println("over max!", nci, params.ContactsMax) return } AddBroadContacts(biA, biB, nci, ncA, ncB) @@ -815,7 +815,7 @@ func (ml *Model) SetMaxContacts() { n = 4 * max(int32(math32.Sqrt(float32(n))), int32(Bodies.DimSize(0))) // fmt.Println("> 1000", params.BodyCollidePairsN, n) } - n = max(n, params.DynamicsN) + n = max(n, 4 * params.DynamicsN) params.ContactsMax = n ml.BroadContacts.SetShapeSizes(int(n), int(BroadContactVarsN)) ml.Contacts.SetShapeSizes(int(n), int(ContactVarsN)) diff --git a/physics/enumgen.go b/physics/enumgen.go index 052368b1..7e297fdf 100644 --- a/physics/enumgen.go +++ b/physics/enumgen.go @@ -389,7 +389,7 @@ const ShapesN Shapes = 6 var _ShapesValueMap = map[string]Shapes{`Plane`: 0, `Sphere`: 1, `Capsule`: 2, `Cylinder`: 3, `Box`: 4, `Cone`: 5} -var _ShapesDescMap = map[Shapes]string{0: `Plane cannot be a dynamic shape, but is most efficient for collision computations. Use size = 0 for an infinite plane. Natively extends in the X-Z plane: SizeX x SizeZ.`, 1: `Sphere. SizeX is the radius.`, 2: `Capsule is a cylinder with half-spheres on the ends. Natively oriented vertically along the Y axis. SizeX = radius, SizeY = half-height.`, 3: `Cylinder, natively oriented vertically along the Y axis. SizeX = radius, SizeY = half-height in Y axis. Cylinder can not collide with a Box.`, 4: `Box is a 3D rectalinear shape. The sizes are _half_ sizes along each dimension, relative to the center.`, 5: `Cone is like a cylinder with the top radius = 0, oriented up. SizeX = bottom radius, SizeY = half-height in Y. Cone does not support any collisions and is not recommended for interacting bodies.`} +var _ShapesDescMap = map[Shapes]string{0: `Plane cannot be a dynamic shape, but is most efficient for collision computations. Use size = 0 for an infinite plane. Natively extends in the X-Z plane: SizeX x SizeZ.`, 1: `Sphere. SizeX is the radius.`, 2: `Capsule is a cylinder with half-spheres on the ends. Natively oriented vertically along the Y axis. SizeX = radius of end caps, SizeY = _total_ half-height (i.e., SizeX + half-height of cylindrical portion, must be >= SizeX). This parameterization allows joint offsets to be SizeY, and direct swapping of shape across Box and Cylinder with same total extent.`, 3: `Cylinder, natively oriented vertically along the Y axis. SizeX = radius, SizeY = half-height of Y axis Cylinder can not collide with a Box.`, 4: `Box is a 3D rectalinear shape. The sizes are _half_ sizes along each dimension, relative to the center.`, 5: `Cone is like a cylinder with the top radius = 0, oriented up. SizeX = bottom radius, SizeY = half-height in Y. Cone does not support any collisions and is not recommended for interacting bodies.`} var _ShapesMap = map[Shapes]string{0: `Plane`, 1: `Sphere`, 2: `Capsule`, 3: `Cylinder`, 4: `Box`, 5: `Cone`} diff --git a/physics/model.go b/physics/model.go index d3b3cfc3..0313a275 100644 --- a/physics/model.go +++ b/physics/model.go @@ -166,6 +166,9 @@ func (ml *Model) NewBody(shape Shapes, hsize, pos math32.Vector3, rot math32.Qua ml.Params[0].BodiesN = idx + 1 SetBodyShape(idx, shape) SetBodyDynamic(idx, -1) + if shape == Capsule { + hsize.Y = max(hsize.Y, hsize.X) + } SetBodyHSize(idx, hsize) SetBodyPos(idx, pos) SetBodyQuat(idx, rot) diff --git a/physics/model.goal b/physics/model.goal index 1c38aac5..80c6afa2 100644 --- a/physics/model.goal +++ b/physics/model.goal @@ -164,6 +164,9 @@ func (ml *Model) NewBody(shape Shapes, hsize, pos math32.Vector3, rot math32.Qua ml.Params[0].BodiesN = idx + 1 SetBodyShape(idx, shape) SetBodyDynamic(idx, -1) + if shape == Capsule { + hsize.Y = max(hsize.Y, hsize.X) + } SetBodyHSize(idx, hsize) SetBodyPos(idx, pos) SetBodyQuat(idx, rot) diff --git a/physics/phyxyz/skin.go b/physics/phyxyz/skin.go index 89b20e2d..133632bf 100644 --- a/physics/phyxyz/skin.go +++ b/physics/phyxyz/skin.go @@ -201,7 +201,7 @@ func (sk *Skin) CapsuleInit(sld *xyz.Solid) { ms = xyz.NewCapsule(sld.Scene, mnm, 1, .2, 32, 1) } sld.SetMeshName(mnm) - sld.Pose.Scale.Set(sk.HSize.X/.2, 2*(sk.HSize.Y/1.4), sk.HSize.Z/.2) + sld.Pose.Scale.Set(sk.HSize.X/.2, (2.0*sk.HSize.Y)/1.4, sk.HSize.X/.2) sk.UpdateColor(sk.Color, sld) sld.Updater(func() { sk.UpdatePose(sld) diff --git a/physics/shaders/CollisionNarrow.wgsl b/physics/shaders/CollisionNarrow.wgsl index eeb8b398..75fd0c41 100644 --- a/physics/shaders/CollisionNarrow.wgsl +++ b/physics/shaders/CollisionNarrow.wgsl @@ -538,13 +538,13 @@ fn ColCapsulePlane(cpi: i32,maxIter: i32, gdA: ptr, gdB: ptr< var pAw: vec3; var pBw: vec3; var diff: vec3; - var hh = (*gdA).Size.y; - if (cpi < 2) { // vertex + var hh = (*gdA).Size.y - (*gdA).Size.x; + if (cpi < 2) { // vertex. Note: radius is automatically subtracted!! so this is correct with hh var side = f32(cpi)*2 - 1; pAw = MulSpatialPoint((*gdA).WbR, (*gdA).WbQ, vec3(0, side*hh, 0)); var queryB = MulSpatialPoint((*gdB).BwR, (*gdB).BwQ, pAw); var pBb = ClosestPointPlane((*gdB).Size.x, (*gdB).Size.z, queryB); - pBw = MulSpatialPoint((*gdA).WbR, (*gdA).WbQ, pBb); + pBw = MulSpatialPoint((*gdB).WbR, (*gdB).WbQ, pBb); diff = pAw-(pBw); if ((*gdB).Size.x > 0) { *norm = Normal3(diff); @@ -559,7 +559,7 @@ fn ColCapsulePlane(cpi: i32,maxIter: i32, gdA: ptr, gdB: ptr< var edge1w = MulSpatialPoint((*gdB).WbR, (*gdB).WbQ, edge1); var edge0a = MulSpatialPoint((*gdA).BwR, (*gdA).BwQ, edge0w); var edge1a = MulSpatialPoint((*gdA).BwR, (*gdA).BwQ, edge1w); - var u = ClosestEdgeCapsule((*gdA).Size.x, (*gdA).Size.y, edge0a, edge1a, maxIter); + var u = ClosestEdgeCapsule((*gdA).Size.x, hh, edge0a, edge1a, maxIter); pBw = edge0w*(1 - u)+(edge1w*(u)); var p0Aw = MulSpatialPoint((*gdA).WbR, (*gdA).WbQ, vec3(0, hh, 0)); var p1Aw = MulSpatialPoint((*gdA).WbR, (*gdA).WbQ, vec3(0, -hh, 0)); @@ -572,15 +572,15 @@ fn ColCapsulePlane(cpi: i32,maxIter: i32, gdA: ptr, gdB: ptr< return Dot3(diff, *norm); } fn ColCapsuleCapsule(cpi: i32,maxIter: i32, gdA: ptr, gdB: ptr, pA: ptr>,pB: ptr>,norm: ptr>) -> f32 { - var hhA = (*gdA).Size.y; - var hhB = (*gdB).Size.y; + var hhA = (*gdA).Size.y - (*gdA).Size.x; + var hhB = (*gdB).Size.y - (*gdA).Size.x; var e0 = vec3(0, 0, hhA*f32(cpi%2)); var e1 = vec3(0, 0, -hhA*f32((cpi+1)%2)); var edge0w = MulSpatialPoint((*gdA).WbR, (*gdA).WbQ, e0); var edge1w = MulSpatialPoint((*gdA).WbR, (*gdA).WbQ, e1); var edge0b = MulSpatialPoint((*gdA).BwR, (*gdA).BwQ, edge0w); var edge1b = MulSpatialPoint((*gdA).BwR, (*gdA).BwQ, edge1w); - var u = ClosestEdgeCapsule((*gdB).Size.x, (*gdB).Size.y, edge0b, edge1b, maxIter); + var u = ClosestEdgeCapsule((*gdB).Size.x, hhB, edge0b, edge1b, maxIter); var pAw = edge0w*(1 - u)+(edge1w*(u)); var p0Bw = MulSpatialPoint((*gdB).WbR, (*gdB).WbQ, vec3(0, hhB, 0)); var p1Bw = MulSpatialPoint((*gdB).WbR, (*gdB).WbQ, vec3(0, -hhB, 0)); @@ -611,7 +611,7 @@ fn ColBoxBox(cpi: i32,maxIter: i32, gdA: ptr, gdB: ptr, gdB: ptr, pA: ptr>,pB: ptr>,norm: ptr>) -> f32 { - var hhB = (*gdB).Size.y; + var hhB = (*gdB).Size.y - (*gdB).Size.x; var e0 = vec3(0, -hhB*f32(cpi%2), 0); var e1 = vec3(0, hhB*f32((cpi+1)%2), 0); var edge0w = MulSpatialPoint((*gdB).WbR, (*gdB).WbQ, e0); @@ -693,7 +693,7 @@ return Dot3(diff, *norm); } fn ColSphereCapsule(cpi: i32,maxIter: i32, gdA: ptr, gdB: ptr, pA: ptr>,pB: ptr>,norm: ptr>) -> f32 { var pAw = (*gdA).WbR; - var hhB = (*gdB).Size.y; + var hhB = (*gdB).Size.y - (*gdB).Size.x; var AB = MulSpatialPoint((*gdB).WbR, (*gdB).WbQ, vec3(0, hhB, 0)); var BB = MulSpatialPoint((*gdB).WbR, (*gdB).WbQ, vec3(0, -hhB, 0)); var pBw = ClosestPointLineSegment(AB, BB, pAw); diff --git a/physics/shapecollide.go b/physics/shapecollide.go index 2f6f5b43..42a1912f 100644 --- a/physics/shapecollide.go +++ b/physics/shapecollide.go @@ -121,13 +121,13 @@ func ColSphereSphere(cpi, maxIter int32, gdA *GeomData, gdB *GeomData, pA, pB, n func ColCapsulePlane(cpi, maxIter int32, gdA *GeomData, gdB *GeomData, pA, pB, norm *math32.Vector3) float32 { var pAw, pBw, diff math32.Vector3 - hh := gdA.Size.Y - if cpi < 2 { // vertex + hh := gdA.Size.Y - gdA.Size.X + if cpi < 2 { // vertex. Note: radius is automatically subtracted!! so this is correct with hh side := float32(cpi)*2 - 1 pAw = slmath.MulSpatialPoint(gdA.WbR, gdA.WbQ, math32.Vec3(0, side*hh, 0)) queryB := slmath.MulSpatialPoint(gdB.BwR, gdB.BwQ, pAw) pBb := ClosestPointPlane(gdB.Size.X, gdB.Size.Z, queryB) - pBw = slmath.MulSpatialPoint(gdA.WbR, gdA.WbQ, pBb) + pBw = slmath.MulSpatialPoint(gdB.WbR, gdB.WbQ, pBb) diff = pAw.Sub(pBw) if gdB.Size.X > 0 { *norm = slmath.Normal3(diff) @@ -141,7 +141,7 @@ func ColCapsulePlane(cpi, maxIter int32, gdA *GeomData, gdB *GeomData, pA, pB, n edge1w := slmath.MulSpatialPoint(gdB.WbR, gdB.WbQ, edge1) edge0a := slmath.MulSpatialPoint(gdA.BwR, gdA.BwQ, edge0w) edge1a := slmath.MulSpatialPoint(gdA.BwR, gdA.BwQ, edge1w) - u := ClosestEdgeCapsule(gdA.Size.X, gdA.Size.Y, edge0a, edge1a, maxIter) + u := ClosestEdgeCapsule(gdA.Size.X, hh, edge0a, edge1a, maxIter) pBw = edge0w.MulScalar(1 - u).Add(edge1w.MulScalar(u)) // find closest point + contact normal on capsule A p0Aw := slmath.MulSpatialPoint(gdA.WbR, gdA.WbQ, math32.Vec3(0, hh, 0)) @@ -158,8 +158,8 @@ func ColCapsulePlane(cpi, maxIter int32, gdA *GeomData, gdB *GeomData, pA, pB, n // Handle collision between two capsules (gdA and gdB). func ColCapsuleCapsule(cpi, maxIter int32, gdA *GeomData, gdB *GeomData, pA, pB, norm *math32.Vector3) float32 { // find closest edge coordinate to capsule SDF B - hhA := gdA.Size.Y - hhB := gdB.Size.Y + hhA := gdA.Size.Y - gdA.Size.X + hhB := gdB.Size.Y - gdA.Size.X // edge from capsule A // depending on point id, we query an edge from 0 to 0.5 or 0.5 to 1 e0 := math32.Vec3(0, 0, hhA*float32(cpi%2)) @@ -168,7 +168,7 @@ func ColCapsuleCapsule(cpi, maxIter int32, gdA *GeomData, gdB *GeomData, pA, pB, edge1w := slmath.MulSpatialPoint(gdA.WbR, gdA.WbQ, e1) edge0b := slmath.MulSpatialPoint(gdA.BwR, gdA.BwQ, edge0w) edge1b := slmath.MulSpatialPoint(gdA.BwR, gdA.BwQ, edge1w) - u := ClosestEdgeCapsule(gdB.Size.X, gdB.Size.Y, edge0b, edge1b, maxIter) + u := ClosestEdgeCapsule(gdB.Size.X, hhB, edge0b, edge1b, maxIter) pAw := edge0w.MulScalar(1 - u).Add(edge1w.MulScalar(u)) p0Bw := slmath.MulSpatialPoint(gdB.WbR, gdB.WbQ, math32.Vec3(0, hhB, 0)) p1Bw := slmath.MulSpatialPoint(gdB.WbR, gdB.WbQ, math32.Vec3(0, -hhB, 0)) @@ -206,7 +206,7 @@ func ColBoxBox(cpi, maxIter int32, gdA *GeomData, gdB *GeomData, pA, pB, norm *m // Handle collision between a box (gdA) and a capsule (gdB). func ColBoxCapsule(cpi, maxIter int32, gdA *GeomData, gdB *GeomData, pA, pB, norm *math32.Vector3) float32 { - hhB := gdB.Size.Y + hhB := gdB.Size.Y - gdB.Size.X // capsule B // depending on point id, we query an edge from 0 to 0.5 or 0.5 to 1 e0 := math32.Vec3(0, -hhB*float32(cpi%2), 0) @@ -310,7 +310,7 @@ func ColSphereBox(cpi, maxIter int32, gdA *GeomData, gdB *GeomData, pA, pB, norm // Handle collision between a sphere (gdA) and a capsule (gdB). func ColSphereCapsule(cpi, maxIter int32, gdA *GeomData, gdB *GeomData, pA, pB, norm *math32.Vector3) float32 { pAw := gdA.WbR - hhB := gdB.Size.Y + hhB := gdB.Size.Y - gdB.Size.X // capsule B AB := slmath.MulSpatialPoint(gdB.WbR, gdB.WbQ, math32.Vec3(0, hhB, 0)) BB := slmath.MulSpatialPoint(gdB.WbR, gdB.WbQ, math32.Vec3(0, -hhB, 0)) diff --git a/physics/shapecollide.goal b/physics/shapecollide.goal index 1001f15f..5884479b 100644 --- a/physics/shapecollide.goal +++ b/physics/shapecollide.goal @@ -119,13 +119,13 @@ func ColSphereSphere(cpi, maxIter int32, gdA *GeomData, gdB *GeomData, pA, pB, n func ColCapsulePlane(cpi, maxIter int32, gdA *GeomData, gdB *GeomData, pA, pB, norm *math32.Vector3) float32 { var pAw, pBw, diff math32.Vector3 - hh := gdA.Size.Y - if cpi < 2 { // vertex + hh := gdA.Size.Y - gdA.Size.X + if cpi < 2 { // vertex. Note: radius is automatically subtracted!! so this is correct with hh side := float32(cpi)*2 - 1 pAw = slmath.MulSpatialPoint(gdA.WbR, gdA.WbQ, math32.Vec3(0, side*hh, 0)) queryB := slmath.MulSpatialPoint(gdB.BwR, gdB.BwQ, pAw) pBb := ClosestPointPlane(gdB.Size.X, gdB.Size.Z, queryB) - pBw = slmath.MulSpatialPoint(gdA.WbR, gdA.WbQ, pBb) + pBw = slmath.MulSpatialPoint(gdB.WbR, gdB.WbQ, pBb) diff = pAw.Sub(pBw) if gdB.Size.X > 0 { *norm = slmath.Normal3(diff) @@ -139,7 +139,7 @@ func ColCapsulePlane(cpi, maxIter int32, gdA *GeomData, gdB *GeomData, pA, pB, n edge1w := slmath.MulSpatialPoint(gdB.WbR, gdB.WbQ, edge1) edge0a := slmath.MulSpatialPoint(gdA.BwR, gdA.BwQ, edge0w) edge1a := slmath.MulSpatialPoint(gdA.BwR, gdA.BwQ, edge1w) - u := ClosestEdgeCapsule(gdA.Size.X, gdA.Size.Y, edge0a, edge1a, maxIter) + u := ClosestEdgeCapsule(gdA.Size.X, hh, edge0a, edge1a, maxIter) pBw = edge0w.MulScalar(1 - u).Add(edge1w.MulScalar(u)) // find closest point + contact normal on capsule A p0Aw := slmath.MulSpatialPoint(gdA.WbR, gdA.WbQ, math32.Vec3(0, hh, 0)) @@ -156,8 +156,8 @@ func ColCapsulePlane(cpi, maxIter int32, gdA *GeomData, gdB *GeomData, pA, pB, n // Handle collision between two capsules (gdA and gdB). func ColCapsuleCapsule(cpi, maxIter int32, gdA *GeomData, gdB *GeomData, pA, pB, norm *math32.Vector3) float32 { // find closest edge coordinate to capsule SDF B - hhA := gdA.Size.Y - hhB := gdB.Size.Y + hhA := gdA.Size.Y - gdA.Size.X + hhB := gdB.Size.Y - gdA.Size.X // edge from capsule A // depending on point id, we query an edge from 0 to 0.5 or 0.5 to 1 e0 := math32.Vec3(0, 0, hhA*float32(cpi%2)) @@ -166,7 +166,7 @@ func ColCapsuleCapsule(cpi, maxIter int32, gdA *GeomData, gdB *GeomData, pA, pB, edge1w := slmath.MulSpatialPoint(gdA.WbR, gdA.WbQ, e1) edge0b := slmath.MulSpatialPoint(gdA.BwR, gdA.BwQ, edge0w) edge1b := slmath.MulSpatialPoint(gdA.BwR, gdA.BwQ, edge1w) - u := ClosestEdgeCapsule(gdB.Size.X, gdB.Size.Y, edge0b, edge1b, maxIter) + u := ClosestEdgeCapsule(gdB.Size.X, hhB, edge0b, edge1b, maxIter) pAw := edge0w.MulScalar(1 - u).Add(edge1w.MulScalar(u)) p0Bw := slmath.MulSpatialPoint(gdB.WbR, gdB.WbQ, math32.Vec3(0, hhB, 0)) p1Bw := slmath.MulSpatialPoint(gdB.WbR, gdB.WbQ, math32.Vec3(0, -hhB, 0)) @@ -204,7 +204,7 @@ func ColBoxBox(cpi, maxIter int32, gdA *GeomData, gdB *GeomData, pA, pB, norm *m // Handle collision between a box (gdA) and a capsule (gdB). func ColBoxCapsule(cpi, maxIter int32, gdA *GeomData, gdB *GeomData, pA, pB, norm *math32.Vector3) float32 { - hhB := gdB.Size.Y + hhB := gdB.Size.Y - gdB.Size.X // capsule B // depending on point id, we query an edge from 0 to 0.5 or 0.5 to 1 e0 := math32.Vec3(0, -hhB*float32(cpi%2), 0) @@ -307,7 +307,7 @@ func ColSphereBox(cpi, maxIter int32, gdA *GeomData, gdB *GeomData, pA, pB, norm // Handle collision between a sphere (gdA) and a capsule (gdB). func ColSphereCapsule(cpi, maxIter int32, gdA *GeomData, gdB *GeomData, pA, pB, norm *math32.Vector3) float32 { pAw := gdA.WbR - hhB := gdB.Size.Y + hhB := gdB.Size.Y - gdB.Size.X // capsule B AB := slmath.MulSpatialPoint(gdB.WbR, gdB.WbQ, math32.Vec3(0, hhB, 0)) BB := slmath.MulSpatialPoint(gdB.WbR, gdB.WbQ, math32.Vec3(0, -hhB, 0)) diff --git a/physics/shapegeom_test.go b/physics/shapegeom_test.go index adb552cc..bcfab8b3 100644 --- a/physics/shapegeom_test.go +++ b/physics/shapegeom_test.go @@ -5,6 +5,7 @@ package physics import ( + "fmt" "testing" "cogentcore.org/core/math32" @@ -70,107 +71,116 @@ func TestSphereSphere(t *testing.T) { // cpB := ctB.Add(offB) // fmt.Println(cpA, cpB, tc.dist, actual) } - - // - // // Check that normal points from geom 0 (sphere 1) into geom 1 (sphere 2) - // for i in range(len(test_cases)): - // pos1 = np.array(test_cases[i][0]) - // pos2 = np.array(test_cases[i][2]) - // normal = normals_np[i] - // self.assertTrue( - // check_normal_direction_sphere_sphere(pos1, pos2, normal), - // msg=f"Test case {i}: Normal does not point from sphere 1 toward sphere 2", - // ) - // - // // Check that contact position is at midpoint between surfaces - // for i in range(len(test_cases)): - // pos1 = np.array(test_cases[i][0]) - // radius1 = test_cases[i][1] - // pos2 = np.array(test_cases[i][2]) - // radius2 = test_cases[i][3] - // contact_pos = positions_np[i] - // normal = normals_np[i] - // penetration_depth = distances_np[i] - // - // self.assertTrue( - // check_contact_position_midpoint(contact_pos, normal, penetration_depth, pos1, radius1, pos2, radius2), - // msg=f"Test case {i}: Contact position is not at midpoint between surfaces", - // ) - // } -func TestPlaneSphere(t *testing.T) { +func TestSpherePlane(t *testing.T) { // Analytical calculation: // - Distance = (sphere_center - plane_point) · plane_normal - sphere_radius // - Negative distance indicates penetration + // note: this data is already configured as A = plane, B = sphere, but function is SpherePlane + // so we're switching below.. + tests := []struct { + normal, posA, posB math32.Vector3 + radius, dist float32 + }{ + {math32.Vector3{0, 1, 0}, math32.Vector3{0, 0, 0}, math32.Vector3{0, 2, 0}, 1, 1}, + {math32.Vector3{0, 1, 0}, math32.Vector3{0, 0, 0}, math32.Vector3{0, 1.5, 0}, 1, 0.5}, + {math32.Vector3{0, 1, 0}, math32.Vector3{0, 0, 0}, math32.Vector3{0, 1, 0}, 1, 0}, + {math32.Vector3{0, 1, 0}, math32.Vector3{0, 0, 0}, math32.Vector3{0, 0.8, 0}, 1, -0.2}, + {math32.Vector3{0, 1, 0}, math32.Vector3{0, 0, 0}, math32.Vector3{0, 0.5, 0}, 1, -0.5}, + {math32.Vector3{0, 1, 0}, math32.Vector3{0, 0, 0}, math32.Vector3{0, 0.2, 0}, 1, -0.8}, + // {math32.Vector3{1, 0, 0}, math32.Vector3{1, 0, 0}, math32.Vector3{2.0, 0, 0}, 0.5, 0.5}, // X-axis, separation = 0.5 + // {math32.Vector3{1, 0, 0}, math32.Vector3{1, 0, 0}, math32.Vector3{1.5, 0, 0}, 0.5, 0}, // X-axis, touching + // {math32.Vector3{1, 0, 0}, math32.Vector3{1, 0, 0}, math32.Vector3{1.3, 0, 0}, 0.5, -0.2}, // X-axis, penetration = 0.2 + } + tol := 1e-5 + + rot := math32.NewQuatIdentity() + for _, tc := range tests { + // note: A = sphere but pos = B.. + gdA := GeomData{Shape: Sphere, Radius: tc.radius, Size: math32.Vector3{tc.radius, 0, 0}, WbR: tc.posB, WbQ: rot} + gdB := GeomData{Shape: Plane, Size: math32.Vector3{0, 0, 0}, WbR: tc.posA, WbQ: rot} + InitGeomData(0, &gdA) + InitGeomData(0, &gdB) + + var ptA, ptB, norm math32.Vector3 + dist := ColSpherePlane(0, 10, &gdA, &gdB, &ptA, &ptB, &norm) + margin := float32(0.01) + + var ctA, ctB, offA, offB math32.Vector3 + var distActual, offMagA, offMagB float32 + actual := ContactPoints(dist, margin, &gdA, &gdB, ptA, ptB, norm, &ctA, &ctB, &offA, &offB, &distActual, &offMagA, &offMagB) + _ = actual + + fmt.Println(dist, distActual, tc.dist, actual, norm, ptA, ptB) + // if actual { + // fmt.Println(ptA, ptB, ctA, ctB, offA, offB, offMagA, offMagB) + // } + + assert.InDelta(t, tc.dist, distActual, tol) + assert.InDelta(t, 1.0, norm.Length(), tol) + assert.Equal(t, distActual < margin, actual) + + if !actual { + continue + } + // cpA := ctA.Add(offA) + // cpB := ctB.Add(offB) + // fmt.Println(cpA, cpB, tc.dist, actual) + } +} + +func TestCapsulePlane(t *testing.T) { + // note: this data is already configured as A = plane, B = capsule, but function is CapsulePlane + // so we're switching below.. tests := []struct { normal, posA, posB math32.Vector3 radius, dist float32 }{ - {math32.Vector3{0.0, 0.0, 1.0}, math32.Vector3{0.0, 0.0, 0.0}, math32.Vector3{0.0, 0.0, 2.0}, 1.0, 1.0}, // Above plane, separation = 1.0 - {math32.Vector3{0.0, 0.0, 1.0}, math32.Vector3{0.0, 0.0, 0.0}, math32.Vector3{0.0, 0.0, 1.5}, 1.0, 0.5}, // Above plane, separation = 0.5 - {math32.Vector3{0.0, 0.0, 1.0}, math32.Vector3{0.0, 0.0, 0.0}, math32.Vector3{0.0, 0.0, 1.0}, 1.0, 0.0}, // Just touching - {math32.Vector3{0.0, 0.0, 1.0}, math32.Vector3{0.0, 0.0, 0.0}, math32.Vector3{0.0, 0.0, 0.8}, 1.0, -0.2}, // Penetration = 0.2 - {math32.Vector3{0.0, 0.0, 1.0}, math32.Vector3{0.0, 0.0, 0.0}, math32.Vector3{0.0, 0.0, 0.5}, 1.0, -0.5}, // Penetration = 0.5 - {math32.Vector3{0.0, 0.0, 1.0}, math32.Vector3{0.0, 0.0, 0.0}, math32.Vector3{0.0, 0.0, 0.2}, 1.0, -0.8}, // Penetration = 0.8 - {math32.Vector3{1.0, 0.0, 0.0}, math32.Vector3{1.0, 0.0, 0.0}, math32.Vector3{2.0, 0.0, 0.0}, 0.5, 0.5}, // X-axis, separation = 0.5 - {math32.Vector3{1.0, 0.0, 0.0}, math32.Vector3{1.0, 0.0, 0.0}, math32.Vector3{1.5, 0.0, 0.0}, 0.5, 0.0}, // X-axis, touching - {math32.Vector3{1.0, 0.0, 0.0}, math32.Vector3{1.0, 0.0, 0.0}, math32.Vector3{1.3, 0.0, 0.0}, 0.5, -0.2}, // X-axis, penetration = 0.2 + {math32.Vector3{0, 1, 0}, math32.Vector3{0, 0, 0}, math32.Vector3{0, 3, 0}, 0.5, 1.5}, + {math32.Vector3{0, 1, 0}, math32.Vector3{0, 0, 0}, math32.Vector3{0, 2.5, 0}, 0.5, 1.0}, + {math32.Vector3{0, 1, 0}, math32.Vector3{0, 0, 0}, math32.Vector3{0, 2.0, 0}, 0.5, 0.5}, + {math32.Vector3{0, 1, 0}, math32.Vector3{0, 0, 0}, math32.Vector3{0, 1.5, 0}, 0.5, 0}, + {math32.Vector3{0, 1, 0}, math32.Vector3{0, 0, 0}, math32.Vector3{0, 1.4, 0}, 0.5, -0.1}, + {math32.Vector3{0, 1, 0}, math32.Vector3{0, 0, 0}, math32.Vector3{0, 1.3, 0}, 0.5, -0.2}, + {math32.Vector3{0, 1, 0}, math32.Vector3{0, 0, 0}, math32.Vector3{0, 1.2, 0}, 0.5, -0.3}, + } + tol := 1e-5 + + // rleft := math32.NewQuatAxisAngle(math32.Vec3(0, 0, 1), -math32.Pi/2) + rot := math32.NewQuatIdentity() + for _, tc := range tests { + // note: A = capsule but pos = B.. + // 1.5 hh = 1 raw hh + gdA := GeomData{Shape: Capsule, Radius: tc.radius, Size: math32.Vector3{tc.radius, 1.5, tc.radius}, WbR: tc.posB, WbQ: rot} + gdB := GeomData{Shape: Plane, Size: math32.Vector3{0, 0, 0}, WbR: tc.posA, WbQ: rot} + InitGeomData(0, &gdA) + InitGeomData(0, &gdB) + + var ptA, ptB, norm math32.Vector3 + // important: we know that the lower axis, cpi = 0, is closest here + dist := ColCapsulePlane(0, 10, &gdA, &gdB, &ptA, &ptB, &norm) + margin := float32(0.01) + + var ctA, ctB, offA, offB math32.Vector3 + var distActual, offMagA, offMagB float32 + actual := ContactPoints(dist, margin, &gdA, &gdB, ptA, ptB, norm, &ctA, &ctB, &offA, &offB, &distActual, &offMagA, &offMagB) + _ = actual + + // fmt.Println(dist, distActual, tc.dist, actual, norm, ptA, ptB) + // if actual { + // fmt.Println(ptA, ptB, ctA, ctB, offA, offB, offMagA, offMagB) + // } + + assert.InDelta(t, tc.dist, distActual, tol) + assert.InDelta(t, 1.0, norm.Length(), tol) + assert.Equal(t, distActual < margin, actual) + + if !actual { + continue + } + // cpA := ctA.Add(offA) + // cpB := ctB.Add(offB) + // fmt.Println(cpA, cpB, tc.dist, actual) } - _ = tests - - // for _, tc := range tests { - // ColSphereSphere(0, ) - // } - // - // wp.launch( - // test_plane_sphere_kernel, - // dim=len(test_cases), - // inputs=[plane_normals, plane_positions, sphere_positions, sphere_radii, distances, contact_positions], - // ) - // wp.synchronize() - // - // distances_np = distances.numpy() - // positions_np = contact_positions.numpy() - // - // // Verify expected distances with analytical validation - // for i, expected_dist in enumerate([tc[4] for tc in test_cases]): - // self.assertAlmostEqual( - // distances_np[i], - // expected_dist, - // places=5, - // msg=f"Test case {i}: Expected distance {expected_dist:.4f}, got {distances_np[i]:.4f}", - // ) - // - // // Check that contact position lies between sphere and plane - // for i in range(len(test_cases)): - // if distances_np[i] >= 0: - // // Skip separated cases - // continue - // - // plane_normal = np.array(test_cases[i][0]) - // plane_pos = np.array(test_cases[i][1]) - // sphere_pos = np.array(test_cases[i][2]) - // sphere_radius = test_cases[i][3] - // contact_pos = positions_np[i] - // - // // Contact position should be between sphere surface and plane - // // Distance from contact to sphere center should be less than sphere radius - // dist_to_sphere_center = np.linalg.norm(contact_pos - sphere_pos) - // self.assertLess( - // dist_to_sphere_center, - // sphere_radius + 0.01, - // msg=f"Test case {i}: Contact position too far from sphere (dist: {dist_to_sphere_center:.4f})", - // ) - // - // // Contact position should be on the plane side of the sphere center - // // (or at most slightly past the plane) - // dist_contact_to_plane = np.dot(contact_pos - plane_pos, plane_normal) - // dist_sphere_to_plane = np.dot(sphere_pos - plane_pos, plane_normal) - // self.assertLessEqual( - // dist_contact_to_plane, - // dist_sphere_to_plane + 0.01, - // msg=f"Test case {i}: Contact position on wrong side of sphere center", - // ) - // } diff --git a/physics/shapes.go b/physics/shapes.go index c3b90ae1..d143a215 100644 --- a/physics/shapes.go +++ b/physics/shapes.go @@ -33,13 +33,17 @@ const ( // Capsule is a cylinder with half-spheres on the ends. // Natively oriented vertically along the Y axis. - // SizeX = radius, SizeY = half-height. + // SizeX = radius of end caps, SizeY = _total_ half-height + // (i.e., SizeX + half-height of cylindrical portion, must + // be >= SizeX). This parameterization allows joint offsets + // to be SizeY, and direct swapping of shape across Box and + // Cylinder with same total extent. Capsule // todo: Ellipsoid goes here // Cylinder, natively oriented vertically along the Y axis. - // SizeX = radius, SizeY = half-height in Y axis. + // SizeX = radius, SizeY = half-height of Y axis // Cylinder can not collide with a Box. Cylinder @@ -123,8 +127,10 @@ func (sh Shapes) Radius(sz math32.Vector3) float32 { return 1.0e6 // infinite case Sphere: return sz.X - case Capsule, Cylinder: - return max(sz.X, sz.Z) + sz.Y // over-estimate for cylinder + case Capsule: + return sz.Y // full half-height + case Cylinder: + return sz.X + sz.Y // over-estimate for cylinder case Box: return sz.Length() } @@ -138,9 +144,9 @@ func (sh Shapes) BBox(sz math32.Vector3) math32.Box3 { case Sphere: bb.SetMinMax(math32.Vec3(-sz.X, -sz.X, -sz.X), math32.Vec3(sz.X, sz.X, sz.X)) case Capsule: - bb.SetMinMax(math32.Vec3(-sz.X, -sz.Y-sz.X, -sz.X), math32.Vec3(sz.Z, sz.Y+sz.Z, sz.Z)) + bb.SetMinMax(math32.Vec3(-sz.X, -sz.Y, -sz.X), math32.Vec3(sz.X, sz.Y, sz.X)) case Cylinder: - bb.SetMinMax(math32.Vec3(-sz.X, -sz.Y, -sz.X), math32.Vec3(sz.Z, sz.Y, sz.Z)) + bb.SetMinMax(math32.Vec3(-sz.X, -sz.Y, -sz.X), math32.Vec3(sz.X, sz.Y, sz.X)) case Box: bb.SetMinMax(sz.Negate(), sz) } @@ -160,7 +166,7 @@ func (sh Shapes) Inertia(sz math32.Vector3, mass float32) math32.Matrix3 { inertia = math32.Mat3(ia, 0.0, 0.0, 0.0, ia, 0.0, 0.0, 0.0, ia) case Capsule: r := sz.X - h := sz.Y * 2 + h := (sz.Y - sz.X) * 2 vs := (4.0 / 3.0) * math32.Pi * r * r * r vc := math32.Pi * r * r * h ms := mass * (vs / (vs + vc)) diff --git a/physics/typegen.go b/physics/typegen.go index 49ed7691..62be9042 100644 --- a/physics/typegen.go +++ b/physics/typegen.go @@ -22,7 +22,7 @@ var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.JointVars", var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.JointDoFVars", IDName: "joint-do-f-vars", Doc: "JointDoFVars are joint DoF state variables stored in tensor.Float32,\none for each DoF."}) -var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.Model", IDName: "model", Doc: "Model contains and manages all of the physics elements.", Directives: []types.Directive{{Tool: "go", Directive: "generate", Args: []string{"core", "generate", "-add-types", "-gosl"}}}, Fields: []types.Field{{Name: "GPU", Doc: "GPU determines whether to use GPU (else CPU)."}, {Name: "Params", Doc: "Params are global parameters."}, {Name: "CurrentWorld", Doc: "CurrentWorld is the [BodyWorld] value to use when creating new bodies.\nSet to -1 to create global elements that interact with everything,\nwhile 0 and positive numbers only interact amongst themselves."}, {Name: "CurrentObject", Doc: "CurrentObject is the Object to use when creating new joints.\nCall NewObject to increment."}, {Name: "CurrentObjectJoint", Doc: "CurrentObjectJoint is the Joint index in CurrentObject\nto use when creating new joints."}, {Name: "ReplicasStart", Doc: "ReplicasStart is the starting body index for replicated world bodies,\nwhich is needed for viewers to efficiently select a specific world to view.\nThis is the start of the World=0 first instance."}, {Name: "ReplicasN", Doc: "ReplicasN is the number of body elements within each set of\nreplicated world bodies, which is needed for viewers to efficiently select\na specific world to view."}, {Name: "Bodies", Doc: "Bodies are the rigid body elements (dynamic and static),\nspecifying the constant, non-dynamic properties,\nwhich is initial state for dynamics.\n[body][BodyVarsN]"}, {Name: "Objects", Doc: "Objects is a list of joint indexes for each object, where each object\ncontains all the joints interconnecting an overlapping set of bodies.\nThis is known as an articulation in other physics software.\nJoints must be added in parent -> child order within objects, as joints\nare updated in sequential order within object.\n[object][MaxObjectJoints+1]"}, {Name: "BodyJoints", Doc: "BodyJoints is a list of joint indexes for each dynamic body, for aggregating.\n[dyn body][parent, child][Params.BodyJointsMax]"}, {Name: "Joints", Doc: "Joints is a list of permanent joints connecting bodies,\nwhich do not change (no dynamic variables).\n[joint][JointVarsN]"}, {Name: "JointDoFs", Doc: "JointDoFs is a list of joint DoF parameters, allocated per joint.\n[dof][JointDoFVars]"}, {Name: "BodyCollidePairs", Doc: "BodyCollidePairs are pairs of Body indexes that could potentially collide\nbased on precomputed collision logic, using World, Group, and Joint indexes.\n[BodyCollidePairsN][2]"}, {Name: "Dynamics", Doc: "Dynamics are the dynamic rigid body elements: these actually move.\nThe first set of variables are for initial values, and the second current.\n[body][cur/next][DynamicVarsN]"}, {Name: "BroadContactsN", Doc: "BroadContactsN has number of points of broad contact\nbetween bodies. [1]"}, {Name: "BroadContacts", Doc: "BroadContacts are the results of broad-phase contact processing,\nestablishing possible points of contact between bodies.\n[ContactsMax][BroadContactVarsN]"}, {Name: "ContactsN", Doc: "ContactsN has number of points of narrow (final) contact\nbetween bodies. [1]"}, {Name: "Contacts", Doc: "Contacts are the results of narrow-phase contact processing,\nwhere only actual contacts with fully-specified values are present.\n[ContactsMax][ContactVarsN]"}, {Name: "JointControls", Doc: "JointControls are dynamic joint control inputs, per joint DoF\n(in correspondence with [JointDoFs]). This can be uploaded to the\nGPU at every step.\n[dof][JointControlVarsN]"}}}) +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.Model", IDName: "model", Doc: "Model contains and manages all of the physics elements.", Directives: []types.Directive{{Tool: "go", Directive: "generate", Args: []string{"core", "generate", "-add-types", "-gosl"}}}, Fields: []types.Field{{Name: "GPU", Doc: "GPU determines whether to use GPU (else CPU)."}, {Name: "Params", Doc: "Params are global parameters."}, {Name: "GetContacts", Doc: "GetContacts will download Contacts from the GPU, if processing them on the CPU."}, {Name: "CurrentWorld", Doc: "CurrentWorld is the [BodyWorld] value to use when creating new bodies.\nSet to -1 to create global elements that interact with everything,\nwhile 0 and positive numbers only interact amongst themselves."}, {Name: "CurrentObject", Doc: "CurrentObject is the Object to use when creating new joints.\nCall NewObject to increment."}, {Name: "CurrentObjectJoint", Doc: "CurrentObjectJoint is the Joint index in CurrentObject\nto use when creating new joints."}, {Name: "ReplicasStart", Doc: "ReplicasStart is the starting body index for replicated world bodies,\nwhich is needed for viewers to efficiently select a specific world to view.\nThis is the start of the World=0 first instance."}, {Name: "ReplicasN", Doc: "ReplicasN is the number of body elements within each set of\nreplicated world bodies, which is needed for viewers to efficiently select\na specific world to view."}, {Name: "Bodies", Doc: "Bodies are the rigid body elements (dynamic and static),\nspecifying the constant, non-dynamic properties,\nwhich is initial state for dynamics.\n[body][BodyVarsN]"}, {Name: "Objects", Doc: "Objects is a list of joint indexes for each object, where each object\ncontains all the joints interconnecting an overlapping set of bodies.\nThis is known as an articulation in other physics software.\nJoints must be added in parent -> child order within objects, as joints\nare updated in sequential order within object.\n[object][MaxObjectJoints+1]"}, {Name: "BodyJoints", Doc: "BodyJoints is a list of joint indexes for each dynamic body, for aggregating.\n[dyn body][parent, child][Params.BodyJointsMax]"}, {Name: "Joints", Doc: "Joints is a list of permanent joints connecting bodies,\nwhich do not change (no dynamic variables).\n[joint][JointVarsN]"}, {Name: "JointDoFs", Doc: "JointDoFs is a list of joint DoF parameters, allocated per joint.\n[dof][JointDoFVars]"}, {Name: "BodyCollidePairs", Doc: "BodyCollidePairs are pairs of Body indexes that could potentially collide\nbased on precomputed collision logic, using World, Group, and Joint indexes.\n[BodyCollidePairsN][2]"}, {Name: "Dynamics", Doc: "Dynamics are the dynamic rigid body elements: these actually move.\nThe first set of variables are for initial values, and the second current.\n[body][cur/next][DynamicVarsN]"}, {Name: "BroadContactsN", Doc: "BroadContactsN has number of points of broad contact\nbetween bodies. [1]"}, {Name: "BroadContacts", Doc: "BroadContacts are the results of broad-phase contact processing,\nestablishing possible points of contact between bodies.\n[ContactsMax][BroadContactVarsN]"}, {Name: "ContactsN", Doc: "ContactsN has number of points of narrow (final) contact\nbetween bodies. [1]"}, {Name: "Contacts", Doc: "Contacts are the results of narrow-phase contact processing,\nwhere only actual contacts with fully-specified values are present.\n[ContactsMax][ContactVarsN]"}, {Name: "JointControls", Doc: "JointControls are dynamic joint control inputs, per joint DoF\n(in correspondence with [JointDoFs]). This can be uploaded to the\nGPU at every step.\n[dof][JointControlVarsN]"}}}) var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.PhysicsParams", IDName: "physics-params", Doc: "PhysicsParams are the physics parameters", Directives: []types.Directive{{Tool: "gosl", Directive: "start"}}, Fields: []types.Field{{Name: "Iterations", Doc: "Iterations is the number of integration iterations to perform\nwithin each solver step. Muller et al (2020) report that 1 is best."}, {Name: "Dt", Doc: "Dt is the integration stepsize.\nFor highly kinetic situations (e.g., rapidly moving bouncing balls)\n0.0001 is needed to ensure contact registration. Use SubSteps to\naccomplish a target effective read-out step size."}, {Name: "SubSteps", Doc: "SubSteps is the number of integration steps to take per Step()\nfunction call. These sub steps are taken without any sync to/from\nthe GPU and are therefore much faster."}, {Name: "ControlDt", Doc: "ControlDt is the stepsize for integrating joint control position values\n[JointTargetPos] over time, to avoid sudden strong changes in force.\nFor higher-DoF joints (e.g., Ball), this can be important for stability,\nbut it can also result in under-shoot of the target position."}, {Name: "ControlDtThr", Doc: "ControlDtThr is the threshold on the control delta above which\nControlDt is used. ControlDt is most important for large changes,\nand can result in under-shoot if engaged for small changes."}, {Name: "ContactMargin", Doc: "Contact margin is the extra distance for broadphase collision\naround rigid bodies. This can make some joints potentially unstable if > 0"}, {Name: "ContactRelax", Doc: "ContactRelax is rigid contact relaxation constant.\nHigher values cause errros"}, {Name: "ContactWeighting", Doc: "Contact weighting: balances contact forces?"}, {Name: "Restitution", Doc: "Restitution takes into account bounciness of objects."}, {Name: "JointLinearRelax", Doc: "JointLinearRelax is joint linear relaxation constant."}, {Name: "JointAngularRelax", Doc: "JointAngularRelax is joint angular relaxation constant."}, {Name: "JointLinearComply", Doc: "JointLinearComply is joint linear compliance constant."}, {Name: "JointAngularComply", Doc: "JointAngularComply is joint angular compliance constant."}, {Name: "AngularDamping", Doc: "AngularDamping is damping of angular motion."}, {Name: "SoftRelax", Doc: "SoftRelax is soft-body relaxation constant."}, {Name: "MaxForce", Doc: "MaxForce is the maximum computed force value, which prevents\nrunaway numerical overflow."}, {Name: "MaxDelta", Doc: "MaxDelta is the maximum computed change in position magnitude,\nwhich prevents runaway numerical overflow."}, {Name: "MaxGeomIter", Doc: "MaxGeomIter is number of iterations to perform in shape-based\ngeometry collision computations"}, {Name: "ContactsMax", Doc: "Maximum number of contacts to process at any given point."}, {Name: "Cur", Doc: "Index for the current state (0 or 1, alternates with Next)."}, {Name: "Next", Doc: "Index for the next state (1 or 0, alternates with Cur)."}, {Name: "BodiesN", Doc: "BodiesN is number of rigid bodies."}, {Name: "DynamicsN", Doc: "DynamicsN is number of dynamics bodies."}, {Name: "ObjectsN", Doc: "ObjectsN is number of objects."}, {Name: "MaxObjectJoints", Doc: "MaxObjectJoints is max number of joints per object."}, {Name: "JointsN", Doc: "JointsN is number of joints."}, {Name: "JointDoFsN", Doc: "JointDoFsN is number of joint DoFs."}, {Name: "BodyJointsMax", Doc: "BodyJointsMax is max number of joints per body + 1 for actual n."}, {Name: "BodyCollidePairsN", Doc: "BodyCollidePairsN is the total number of pre-compiled collision pairs\nto examine."}, {Name: "pad"}, {Name: "pad1"}, {Name: "pad2"}, {Name: "Gravity", Doc: "Gravity is the gravity acceleration function"}}}) From 0992f5660bbab6c811f711e00a7bd52ae93b1ef7 Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Mon, 5 Jan 2026 04:16:20 -0800 Subject: [PATCH 82/97] physics: cylinder-plane collision working, other collision updates --- physics/examples/collision/collision.go | 50 +++++++++- physics/examples/collision/typegen.go | 9 ++ physics/model.go | 2 +- physics/model.goal | 2 +- physics/phyxyz/skin.go | 9 +- physics/shaders/CollisionNarrow.wgsl | 11 ++- physics/shapecollide.go | 18 ++-- physics/shapecollide.goal | 18 ++-- physics/shapegeom_test.go | 126 ++++++++++++++++++++++-- 9 files changed, 208 insertions(+), 37 deletions(-) create mode 100644 physics/examples/collision/typegen.go diff --git a/physics/examples/collision/collision.go b/physics/examples/collision/collision.go index 5dd07d64..20adfd31 100644 --- a/physics/examples/collision/collision.go +++ b/physics/examples/collision/collision.go @@ -16,19 +16,40 @@ import ( // Collide has sim params type Collide struct { + // Shape of left body ShapeA physics.Shapes + + // Shape of right body ShapeB physics.Shapes + // Size of left body (radius, capsule, cylinder, box are 2x taller) SizeA float32 + + // Size of right body (radius, capsule, cylinder, box are 2x taller) SizeB float32 + // Mass of left object: if lighter than B, it will bounce back more. MassA float32 + + // Mass of right object: if lighter than B, it will move faster. MassB float32 + // Z (depth) position: offset to get different collision angles. ZposA float32 + + // Z (depth) position: offset to get different collision angles. ZposB float32 + // Mass of the pusher panel: if lighter, it transfers less energy. PushMass float32 + + // Friction is for sliding: around 0.01 seems pretty realistic + Friction float32 + + FrictionTortion float32 + + // FrictionRolling is for rolling: around 0.01 seems pretty realistic + FrictionRolling float32 } func (cl *Collide) Defaults() { @@ -39,6 +60,8 @@ func (cl *Collide) Defaults() { cl.MassA = 1 cl.MassB = 1 cl.PushMass = 1 + cl.Friction = 0.01 + cl.FrictionRolling = 0.01 } func main() { @@ -61,10 +84,29 @@ func main() { ml.GPU = false sc := ed.Scene rot := math32.NewQuatIdentity() - sc.NewBody(ml, "floor", physics.Plane, "#D0D0D080", math32.Vec3(0, 0, 0), math32.Vec3(0, 0, 0), rot) - - sc.NewDynamic(ml, "A", cl.ShapeA, "blue", cl.MassA, math32.Vec3(cl.SizeA, cl.SizeA, cl.SizeA), math32.Vec3(-5, cl.SizeA, cl.ZposA), rot) - sc.NewDynamic(ml, "B", cl.ShapeB, "red", cl.MassB, math32.Vec3(cl.SizeB, cl.SizeB, cl.SizeB), math32.Vec3(5, cl.SizeB, cl.ZposB), rot) + fl := sc.NewBody(ml, "floor", physics.Plane, "#D0D0D080", math32.Vec3(0, 0, 0), math32.Vec3(0, 0, 0), rot) + physics.SetBodyFriction(fl.BodyIndex, cl.Friction) + physics.SetBodyFrictionRolling(fl.BodyIndex, cl.FrictionRolling) + physics.SetBodyFrictionTortion(fl.BodyIndex, cl.FrictionTortion) + + hhA := 2 * cl.SizeA + hhB := 2 * cl.SizeB + if cl.ShapeA == physics.Sphere { + hhA = cl.SizeA + } + if cl.ShapeB == physics.Sphere { + hhB = cl.SizeB + } + + ba := sc.NewDynamic(ml, "A", cl.ShapeA, "blue", cl.MassA, math32.Vec3(cl.SizeA, 2*cl.SizeA, cl.SizeA), math32.Vec3(-5, hhA, cl.ZposA), rot) + physics.SetBodyFriction(ba.BodyIndex, cl.Friction) + physics.SetBodyFrictionRolling(ba.BodyIndex, cl.FrictionRolling) + physics.SetBodyFrictionTortion(ba.BodyIndex, cl.FrictionTortion) + + bb := sc.NewDynamic(ml, "B", cl.ShapeB, "red", cl.MassB, math32.Vec3(cl.SizeB, 2*cl.SizeB, cl.SizeB), math32.Vec3(5, hhB, cl.ZposB), rot) + physics.SetBodyFriction(bb.BodyIndex, cl.Friction) + physics.SetBodyFrictionRolling(bb.BodyIndex, cl.FrictionRolling) + physics.SetBodyFrictionTortion(bb.BodyIndex, cl.FrictionTortion) push := sc.NewDynamic(ml, "push", physics.Box, "grey", cl.PushMass, math32.Vec3(.1, 2, 2), math32.Vec3(-8, 2, 0), rot) ml.NewObject() diff --git a/physics/examples/collision/typegen.go b/physics/examples/collision/typegen.go new file mode 100644 index 00000000..9df92715 --- /dev/null +++ b/physics/examples/collision/typegen.go @@ -0,0 +1,9 @@ +// Code generated by "core generate -add-types"; DO NOT EDIT. + +package main + +import ( + "cogentcore.org/core/types" +) + +var _ = types.AddType(&types.Type{Name: "main.Collide", IDName: "collide", Doc: "Collide has sim params", Fields: []types.Field{{Name: "ShapeA", Doc: "Shape of left body"}, {Name: "ShapeB", Doc: "Shape of right body"}, {Name: "SizeA", Doc: "Size of left body (radius, capsule, cylinder, box are 2x taller)"}, {Name: "SizeB", Doc: "Size of right body (radius, capsule, cylinder, box are 2x taller)"}, {Name: "MassA", Doc: "Mass of left object: if lighter than B, it will bounce back more."}, {Name: "MassB", Doc: "Mass of right object: if lighter than B, it will move faster."}, {Name: "ZposA", Doc: "Z (depth) position: offset to get different collision angles."}, {Name: "ZposB", Doc: "Z (depth) position: offset to get different collision angles."}, {Name: "PushMass", Doc: "Mass of the pusher panel: if lighter, it transfers less energy."}, {Name: "Friction", Doc: "Friction is for sliding: around 0.01 seems pretty realistic"}, {Name: "FrictionTortion"}, {Name: "FrictionRolling", Doc: "FrictionRolling is for rolling: around 0.01 seems pretty realistic"}}}) diff --git a/physics/model.go b/physics/model.go index 0313a275..fbc75256 100644 --- a/physics/model.go +++ b/physics/model.go @@ -167,7 +167,7 @@ func (ml *Model) NewBody(shape Shapes, hsize, pos math32.Vector3, rot math32.Qua SetBodyShape(idx, shape) SetBodyDynamic(idx, -1) if shape == Capsule { - hsize.Y = max(hsize.Y, hsize.X) + hsize.Y = max(hsize.Y, hsize.X*1.01) } SetBodyHSize(idx, hsize) SetBodyPos(idx, pos) diff --git a/physics/model.goal b/physics/model.goal index 80c6afa2..d31c6be6 100644 --- a/physics/model.goal +++ b/physics/model.goal @@ -165,7 +165,7 @@ func (ml *Model) NewBody(shape Shapes, hsize, pos math32.Vector3, rot math32.Qua SetBodyShape(idx, shape) SetBodyDynamic(idx, -1) if shape == Capsule { - hsize.Y = max(hsize.Y, hsize.X) + hsize.Y = max(hsize.Y, hsize.X*1.01) } SetBodyHSize(idx, hsize) SetBodyPos(idx, pos) diff --git a/physics/phyxyz/skin.go b/physics/phyxyz/skin.go index 133632bf..3237c886 100644 --- a/physics/phyxyz/skin.go +++ b/physics/phyxyz/skin.go @@ -5,6 +5,7 @@ package phyxyz import ( + "fmt" "strconv" "cogentcore.org/core/base/errors" @@ -196,12 +197,14 @@ func (sk *Skin) CylinderInit(sld *xyz.Solid) { // Only updates Pose in Updater: if node will change size or color, // add updaters for that. func (sk *Skin) CapsuleInit(sld *xyz.Solid) { - mnm := "physics.Capsule" + rat := sk.HSize.Y / sk.HSize.X + mnm := fmt.Sprintf("physics.Capsule_%3g", rat) if ms, _ := sld.Scene.MeshByName(mnm); ms == nil { - ms = xyz.NewCapsule(sld.Scene, mnm, 1, .2, 32, 1) + fmt.Println(rat, mnm, 2*(sk.HSize.Y-sk.HSize.X)/sk.HSize.X) + ms = xyz.NewCapsule(sld.Scene, mnm, 2*(sk.HSize.Y-sk.HSize.X)/sk.HSize.X, 1, 32, 1) } sld.SetMeshName(mnm) - sld.Pose.Scale.Set(sk.HSize.X/.2, (2.0*sk.HSize.Y)/1.4, sk.HSize.X/.2) + sld.Pose.Scale.Set(sk.HSize.X, sk.HSize.X, sk.HSize.X) sk.UpdateColor(sk.Color, sld) sld.Updater(func() { sk.UpdatePose(sld) diff --git a/physics/shaders/CollisionNarrow.wgsl b/physics/shaders/CollisionNarrow.wgsl index 75fd0c41..dfc05f93 100644 --- a/physics/shaders/CollisionNarrow.wgsl +++ b/physics/shaders/CollisionNarrow.wgsl @@ -573,7 +573,7 @@ return Dot3(diff, *norm); } fn ColCapsuleCapsule(cpi: i32,maxIter: i32, gdA: ptr, gdB: ptr, pA: ptr>,pB: ptr>,norm: ptr>) -> f32 { var hhA = (*gdA).Size.y - (*gdA).Size.x; - var hhB = (*gdB).Size.y - (*gdA).Size.x; + var hhB = (*gdB).Size.y - (*gdB).Size.x; var e0 = vec3(0, 0, hhA*f32(cpi%2)); var e1 = vec3(0, 0, -hhA*f32((cpi+1)%2)); var edge0w = MulSpatialPoint((*gdA).WbR, (*gdA).WbQ, e0); @@ -716,7 +716,7 @@ return Dot3(diff, *norm); fn ColCylinderPlane(cpi: i32,maxIter: i32, gdA: ptr, gdB: ptr, pA: ptr>,pB: ptr>,norm: ptr>) -> f32 { var plNorm = MulQuatVector((*gdB).WbQ, vec3(0, 1, 0)); var plPos = MulSpatialPoint((*gdB).WbR, (*gdB).WbQ, vec3(0, 0, 0)); - var cylCtr = MulSpatialPoint((*gdA).WbR, (*gdA).WbQ, vec3(0, 0, 0)); + var cylCtr = (*gdA).WbR; var cylAx = Normal3(MulQuatVector((*gdA).WbQ, vec3(0, 1, 0))); var cylRad = (*gdA).Size.x; var cylHh = (*gdA).Size.y; @@ -731,6 +731,12 @@ fn ColCylinderPlane(cpi: i32,maxIter: i32, gdA: ptr, gdB: ptr } var dist0 = Dot3(cylCtr-(plPos), n); var vec = axis*(prjaxis)-(n); + var lenSqr = Dot3(vec, vec); + if (lenSqr >= 1e-12) { + vec = vec*(cylRad / sqrt(lenSqr)); + } else { + vec = vec3(1, 0, 0)*(cylRad); // Default x-axis when degenerate + } var prjvec = Dot3(vec, n); axis = axis*(cylHh); prjaxis *= cylHh; @@ -759,6 +765,7 @@ fn ColCylinderPlane(cpi: i32,maxIter: i32, gdA: ptr, gdB: ptr } *pA = pos+(n*(dist * 0.5)); *pB = pos-(n*(dist * 0.5)); + *norm = n; return dist; } diff --git a/physics/shapecollide.go b/physics/shapecollide.go index 42a1912f..73f41c42 100644 --- a/physics/shapecollide.go +++ b/physics/shapecollide.go @@ -159,7 +159,7 @@ func ColCapsulePlane(cpi, maxIter int32, gdA *GeomData, gdB *GeomData, pA, pB, n func ColCapsuleCapsule(cpi, maxIter int32, gdA *GeomData, gdB *GeomData, pA, pB, norm *math32.Vector3) float32 { // find closest edge coordinate to capsule SDF B hhA := gdA.Size.Y - gdA.Size.X - hhB := gdB.Size.Y - gdA.Size.X + hhB := gdB.Size.Y - gdB.Size.X // edge from capsule A // depending on point id, we query an edge from 0 to 0.5 or 0.5 to 1 e0 := math32.Vec3(0, 0, hhA*float32(cpi%2)) @@ -341,7 +341,7 @@ func ColCylinderPlane(cpi, maxIter int32, gdA *GeomData, gdB *GeomData, pA, pB, plPos := slmath.MulSpatialPoint(gdB.WbR, gdB.WbQ, math32.Vec3(0, 0, 0)) // World-space cylinder params - cylCtr := slmath.MulSpatialPoint(gdA.WbR, gdA.WbQ, math32.Vec3(0, 0, 0)) + cylCtr := gdA.WbR cylAx := slmath.Normal3(slmath.MulQuatVector(gdA.WbQ, math32.Vec3(0, 1, 0))) cylRad := gdA.Size.X cylHh := gdA.Size.Y @@ -364,16 +364,15 @@ func ColCylinderPlane(cpi, maxIter int32, gdA *GeomData, gdB *GeomData, pA, pB, // Remove component of -normal along cylinder axis vec := axis.MulScalar(prjaxis).Sub(n) - // len_sqr := slmath.Dot3(vec, vec) + lenSqr := slmath.Dot3(vec, vec) // If vector is nondegenerate, normalize and scale by radius // Otherwise use cylinder's x-axis scaled by radius - // todo: - // vec = wp.where( - // len_sqr >= 1e-12, - // vec * safe_div(cylinder_radius, wp.sqrt(len_sqr)), - // math32.Vec3(1, 0, 0).MuScalar(cylRad), // Default x-axis when degenerate - // ) + if lenSqr >= 1e-12 { + vec = vec.MulScalar(cylRad / math32.Sqrt(lenSqr)) + } else { + vec = math32.Vec3(1, 0, 0).MulScalar(cylRad) // Default x-axis when degenerate + } // Project scaled vector on normal prjvec := slmath.Dot3(vec, n) @@ -406,6 +405,7 @@ func ColCylinderPlane(cpi, maxIter int32, gdA *GeomData, gdB *GeomData, pA, pB, // Split midpoint into shape-plane endpoints *pA = pos.Add(n.MulScalar(dist * 0.5)) *pB = pos.Sub(n.MulScalar(dist * 0.5)) + *norm = n return dist } diff --git a/physics/shapecollide.goal b/physics/shapecollide.goal index 5884479b..4317432b 100644 --- a/physics/shapecollide.goal +++ b/physics/shapecollide.goal @@ -157,7 +157,7 @@ func ColCapsulePlane(cpi, maxIter int32, gdA *GeomData, gdB *GeomData, pA, pB, n func ColCapsuleCapsule(cpi, maxIter int32, gdA *GeomData, gdB *GeomData, pA, pB, norm *math32.Vector3) float32 { // find closest edge coordinate to capsule SDF B hhA := gdA.Size.Y - gdA.Size.X - hhB := gdB.Size.Y - gdA.Size.X + hhB := gdB.Size.Y - gdB.Size.X // edge from capsule A // depending on point id, we query an edge from 0 to 0.5 or 0.5 to 1 e0 := math32.Vec3(0, 0, hhA*float32(cpi%2)) @@ -338,7 +338,7 @@ func ColCylinderPlane(cpi, maxIter int32, gdA *GeomData, gdB *GeomData, pA, pB, plPos := slmath.MulSpatialPoint(gdB.WbR, gdB.WbQ, math32.Vec3(0, 0, 0)) // World-space cylinder params - cylCtr := slmath.MulSpatialPoint(gdA.WbR, gdA.WbQ, math32.Vec3(0, 0, 0)) + cylCtr := gdA.WbR cylAx := slmath.Normal3(slmath.MulQuatVector(gdA.WbQ, math32.Vec3(0, 1, 0))) cylRad := gdA.Size.X cylHh := gdA.Size.Y @@ -361,16 +361,15 @@ func ColCylinderPlane(cpi, maxIter int32, gdA *GeomData, gdB *GeomData, pA, pB, // Remove component of -normal along cylinder axis vec := axis.MulScalar(prjaxis).Sub(n) - // len_sqr := slmath.Dot3(vec, vec) + lenSqr := slmath.Dot3(vec, vec) // If vector is nondegenerate, normalize and scale by radius // Otherwise use cylinder's x-axis scaled by radius - // todo: - // vec = wp.where( - // len_sqr >= 1e-12, - // vec * safe_div(cylinder_radius, wp.sqrt(len_sqr)), - // math32.Vec3(1, 0, 0).MuScalar(cylRad), // Default x-axis when degenerate - // ) + if lenSqr >= 1e-12 { + vec = vec.MulScalar(cylRad / math32.Sqrt(lenSqr)) + } else { + vec = math32.Vec3(1, 0, 0).MulScalar(cylRad) // Default x-axis when degenerate + } // Project scaled vector on normal prjvec := slmath.Dot3(vec, n) @@ -403,6 +402,7 @@ func ColCylinderPlane(cpi, maxIter int32, gdA *GeomData, gdB *GeomData, pA, pB, // Split midpoint into shape-plane endpoints *pA = pos.Add(n.MulScalar(dist * 0.5)) *pB = pos.Sub(n.MulScalar(dist * 0.5)) + *norm = n return dist } diff --git a/physics/shapegeom_test.go b/physics/shapegeom_test.go index bcfab8b3..041c4352 100644 --- a/physics/shapegeom_test.go +++ b/physics/shapegeom_test.go @@ -5,7 +5,6 @@ package physics import ( - "fmt" "testing" "cogentcore.org/core/math32" @@ -83,12 +82,12 @@ func TestSpherePlane(t *testing.T) { normal, posA, posB math32.Vector3 radius, dist float32 }{ - {math32.Vector3{0, 1, 0}, math32.Vector3{0, 0, 0}, math32.Vector3{0, 2, 0}, 1, 1}, - {math32.Vector3{0, 1, 0}, math32.Vector3{0, 0, 0}, math32.Vector3{0, 1.5, 0}, 1, 0.5}, - {math32.Vector3{0, 1, 0}, math32.Vector3{0, 0, 0}, math32.Vector3{0, 1, 0}, 1, 0}, - {math32.Vector3{0, 1, 0}, math32.Vector3{0, 0, 0}, math32.Vector3{0, 0.8, 0}, 1, -0.2}, - {math32.Vector3{0, 1, 0}, math32.Vector3{0, 0, 0}, math32.Vector3{0, 0.5, 0}, 1, -0.5}, - {math32.Vector3{0, 1, 0}, math32.Vector3{0, 0, 0}, math32.Vector3{0, 0.2, 0}, 1, -0.8}, + {math32.Vector3{0, 1, 0}, math32.Vector3{0, 0, 0}, math32.Vector3{2, 2, 0}, 1, 1}, + {math32.Vector3{0, 1, 0}, math32.Vector3{0, 0, 0}, math32.Vector3{2, 1.5, 0}, 1, 0.5}, + {math32.Vector3{0, 1, 0}, math32.Vector3{0, 0, 0}, math32.Vector3{2, 1, 0}, 1, 0}, + {math32.Vector3{0, 1, 0}, math32.Vector3{0, 0, 0}, math32.Vector3{2, 0.8, 0}, 1, -0.2}, + {math32.Vector3{0, 1, 0}, math32.Vector3{0, 0, 0}, math32.Vector3{2, 0.5, 0}, 1, -0.5}, + {math32.Vector3{0, 1, 0}, math32.Vector3{0, 0, 0}, math32.Vector3{2, 0.2, 0}, 1, -0.8}, // {math32.Vector3{1, 0, 0}, math32.Vector3{1, 0, 0}, math32.Vector3{2.0, 0, 0}, 0.5, 0.5}, // X-axis, separation = 0.5 // {math32.Vector3{1, 0, 0}, math32.Vector3{1, 0, 0}, math32.Vector3{1.5, 0, 0}, 0.5, 0}, // X-axis, touching // {math32.Vector3{1, 0, 0}, math32.Vector3{1, 0, 0}, math32.Vector3{1.3, 0, 0}, 0.5, -0.2}, // X-axis, penetration = 0.2 @@ -112,7 +111,7 @@ func TestSpherePlane(t *testing.T) { actual := ContactPoints(dist, margin, &gdA, &gdB, ptA, ptB, norm, &ctA, &ctB, &offA, &offB, &distActual, &offMagA, &offMagB) _ = actual - fmt.Println(dist, distActual, tc.dist, actual, norm, ptA, ptB) + // fmt.Println(dist, distActual, tc.dist, actual, norm, ptA, ptB) // if actual { // fmt.Println(ptA, ptB, ctA, ctB, offA, offB, offMagA, offMagB) // } @@ -184,3 +183,114 @@ func TestCapsulePlane(t *testing.T) { // fmt.Println(cpA, cpB, tc.dist, actual) } } + +func TestCylinderPlane(t *testing.T) { + // note: this data is already configured as A = plane, B = capsule, but function is CapsulePlane + // so we're switching below.. + tests := []struct { + normal, posA, posB math32.Vector3 + radius, dist float32 + }{ + {math32.Vector3{0, 1, 0}, math32.Vector3{0, 0, 0}, math32.Vector3{0, 2.5, 0}, 0.5, 1.5}, + {math32.Vector3{0, 1, 0}, math32.Vector3{0, 0, 0}, math32.Vector3{0, 2.0, 0}, 0.5, 1.0}, + {math32.Vector3{0, 1, 0}, math32.Vector3{0, 0, 0}, math32.Vector3{0, 1.5, 0}, 0.5, 0.5}, + {math32.Vector3{0, 1, 0}, math32.Vector3{0, 0, 0}, math32.Vector3{0, 1.0, 0}, 0.5, 0}, + {math32.Vector3{0, 1, 0}, math32.Vector3{0, 0, 0}, math32.Vector3{0, 0.9, 0}, 0.5, -0.1}, + {math32.Vector3{0, 1, 0}, math32.Vector3{0, 0, 0}, math32.Vector3{0, 0.8, 0}, 0.5, -0.2}, + {math32.Vector3{0, 1, 0}, math32.Vector3{0, 0, 0}, math32.Vector3{0, 0.7, 0}, 0.5, -0.3}, + } + tol := 1e-5 + + // rleft := math32.NewQuatAxisAngle(math32.Vec3(0, 0, 1), -math32.Pi/2) + rot := math32.NewQuatIdentity() + for _, tc := range tests { + // note: A = capsule but pos = B.. + // 1 hh + gdA := GeomData{Shape: Cylinder, Radius: tc.radius, Size: math32.Vector3{tc.radius, 1.0, tc.radius}, WbR: tc.posB, WbQ: rot} + gdB := GeomData{Shape: Plane, Size: math32.Vector3{0, 0, 0}, WbR: tc.posA, WbQ: rot} + InitGeomData(0, &gdA) + InitGeomData(0, &gdB) + + var ptA, ptB, norm math32.Vector3 + // important: we know that the lower axis, cpi = 0, is closest here + var dist float32 + // for cpi := range int32(4) { + dist = ColCylinderPlane(0, 10, &gdA, &gdB, &ptA, &ptB, &norm) + // fmt.Println(cpi, dist, ptA, ptB, norm) + // } + margin := float32(0.01) + + var ctA, ctB, offA, offB math32.Vector3 + var distActual, offMagA, offMagB float32 + actual := ContactPoints(dist, margin, &gdA, &gdB, ptA, ptB, norm, &ctA, &ctB, &offA, &offB, &distActual, &offMagA, &offMagB) + _ = actual + + // fmt.Println(dist, distActual, tc.dist, actual, norm, ptA, ptB) + // if actual { + // fmt.Println(ptA, ptB, ctA, ctB, offA, offB, offMagA, offMagB) + // } + + assert.InDelta(t, tc.dist, distActual, tol) + assert.InDelta(t, 1.0, norm.Length(), tol) + assert.Equal(t, distActual < margin, actual) + + if !actual { + continue + } + // cpA := ctA.Add(offA) + // cpB := ctB.Add(offB) + // fmt.Println(cpA, cpB, tc.dist, actual) + } +} + +func TestSphereCapsule(t *testing.T) { + tests := []struct { + posA, posB math32.Vector3 + radiusA, radiusB, dist float32 + }{ + {math32.Vector3{0, 0, 0}, math32.Vector3{0, 4, 0}, 1.0, 0.5, 1.5}, + {math32.Vector3{0, 0, 0}, math32.Vector3{0, 3.5, 0}, 1.0, 0.5, 1.0}, + {math32.Vector3{0, 0, 0}, math32.Vector3{0, 3.0, 0}, 1.0, 0.5, 0.5}, + {math32.Vector3{0, 0, 0}, math32.Vector3{0, 2.5, 0}, 1.0, 0.5, 0}, + {math32.Vector3{0, 0, 0}, math32.Vector3{0, 2.4, 0}, 1.0, 0.5, -0.1}, + {math32.Vector3{0, 0, 0}, math32.Vector3{0, 2.3, 0}, 1.0, 0.5, -0.2}, + {math32.Vector3{0, 0, 0}, math32.Vector3{0, 2.2, 0}, 1.0, 0.5, -0.3}, + } + tol := 1e-5 + + rot := math32.NewQuatIdentity() + for _, tc := range tests { + // note: A = capsule but pos = B.. + // 1.5 hh = 1 raw hh + gdA := GeomData{Shape: Sphere, Radius: tc.radiusA, Size: math32.Vector3{tc.radiusA, 0, 0}, WbR: tc.posA, WbQ: rot} + gdB := GeomData{Shape: Capsule, Radius: tc.radiusB, Size: math32.Vector3{tc.radiusB, 1.5, tc.radiusB}, WbR: tc.posB, WbQ: rot} + InitGeomData(0, &gdA) + InitGeomData(0, &gdB) + + var ptA, ptB, norm math32.Vector3 + // important: we know that the lower axis, cpi = 0, is closest here + dist := ColSphereCapsule(0, 10, &gdA, &gdB, &ptA, &ptB, &norm) + margin := float32(0.01) + + var ctA, ctB, offA, offB math32.Vector3 + var distActual, offMagA, offMagB float32 + actual := ContactPoints(dist, margin, &gdA, &gdB, ptA, ptB, norm, &ctA, &ctB, &offA, &offB, &distActual, &offMagA, &offMagB) + _ = actual + + // fmt.Println(dist, distActual, tc.dist, actual, norm, ptA, ptB) + // if actual { + // fmt.Println(ptA, ptB, ctA, ctB, offA, offB, offMagA, offMagB) + // } + + assert.InDelta(t, tc.dist, distActual, tol) + assert.InDelta(t, 1.0, norm.Length(), tol) + assert.Equal(t, distActual < margin, actual) + + if !actual { + continue + } + // cpA := ctA.Add(offA) + // cpB := ctB.Add(offB) + // fmt.Println(cpA, cpB, tc.dist, actual) + } +} From 7f8de3c897b4ed84d08bc80751615fd9ae8ff139 Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Mon, 5 Jan 2026 05:21:34 -0800 Subject: [PATCH 83/97] physics: docs updates --- docs/content/physics.md | 27 +++++++++++++++---------- physics/enumgen.go | 2 +- physics/examples/collision/collision.go | 2 ++ physics/examples/pendula/pendula.go | 2 +- physics/phyxyz/skin.go | 3 +-- physics/shapecollide.go | 4 ++++ physics/shapecollide.goal | 4 ++++ physics/shapes.go | 3 ++- 8 files changed, 31 insertions(+), 16 deletions(-) diff --git a/docs/content/physics.md b/docs/content/physics.md index b4e6a333..c469abc3 100644 --- a/docs/content/physics.md +++ b/docs/content/physics.md @@ -31,6 +31,7 @@ ed.SetUserParams(¶ms) ed.SetConfigFunc(func() { ml := ed.Model + ml.GPU = false // small models run faster on CPU sc := ed.Scene hsz := math32.Vec3(0.05, .2, 0.05) mass := float32(0.1) @@ -58,7 +59,9 @@ ed.SetConfigFunc(func() { }) ``` -The [[doc:physics/phyxyz/Editor]] widget provides the [[doc:physics/Model]] and [[doc:physics/phyxyz/Scene]] elements, and the `ConfigFunc` function that configures the physics elements. Stepping through these elements in order: +The [[doc:physics/phyxyz/Editor]] widget provides the [[doc:physics/Model]] and [[doc:physics/phyxyz/Scene]] elements. Click the `Step` buttons to run the physics updates for the given number of steps. `Restart` puts the configuration back to the initial conditions, while `Rebuild` re-builds the model using any updated parameters (in this case, the `N pendula` -- try increasing the number of links!). + +The `ConfigFunc` function configures the physics elements. Stepping through these elements in order: ```go rleft := math32.NewQuatAxisAngle(math32.Vec3(0, 0, 1), -math32.Pi/2) @@ -257,6 +260,8 @@ There is also an `Object` index for each body, that is used for external manipul The elemental shapes are a `Plane`, `Sphere`, `Capsule`, `Cylinder`, `Cone`, and `Box`: [[doc:physics.Shapes]]. The `Size` property on bodies is always the _half_ size, such as the radius or the half-height of a cylinder or capsule. This is used in `newton-physics` and makes more sense for center-based computations: physics operates on the center-of-mass of a body. Consistent with the overall coordinate system, the `Cylinder` and `Capsule` are oriented with `Y` as the height dimension, which is unfortunately inconsistent with the Z=up convention in `newton-physics`. +The `Capsule` half-size (Y axis) specifies the _entire_ half-size of the capsule, _including_ the end cap radius (X axis value). This allows one to use the same size specification for `Box`, `Capsule`, and `Cylinder` and have it make sense in terms of total vertical size, and it also allows using this Y half-size directly as an offset in joints, to make bodies connect at the top or bottom of the capsule. The Y half-size is constrained to be >= 1.01 * X half-size, where the 1.01 multiplier ensures that there is at least a very thin amount of cylindrical height, which is necessary for the collision computations. This height specification differs from `newton-physics`, where the half-height only specifies the height of the cylindrical portion of the capsule. + ### Multi-shape bodies The newton-physics framework, and MuJoCo upon which it is based, support multiple shapes per body, which can then be integrated to produce an aggregate inertia. This adds an additional level of complexity and management overhead, which we are currently avoiding in favor of putting the shapes directly on the body, so each body has 1 and only 1 shape. This simplifies collision considerably as well. It would not be difficult to add a shape layer at some point in the future. The same goes for Mesh, SDF, and HeightField types. @@ -279,27 +284,27 @@ The supported [[doc:physics.JointTypes]] include the following (DoF = degrees-of * `D6` is a generic 6-DoF joint that can be configured with up to 3 linear DoF and 3 angular DoF. +* `PlaneXZ` is a configuration of `D6` for navigation within the X-Z horizontal plane, with two linear DoF for motion in X and Z, and one angular DoF for rotation along the Y (vertical) axis. + Use `NewJoint*` with _dynamic_ body indexes to create joints (e.g., `NewJointPrismatic` etc). Each joint can be positioned with a relative offset and orientation relative to the _parent_ and _child_ elements. The parent index can be set to -1 to anchor a child body in an arbitrary and fixed position within the overall world. ## Phyxyz viewer -Typically, bodies are created using the enhanced functions in the [[doc:physics/phyxyz]] package, which provides a [[doc:physics/phyxyz.View]] wrapper for physics bodies. This wrapper has a default `Color` setting to provide simple color coding of bodies, and supports `NewView` and `InitView` functions that allow arbitrary visualization dynamics to be associated with each body (textures, meshes, dynamic updating etc). +Typically, bodies are created using the enhanced functions in the [[doc:physics/phyxyz]] package, which provides a [[doc:physics/phyxyz.Skin]] wrapper for physics bodies. This wrapper has a default `Color` setting to provide simple color coding of bodies, and supports `NewSkin` and `InitSkin` functions that allow arbitrary visualization dynamics to be associated with each body (textures, meshes, dynamic updating etc). -## Parallel worlds +## Builder -TODO: switch over to builder here. +The [[doc:physics/builder]] package provides a hierarchically-structured tree for constructing models, where you construct the specification of everything in terms of `World`, `Object`, and `Body` elements, and then call the `Build` function to actually generate the flat, GPU-compatible representation used directly in the `physics` engine. The overall API is similar, so it is easy to switch to using builder instead of directly constructing in `physics` or `phyxyz` (builder also supports making phyxyz Skins). -The compute efficiency of the GPU goes up with the more elements that are processed in parallel, amortizing the memory transfer overhead and leveraging the parallel cores. Furthermore, in AI applications for example, models can be trained in parallel on different instances of the same environment, with each instance having its own random initial starting point and trajectory over time. All of these instances can be simulated in one `physics.Model` by using the `World` index on the bodies, with the shared static environment living in World -1, and the elements of each instance (e.g., a simulated robot) living in its own separate world. +A major advantage of using `builder` is to take advantage of the `ReplicateWorld` method, which makes it easy to configure parallel worlds, as described next. It also provides convenient functions for incrementally adjusting positions relative to existing values, and manipulating the position of an entire `Object` as a collection of interconnected bodies (see [[doc:physics/builder.Object]] methods such as `Move` and `RotateAround`. -The `NewBody` and `NewDynamic` methods automatically use the `Model.CurrentWorld` index by default, or you can directly use `SetBodyWorld` to assign a specific world index. - -The `ReplicateWorld` method creates N replicas of an existing world, including all associated joints. This can only be called once, as it records the start and N-per-world of each such replicated world, which allows the `phyxyz` viewer to efficiently view a specific world. Thus, under this scenario, you create world 0 and then replicate it, then modify the initial positions and orientations accordingly, using `PositionObject`, as described next. The object numbers are also replicated so uniquely indexing a specific object instance requires specifying the world and object indexes. +## Parallel worlds -The phyxyz viewer can display a specific world, or all worlds. +The compute efficiency of the GPU goes up with the more elements that are processed in parallel, amortizing the memory transfer overhead and leveraging the parallel cores. Furthermore, in AI applications for example, models can be trained in parallel on different instances of the same environment, with each instance having its own random initial starting point and trajectory over time. All of these instances can be simulated in one `physics.Model` by using the `World` index on the bodies, with the shared static environment living in World -1, and the elements of each instance (e.g., a simulated robot) living in its own separate world. -## Manipulating objects +The `NewBody` and `NewDynamic` methods automatically use the `Model.CurrentWorld` index by default, or you can directly use `SetBodyWorld` to assign a specific world index. -TODO. +The [[doc:physics/builder.Builder.ReplicateWorld]] method creates N replicas of an existing world, including all associated joints. This can only be called once, as it records the start and N-per-world of each such replicated world, which allows the `phyxyz` `Editor` to efficiently view a specific world. Thus, under this scenario, you create world 0 and then replicate it, then modify the initial positions and orientations accordingly, using the Object-based methods. ## Sensors diff --git a/physics/enumgen.go b/physics/enumgen.go index 7e297fdf..18b78860 100644 --- a/physics/enumgen.go +++ b/physics/enumgen.go @@ -389,7 +389,7 @@ const ShapesN Shapes = 6 var _ShapesValueMap = map[string]Shapes{`Plane`: 0, `Sphere`: 1, `Capsule`: 2, `Cylinder`: 3, `Box`: 4, `Cone`: 5} -var _ShapesDescMap = map[Shapes]string{0: `Plane cannot be a dynamic shape, but is most efficient for collision computations. Use size = 0 for an infinite plane. Natively extends in the X-Z plane: SizeX x SizeZ.`, 1: `Sphere. SizeX is the radius.`, 2: `Capsule is a cylinder with half-spheres on the ends. Natively oriented vertically along the Y axis. SizeX = radius of end caps, SizeY = _total_ half-height (i.e., SizeX + half-height of cylindrical portion, must be >= SizeX). This parameterization allows joint offsets to be SizeY, and direct swapping of shape across Box and Cylinder with same total extent.`, 3: `Cylinder, natively oriented vertically along the Y axis. SizeX = radius, SizeY = half-height of Y axis Cylinder can not collide with a Box.`, 4: `Box is a 3D rectalinear shape. The sizes are _half_ sizes along each dimension, relative to the center.`, 5: `Cone is like a cylinder with the top radius = 0, oriented up. SizeX = bottom radius, SizeY = half-height in Y. Cone does not support any collisions and is not recommended for interacting bodies.`} +var _ShapesDescMap = map[Shapes]string{0: `Plane cannot be a dynamic shape, but is most efficient for collision computations. Use size = 0 for an infinite plane. Natively extends in the X-Z plane: SizeX x SizeZ.`, 1: `Sphere. SizeX is the radius.`, 2: `Capsule is a cylinder with half-spheres on the ends. Natively oriented vertically along the Y axis. SizeX = radius of end caps, SizeY = _total_ half-height (i.e., SizeX + half-height of cylindrical portion, must be >= SizeX). This parameterization allows joint offsets to be SizeY, and direct swapping of shape across Box and Cylinder with same total extent.`, 3: `Cylinder, natively oriented vertically along the Y axis. SizeX = radius, SizeY = half-height of Y axis Cylinder does not support most collisions and is thus not recommended where collision data is needed.`, 4: `Box is a 3D rectalinear shape. The sizes are _half_ sizes along each dimension, relative to the center.`, 5: `Cone is like a cylinder with the top radius = 0, oriented up. SizeX = bottom radius, SizeY = half-height in Y. Cone does not support any collisions and is not recommended for interacting bodies.`} var _ShapesMap = map[Shapes]string{0: `Plane`, 1: `Sphere`, 2: `Capsule`, 3: `Cylinder`, 4: `Box`, 5: `Cone`} diff --git a/physics/examples/collision/collision.go b/physics/examples/collision/collision.go index 20adfd31..02c48a7b 100644 --- a/physics/examples/collision/collision.go +++ b/physics/examples/collision/collision.go @@ -46,6 +46,7 @@ type Collide struct { // Friction is for sliding: around 0.01 seems pretty realistic Friction float32 + // FrictionTortion is for rotating. Not generally relevant here. FrictionTortion float32 // FrictionRolling is for rolling: around 0.01 seems pretty realistic @@ -61,6 +62,7 @@ func (cl *Collide) Defaults() { cl.MassB = 1 cl.PushMass = 1 cl.Friction = 0.01 + cl.FrictionTortion = 0.01 cl.FrictionRolling = 0.01 } diff --git a/physics/examples/pendula/pendula.go b/physics/examples/pendula/pendula.go index c55d7dfe..8c8a40ba 100644 --- a/physics/examples/pendula/pendula.go +++ b/physics/examples/pendula/pendula.go @@ -92,7 +92,7 @@ func main() { obj := wld.NewObject() ml := ed.Model - // ml.GPU = false + ml.GPU = false sc := ed.Scene rot := math32.NewQuatIdentity() rleft := math32.NewQuatAxisAngle(math32.Vec3(0, 0, 1), -math32.Pi/2) diff --git a/physics/phyxyz/skin.go b/physics/phyxyz/skin.go index 3237c886..28923ab0 100644 --- a/physics/phyxyz/skin.go +++ b/physics/phyxyz/skin.go @@ -198,9 +198,8 @@ func (sk *Skin) CylinderInit(sld *xyz.Solid) { // add updaters for that. func (sk *Skin) CapsuleInit(sld *xyz.Solid) { rat := sk.HSize.Y / sk.HSize.X - mnm := fmt.Sprintf("physics.Capsule_%3g", rat) + mnm := fmt.Sprintf("physics.Capsule_%g", math32.Truncate(rat, 3)) if ms, _ := sld.Scene.MeshByName(mnm); ms == nil { - fmt.Println(rat, mnm, 2*(sk.HSize.Y-sk.HSize.X)/sk.HSize.X) ms = xyz.NewCapsule(sld.Scene, mnm, 2*(sk.HSize.Y-sk.HSize.X)/sk.HSize.X, 1, 32, 1) } sld.SetMeshName(mnm) diff --git a/physics/shapecollide.go b/physics/shapecollide.go index 73f41c42..1bc61174 100644 --- a/physics/shapecollide.go +++ b/physics/shapecollide.go @@ -409,4 +409,8 @@ func ColCylinderPlane(cpi, maxIter int32, gdA *GeomData, gdB *GeomData, pA, pB, return dist } +// todo: newton geometry/collision_primitive.py supports collide_sphere_cylinder +// could adapt that. But there is no Box-Cylinder collision, nor anything with Capsule. +// so in general it is not super urgent. + //gosl:end diff --git a/physics/shapecollide.goal b/physics/shapecollide.goal index 4317432b..7e8c08b5 100644 --- a/physics/shapecollide.goal +++ b/physics/shapecollide.goal @@ -406,4 +406,8 @@ func ColCylinderPlane(cpi, maxIter int32, gdA *GeomData, gdB *GeomData, pA, pB, return dist } +// todo: newton geometry/collision_primitive.py supports collide_sphere_cylinder +// could adapt that. But there is no Box-Cylinder collision, nor anything with Capsule. +// so in general it is not super urgent. + //gosl:end diff --git a/physics/shapes.go b/physics/shapes.go index d143a215..e089ae8e 100644 --- a/physics/shapes.go +++ b/physics/shapes.go @@ -44,7 +44,8 @@ const ( // Cylinder, natively oriented vertically along the Y axis. // SizeX = radius, SizeY = half-height of Y axis - // Cylinder can not collide with a Box. + // Cylinder does not support most collisions and is thus not recommended + // where collision data is needed. Cylinder // Box is a 3D rectalinear shape. From f00aa6bd79e7c792d5ce0ea4f4750d598d9bc273 Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Mon, 5 Jan 2026 09:56:58 -0800 Subject: [PATCH 84/97] physics: update kinetics from delta changes: looking very stable in pendula, collision for total KE. balls seems to go up so there is more work to be done there.. --- physics/dynamics.go | 24 ++++++++++++ physics/dynamics.goal | 25 ++++++++++++ physics/examples/balls/balls.go | 1 + physics/examples/collision/collision.go | 1 + physics/examples/pendula/pendula.go | 1 + physics/model.go | 4 ++ physics/model.goal | 4 ++ physics/shaders/StepBodyContactDeltas.wgsl | 45 ++++++++++++++++++++++ physics/shaders/StepSolveJoints.wgsl | 35 +++++++++++++++++ physics/step.go | 16 +++++--- physics/step.goal | 16 +++++--- physics/step_body.go | 43 +++++++++++++++++++-- physics/step_body.goal | 45 ++++++++++++++++++++-- physics/typegen.go | 2 +- 14 files changed, 243 insertions(+), 19 deletions(-) diff --git a/physics/dynamics.go b/physics/dynamics.go index 09447d45..a0c2d504 100644 --- a/physics/dynamics.go +++ b/physics/dynamics.go @@ -7,9 +7,11 @@ package physics import ( + // "fmt" "math" "cogentcore.org/core/math32" + "cogentcore.org/lab/gosl/slmath" ) //gosl:start @@ -207,3 +209,25 @@ func (ml *Model) SetMass(idx int32, shape Shapes, size math32.Vector3, mass floa SetBodyInertia(idx, inertia) SetBodyInvInertia(idx, inertia.Inverse()) } + +// TotalKineticEnergy returns the total kinetic energy of the dynamic bodies, +// as a function of the velocities. +func (ml *Model) TotalKineticEnergy() float32 { + params := GetParams(0) + ke := float32(0) + n := int32(Dynamics.DimSize(0)) + for di := range n { + bi := DynamicBody(di) + mass := Bodies.Value(int(bi), int(BodyMass)) + inertia := BodyInertia(bi) + + v := DynamicVel(di, params.Next) + mv := 0.5 * mass * slmath.LengthSquared3(v) + + w := DynamicAngVel(di, params.Next) + iw := 0.5 * slmath.Dot3(w, inertia.MulVector3(w)) + + ke += mv + iw + } + return ke +} diff --git a/physics/dynamics.goal b/physics/dynamics.goal index 313bc999..77391cad 100644 --- a/physics/dynamics.goal +++ b/physics/dynamics.goal @@ -5,9 +5,11 @@ package physics import ( + // "fmt" "math" "cogentcore.org/core/math32" + "cogentcore.org/lab/gosl/slmath" ) //gosl:start @@ -205,3 +207,26 @@ func (ml *Model) SetMass(idx int32, shape Shapes, size math32.Vector3, mass floa SetBodyInertia(idx, inertia) SetBodyInvInertia(idx, inertia.Inverse()) } + +// TotalKineticEnergy returns the total kinetic energy of the dynamic bodies, +// as a function of the velocities. +func (ml *Model) TotalKineticEnergy() float32 { + params := GetParams(0) + ke := float32(0) + n := int32(Dynamics.DimSize(0)) + for di := range n { + bi := DynamicBody(di) + mass := Bodies[bi, BodyMass] + inertia := BodyInertia(bi) + + v := DynamicVel(di, params.Next) + mv := 0.5 * mass * slmath.LengthSquared3(v) + + w := DynamicAngVel(di, params.Next) + iw := 0.5 * slmath.Dot3(w, inertia.MulVector3(w)) + + ke += mv + iw + } + return ke +} + diff --git a/physics/examples/balls/balls.go b/physics/examples/balls/balls.go index 681f2c28..299efa60 100644 --- a/physics/examples/balls/balls.go +++ b/physics/examples/balls/balls.go @@ -76,6 +76,7 @@ func main() { ml.Params[0].SubSteps = 100 ml.Params[0].Dt = 0.001 // ml.GPU = false + // ml.ReportTotalKE = true sc := ed.Scene rot := math32.NewQuatIdentity() sc.NewBody(ml, "floor", physics.Plane, "#D0D0D080", math32.Vec3(0, 0, 0), diff --git a/physics/examples/collision/collision.go b/physics/examples/collision/collision.go index 02c48a7b..d88c5327 100644 --- a/physics/examples/collision/collision.go +++ b/physics/examples/collision/collision.go @@ -84,6 +84,7 @@ func main() { ed.SetConfigFunc(func() { ml := ed.Model ml.GPU = false + // ml.ReportTotalKE = true sc := ed.Scene rot := math32.NewQuatIdentity() fl := sc.NewBody(ml, "floor", physics.Plane, "#D0D0D080", math32.Vec3(0, 0, 0), math32.Vec3(0, 0, 0), rot) diff --git a/physics/examples/pendula/pendula.go b/physics/examples/pendula/pendula.go index 8c8a40ba..8367c875 100644 --- a/physics/examples/pendula/pendula.go +++ b/physics/examples/pendula/pendula.go @@ -93,6 +93,7 @@ func main() { ml := ed.Model ml.GPU = false + // ml.ReportTotalKE = true sc := ed.Scene rot := math32.NewQuatIdentity() rleft := math32.NewQuatAxisAngle(math32.Vec3(0, 0, 1), -math32.Pi/2) diff --git a/physics/model.go b/physics/model.go index fbc75256..5ec4d6e2 100644 --- a/physics/model.go +++ b/physics/model.go @@ -24,6 +24,10 @@ type Model struct { // GetContacts will download Contacts from the GPU, if processing them on the CPU. GetContacts bool + // ReportTotalKE prints out the total computed kinetic energy in the system after + // every step. + ReportTotalKE bool + // CurrentWorld is the [BodyWorld] value to use when creating new bodies. // Set to -1 to create global elements that interact with everything, // while 0 and positive numbers only interact amongst themselves. diff --git a/physics/model.goal b/physics/model.goal index d31c6be6..d9ee575a 100644 --- a/physics/model.goal +++ b/physics/model.goal @@ -22,6 +22,10 @@ type Model struct { // GetContacts will download Contacts from the GPU, if processing them on the CPU. GetContacts bool + // ReportTotalKE prints out the total computed kinetic energy in the system after + // every step. + ReportTotalKE bool + // CurrentWorld is the [BodyWorld] value to use when creating new bodies. // Set to -1 to create global elements that interact with everything, // while 0 and positive numbers only interact amongst themselves. diff --git a/physics/shaders/StepBodyContactDeltas.wgsl b/physics/shaders/StepBodyContactDeltas.wgsl index 306e2790..f4084ef9 100644 --- a/physics/shaders/StepBodyContactDeltas.wgsl +++ b/physics/shaders/StepBodyContactDeltas.wgsl @@ -241,6 +241,13 @@ fn DynamicPos(idx: i32,cni: i32) -> vec3 { return vec3(Dynamics[Index3 fn SetDynamicPos(idx: i32,cni: i32, pos: vec3) { Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynPosX))] = pos.x;; Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynPosY))] = pos.y;; Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynPosZ))] = pos.z; } fn DynamicQuat(idx: i32,cni: i32) -> vec4 { return vec4(Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynQuatX))], Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynQuatY))], Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynQuatZ))], Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynQuatW))]); } fn SetDynamicQuat(idx: i32,cni: i32, rot: vec4) { Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynQuatX))] = rot.x;; Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynQuatY))] = rot.y;; Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynQuatZ))] = rot.z;; Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynQuatW))] = rot.w; } +fn DynamicVel(idx: i32,cni: i32) -> vec3 { return vec3(Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynVelX))], Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynVelY))], Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynVelZ))]); } +fn SetDynamicVel(idx: i32,cni: i32, vel: vec3) { Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynVelX))] = vel.x;; Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynVelY))] = vel.y;; Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynVelZ))] = vel.z; } +fn SetDynamicAcc(idx: i32,cni: i32, acc: vec3) { Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynAccX))] = acc.x;; Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynAccY))] = acc.y;; Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynAccZ))] = acc.z; } +fn DynamicAngVel(idx: i32,cni: i32) -> vec3 { return vec3(Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynAngVelX))], Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynAngVelY))], Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynAngVelZ))]); } +fn SetDynamicAngVel(idx: i32,cni: i32, angVel: vec3) { Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynAngVelX))] = angVel.x;; Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynAngVelY))] = angVel.y;; Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynAngVelZ))] = angVel.z; } +fn SetDynamicAngAcc(idx: i32,cni: i32, angAcc: vec3) { Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynAngAccX))] = angAcc.x;; Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynAngAccY))] = angAcc.y;; Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], +u32(idx), u32(cni), u32(DynAngAccZ))] = angAcc.z; } fn DynamicDelta(idx: i32,cni: i32) -> vec3 { return vec3(Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynDeltaX))], Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynDeltaY))], Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynDeltaZ))]); } @@ -439,10 +446,20 @@ fn MulQuats(a: vec4,b: vec4) -> vec4 { q.w = a.w*b.w - a.x*b.x - a.y*b.y - a.z*b.z; return q; } +fn QuatInverse(q: vec4) -> vec4 { + var nq = q; + nq.x *= f32(-1); + nq.y *= f32(-1); + nq.z *= f32(-1); +return QuatNormalize(nq); +} //////// import: "slmath-vector2.go" //////// import: "slmath-vector3.go" +fn Negate3(v: vec3) -> vec3 { + return vec3(-v.x, -v.y, -v.z); +} fn Length3(v: vec3) -> f32 { return sqrt(v.x*v.x + v.y*v.y + v.z*v.z); } @@ -494,6 +511,34 @@ fn StepBodyDeltas(di: i32,bi: i32, contacts: bool, cWt: f32, linDel: vec3,a SetDynamicQuat(di, params.Next, q1); SetDynamicDelta(di, params.Next, v1); SetDynamicAngDelta(di, params.Next, w1); + if (contacts) { + StepBodyKinetics(di, bi); + } + Params[0] = params; +} +fn StepBodyKinetics(di: i32,bi: i32) { + var params = Params[0]; + var r0 = DynamicPos(di, params.Cur); + var q0 = DynamicQuat(di, params.Cur); + var v0 = DynamicVel(di, params.Cur); + var w0 = DynamicAngVel(di, params.Cur); + var r1 = DynamicPos(di, params.Next); + var q1 = DynamicQuat(di, params.Next); + var com = BodyCom(bi); + var com0 = MulQuatVector(q0, com)+(r0); + var com1 = MulQuatVector(q1, com)+(r1); + var v1 = com1-(com0)/(params.Dt); + var dq = MulQuats(q1, QuatInverse(q0)); + var w1 = vec3(dq.x, dq.y, dq.z)*(2 / params.Dt); + if (dq.w < 0) { + w1 = Negate3(w1); + } + SetDynamicVel(di, params.Next, v1); + SetDynamicAngVel(di, params.Next, w1); + var a1 = v1-(v0)/(params.Dt); + var wa1 = w1-(w0)/(params.Dt); + SetDynamicAcc(di, params.Next, a1); + SetDynamicAngAcc(di, params.Next, wa1); Params[0] = params; } fn LimitDelta(v: vec3, lim: f32) -> vec3 { diff --git a/physics/shaders/StepSolveJoints.wgsl b/physics/shaders/StepSolveJoints.wgsl index 097f5c46..6c9259a4 100644 --- a/physics/shaders/StepSolveJoints.wgsl +++ b/physics/shaders/StepSolveJoints.wgsl @@ -189,6 +189,13 @@ fn DynamicPos(idx: i32,cni: i32) -> vec3 { return vec3(Dynamics[Index3 fn SetDynamicPos(idx: i32,cni: i32, pos: vec3) { Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynPosX))] = pos.x;; Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynPosY))] = pos.y;; Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynPosZ))] = pos.z; } fn DynamicQuat(idx: i32,cni: i32) -> vec4 { return vec4(Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynQuatX))], Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynQuatY))], Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynQuatZ))], Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynQuatW))]); } fn SetDynamicQuat(idx: i32,cni: i32, rot: vec4) { Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynQuatX))] = rot.x;; Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynQuatY))] = rot.y;; Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynQuatZ))] = rot.z;; Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynQuatW))] = rot.w; } +fn DynamicVel(idx: i32,cni: i32) -> vec3 { return vec3(Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynVelX))], Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynVelY))], Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynVelZ))]); } +fn SetDynamicVel(idx: i32,cni: i32, vel: vec3) { Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynVelX))] = vel.x;; Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynVelY))] = vel.y;; Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynVelZ))] = vel.z; } +fn SetDynamicAcc(idx: i32,cni: i32, acc: vec3) { Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynAccX))] = acc.x;; Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynAccY))] = acc.y;; Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynAccZ))] = acc.z; } +fn DynamicAngVel(idx: i32,cni: i32) -> vec3 { return vec3(Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynAngVelX))], Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynAngVelY))], Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynAngVelZ))]); } +fn SetDynamicAngVel(idx: i32,cni: i32, angVel: vec3) { Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynAngVelX))] = angVel.x;; Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynAngVelY))] = angVel.y;; Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynAngVelZ))] = angVel.z; } +fn SetDynamicAngAcc(idx: i32,cni: i32, angAcc: vec3) { Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynAngAccX))] = angAcc.x;; Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynAngAccY))] = angAcc.y;; Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], +u32(idx), u32(cni), u32(DynAngAccZ))] = angAcc.z; } fn DynamicDelta(idx: i32,cni: i32) -> vec3 { return vec3(Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynDeltaX))], Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynDeltaY))], Dynamics[Index3D(TensorStrides[60], TensorStrides[61], TensorStrides[62], u32(idx), u32(cni), u32(DynDeltaZ))]); } @@ -602,6 +609,34 @@ fn StepBodyDeltas(di: i32,bi: i32, contacts: bool, cWt: f32, linDel: vec3,a SetDynamicQuat(di, params.Next, q1); SetDynamicDelta(di, params.Next, v1); SetDynamicAngDelta(di, params.Next, w1); + if (contacts) { + StepBodyKinetics(di, bi); + } + Params[0] = params; +} +fn StepBodyKinetics(di: i32,bi: i32) { + var params = Params[0]; + var r0 = DynamicPos(di, params.Cur); + var q0 = DynamicQuat(di, params.Cur); + var v0 = DynamicVel(di, params.Cur); + var w0 = DynamicAngVel(di, params.Cur); + var r1 = DynamicPos(di, params.Next); + var q1 = DynamicQuat(di, params.Next); + var com = BodyCom(bi); + var com0 = MulQuatVector(q0, com)+(r0); + var com1 = MulQuatVector(q1, com)+(r1); + var v1 = com1-(com0)/(params.Dt); + var dq = MulQuats(q1, QuatInverse(q0)); + var w1 = vec3(dq.x, dq.y, dq.z)*(2 / params.Dt); + if (dq.w < 0) { + w1 = Negate3(w1); + } + SetDynamicVel(di, params.Next, v1); + SetDynamicAngVel(di, params.Next, w1); + var a1 = v1-(v0)/(params.Dt); + var wa1 = w1-(w0)/(params.Dt); + SetDynamicAcc(di, params.Next, a1); + SetDynamicAngAcc(di, params.Next, wa1); Params[0] = params; } fn LimitDelta(v: vec3, lim: f32) -> vec3 { diff --git a/physics/step.go b/physics/step.go index fb3a3705..5e7c525d 100644 --- a/physics/step.go +++ b/physics/step.go @@ -52,7 +52,7 @@ func StepInit(i uint32) { //gosl:kernel read-write:Params } } -// step does the following: +// newton step does the following: // if self.compute_body_velocity_from_position_delta or self.enable_restitution: // // save initial state: // body_q_init = wp.clone(state_in.body_q) @@ -74,10 +74,10 @@ func StepInit(i uint32) { //gosl:kernel read-write:Params // model, state_in, state_out, body_deltas, dt, rigid_contact_inv_weight // ) // # update body velocities from position changes -// if self.compute_body_velocity_from_position_delta and model.body_count and not requires_grad: -// kernel=update_body_velocities, -// kernel=apply_rigid_restitution, -// kernel=apply_body_delta_velocities, +// if self.compute_body_velocity_from_position_delta and model.body_count and not requires_grad: +// kernel=update_body_velocities, +// kernel=apply_rigid_restitution, +// kernel=apply_body_delta_velocities, //gosl:end @@ -100,6 +100,10 @@ func (ml *Model) Step() { if ContactsN.Value(0) >= params.ContactsMax { fmt.Println("Warning: over ContactsMax:", ContactsN.Value(0), "Max:", params.ContactsMax) } + if ml.ReportTotalKE { + ke := ml.TotalKineticEnergy() + fmt.Println("Total KE:", ke) + } } // StepGet runs one physics step and gets the given vars back @@ -150,8 +154,8 @@ func (ml *Model) StepBodyContacts() { cmax := int(ContactsN.Values[0]) if cmax > 0 { RunStepBodyContacts(cmax) - RunStepBodyContactDeltas(int(params.DynamicsN)) } + RunStepBodyContactDeltas(int(params.DynamicsN)) } else { RunStepBodyContacts(int(params.ContactsMax)) // just do max and let the routines bail RunStepBodyContactDeltas(int(params.DynamicsN)) diff --git a/physics/step.goal b/physics/step.goal index c537213f..fa8e631e 100644 --- a/physics/step.goal +++ b/physics/step.goal @@ -50,7 +50,7 @@ func StepInit(i uint32) { //gosl:kernel read-write:Params } } -// step does the following: +// newton step does the following: // if self.compute_body_velocity_from_position_delta or self.enable_restitution: // // save initial state: // body_q_init = wp.clone(state_in.body_q) @@ -72,10 +72,10 @@ func StepInit(i uint32) { //gosl:kernel read-write:Params // model, state_in, state_out, body_deltas, dt, rigid_contact_inv_weight // ) // # update body velocities from position changes -// if self.compute_body_velocity_from_position_delta and model.body_count and not requires_grad: -// kernel=update_body_velocities, -// kernel=apply_rigid_restitution, -// kernel=apply_body_delta_velocities, +// if self.compute_body_velocity_from_position_delta and model.body_count and not requires_grad: +// kernel=update_body_velocities, +// kernel=apply_rigid_restitution, +// kernel=apply_body_delta_velocities, //gosl:end @@ -98,6 +98,10 @@ func (ml *Model) Step() { if ContactsN.Value(0) >= params.ContactsMax { fmt.Println("Warning: over ContactsMax:", ContactsN.Value(0), "Max:", params.ContactsMax) } + if ml.ReportTotalKE { + ke := ml.TotalKineticEnergy() + fmt.Println("Total KE:", ke) + } } // StepGet runs one physics step and gets the given vars back @@ -148,8 +152,8 @@ func (ml *Model) StepBodyContacts() { cmax := int(ContactsN.Values[0]) if cmax > 0 { RunStepBodyContacts(cmax) - RunStepBodyContactDeltas(int(params.DynamicsN)) } + RunStepBodyContactDeltas(int(params.DynamicsN)) } else { RunStepBodyContacts(int(params.ContactsMax)) // just do max and let the routines bail RunStepBodyContactDeltas(int(params.DynamicsN)) diff --git a/physics/step_body.go b/physics/step_body.go index 252c4f49..5877fbab 100644 --- a/physics/step_body.go +++ b/physics/step_body.go @@ -152,7 +152,8 @@ func StepIntegrateBodies(i uint32) { //gosl:kernel // newton: solvers/xpbd/kernels.py: apply_body_deltas // StepBodyDeltas updates Next position with deltas from joints -// or contacts (if contacts true). +// or contacts (if contacts true). Also updates kinetics (velocity and acceleration) +// based on position & orientation changes if contacts=true (usually just 1 iteration). func StepBodyDeltas(di, bi int32, contacts bool, cWt float32, linDel, angDel math32.Vector3) { params := GetParams(0) @@ -213,12 +214,48 @@ func StepBodyDeltas(di, bi int32, contacts bool, cWt float32, linDel, angDel mat w1 = math32.Vec3(0, 0, 0) } - // fmt.Println(params.Next, "delta:", v0, v1) - SetDynamicPos(di, params.Next, p1) SetDynamicQuat(di, params.Next, q1) SetDynamicDelta(di, params.Next, v1) SetDynamicAngDelta(di, params.Next, w1) + + if contacts { + StepBodyKinetics(di, bi) + } +} + +// StepBodyKinetics computes the empirical velocities and +// accelerations from the final changes in position and orientation +// from Cur to Next. +func StepBodyKinetics(di, bi int32) { + params := GetParams(0) + r0 := DynamicPos(di, params.Cur) + q0 := DynamicQuat(di, params.Cur) + v0 := DynamicVel(di, params.Cur) + w0 := DynamicAngVel(di, params.Cur) + + r1 := DynamicPos(di, params.Next) + q1 := DynamicQuat(di, params.Next) + + com := BodyCom(bi) + com0 := slmath.MulQuatVector(q0, com).Add(r0) + com1 := slmath.MulQuatVector(q1, com).Add(r1) + + v1 := com1.Sub(com0).DivScalar(params.Dt) + dq := slmath.MulQuats(q1, slmath.QuatInverse(q0)) + + w1 := math32.Vec3(dq.X, dq.Y, dq.Z).MulScalar(2 / params.Dt) + if dq.W < 0 { + w1 = slmath.Negate3(w1) + } + + SetDynamicVel(di, params.Next, v1) + SetDynamicAngVel(di, params.Next, w1) + + a1 := v1.Sub(v0).DivScalar(params.Dt) + wa1 := w1.Sub(w0).DivScalar(params.Dt) + SetDynamicAcc(di, params.Next, a1) + SetDynamicAngAcc(di, params.Next, wa1) } // LimitDelta limits the magnitude of a delta vector diff --git a/physics/step_body.goal b/physics/step_body.goal index fe1bb09f..33c7af37 100644 --- a/physics/step_body.goal +++ b/physics/step_body.goal @@ -150,7 +150,8 @@ func StepIntegrateBodies(i uint32) { //gosl:kernel // newton: solvers/xpbd/kernels.py: apply_body_deltas // StepBodyDeltas updates Next position with deltas from joints -// or contacts (if contacts true). +// or contacts (if contacts true). Also updates kinetics (velocity and acceleration) +// based on position & orientation changes if contacts=true (usually just 1 iteration). func StepBodyDeltas(di, bi int32, contacts bool, cWt float32, linDel, angDel math32.Vector3) { params := GetParams(0) @@ -211,12 +212,48 @@ func StepBodyDeltas(di, bi int32, contacts bool, cWt float32, linDel, angDel mat w1 = math32.Vec3(0, 0, 0) } - // fmt.Println(params.Next, "delta:", v0, v1) - SetDynamicPos(di, params.Next, p1) SetDynamicQuat(di, params.Next, q1) SetDynamicDelta(di, params.Next, v1) SetDynamicAngDelta(di, params.Next, w1) + + if contacts { + StepBodyKinetics(di, bi) + } +} + +// StepBodyKinetics computes the empirical velocities and +// accelerations from the final changes in position and orientation +// from Cur to Next. +func StepBodyKinetics(di, bi int32) { + params := GetParams(0) + r0 := DynamicPos(di, params.Cur) + q0 := DynamicQuat(di, params.Cur) + v0 := DynamicVel(di, params.Cur) + w0 := DynamicAngVel(di, params.Cur) + + r1 := DynamicPos(di, params.Next) + q1 := DynamicQuat(di, params.Next) + + com := BodyCom(bi) + com0 := slmath.MulQuatVector(q0, com).Add(r0) + com1 := slmath.MulQuatVector(q1, com).Add(r1) + + v1 := com1.Sub(com0).DivScalar(params.Dt) + dq := slmath.MulQuats(q1, slmath.QuatInverse(q0)) + + w1 := math32.Vec3(dq.X, dq.Y, dq.Z).MulScalar(2/params.Dt) + if dq.W < 0 { + w1 = slmath.Negate3(w1) + } + + SetDynamicVel(di, params.Next, v1) + SetDynamicAngVel(di, params.Next, w1) + + a1 := v1.Sub(v0).DivScalar(params.Dt) + wa1 := w1.Sub(w0).DivScalar(params.Dt) + SetDynamicAcc(di, params.Next, a1) + SetDynamicAngAcc(di, params.Next, wa1) } // LimitDelta limits the magnitude of a delta vector @@ -233,3 +270,5 @@ func VelocityAtPoint(lin, ang, r math32.Vector3) math32.Vector3 { } //gosl:end + + diff --git a/physics/typegen.go b/physics/typegen.go index 62be9042..6a00275e 100644 --- a/physics/typegen.go +++ b/physics/typegen.go @@ -22,7 +22,7 @@ var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.JointVars", var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.JointDoFVars", IDName: "joint-do-f-vars", Doc: "JointDoFVars are joint DoF state variables stored in tensor.Float32,\none for each DoF."}) -var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.Model", IDName: "model", Doc: "Model contains and manages all of the physics elements.", Directives: []types.Directive{{Tool: "go", Directive: "generate", Args: []string{"core", "generate", "-add-types", "-gosl"}}}, Fields: []types.Field{{Name: "GPU", Doc: "GPU determines whether to use GPU (else CPU)."}, {Name: "Params", Doc: "Params are global parameters."}, {Name: "GetContacts", Doc: "GetContacts will download Contacts from the GPU, if processing them on the CPU."}, {Name: "CurrentWorld", Doc: "CurrentWorld is the [BodyWorld] value to use when creating new bodies.\nSet to -1 to create global elements that interact with everything,\nwhile 0 and positive numbers only interact amongst themselves."}, {Name: "CurrentObject", Doc: "CurrentObject is the Object to use when creating new joints.\nCall NewObject to increment."}, {Name: "CurrentObjectJoint", Doc: "CurrentObjectJoint is the Joint index in CurrentObject\nto use when creating new joints."}, {Name: "ReplicasStart", Doc: "ReplicasStart is the starting body index for replicated world bodies,\nwhich is needed for viewers to efficiently select a specific world to view.\nThis is the start of the World=0 first instance."}, {Name: "ReplicasN", Doc: "ReplicasN is the number of body elements within each set of\nreplicated world bodies, which is needed for viewers to efficiently select\na specific world to view."}, {Name: "Bodies", Doc: "Bodies are the rigid body elements (dynamic and static),\nspecifying the constant, non-dynamic properties,\nwhich is initial state for dynamics.\n[body][BodyVarsN]"}, {Name: "Objects", Doc: "Objects is a list of joint indexes for each object, where each object\ncontains all the joints interconnecting an overlapping set of bodies.\nThis is known as an articulation in other physics software.\nJoints must be added in parent -> child order within objects, as joints\nare updated in sequential order within object.\n[object][MaxObjectJoints+1]"}, {Name: "BodyJoints", Doc: "BodyJoints is a list of joint indexes for each dynamic body, for aggregating.\n[dyn body][parent, child][Params.BodyJointsMax]"}, {Name: "Joints", Doc: "Joints is a list of permanent joints connecting bodies,\nwhich do not change (no dynamic variables).\n[joint][JointVarsN]"}, {Name: "JointDoFs", Doc: "JointDoFs is a list of joint DoF parameters, allocated per joint.\n[dof][JointDoFVars]"}, {Name: "BodyCollidePairs", Doc: "BodyCollidePairs are pairs of Body indexes that could potentially collide\nbased on precomputed collision logic, using World, Group, and Joint indexes.\n[BodyCollidePairsN][2]"}, {Name: "Dynamics", Doc: "Dynamics are the dynamic rigid body elements: these actually move.\nThe first set of variables are for initial values, and the second current.\n[body][cur/next][DynamicVarsN]"}, {Name: "BroadContactsN", Doc: "BroadContactsN has number of points of broad contact\nbetween bodies. [1]"}, {Name: "BroadContacts", Doc: "BroadContacts are the results of broad-phase contact processing,\nestablishing possible points of contact between bodies.\n[ContactsMax][BroadContactVarsN]"}, {Name: "ContactsN", Doc: "ContactsN has number of points of narrow (final) contact\nbetween bodies. [1]"}, {Name: "Contacts", Doc: "Contacts are the results of narrow-phase contact processing,\nwhere only actual contacts with fully-specified values are present.\n[ContactsMax][ContactVarsN]"}, {Name: "JointControls", Doc: "JointControls are dynamic joint control inputs, per joint DoF\n(in correspondence with [JointDoFs]). This can be uploaded to the\nGPU at every step.\n[dof][JointControlVarsN]"}}}) +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.Model", IDName: "model", Doc: "Model contains and manages all of the physics elements.", Directives: []types.Directive{{Tool: "go", Directive: "generate", Args: []string{"core", "generate", "-add-types", "-gosl"}}}, Fields: []types.Field{{Name: "GPU", Doc: "GPU determines whether to use GPU (else CPU)."}, {Name: "Params", Doc: "Params are global parameters."}, {Name: "GetContacts", Doc: "GetContacts will download Contacts from the GPU, if processing them on the CPU."}, {Name: "ReportTotalKE", Doc: "ReportTotalKE prints out the total computed kinetic energy in the system after\nevery step."}, {Name: "CurrentWorld", Doc: "CurrentWorld is the [BodyWorld] value to use when creating new bodies.\nSet to -1 to create global elements that interact with everything,\nwhile 0 and positive numbers only interact amongst themselves."}, {Name: "CurrentObject", Doc: "CurrentObject is the Object to use when creating new joints.\nCall NewObject to increment."}, {Name: "CurrentObjectJoint", Doc: "CurrentObjectJoint is the Joint index in CurrentObject\nto use when creating new joints."}, {Name: "ReplicasStart", Doc: "ReplicasStart is the starting body index for replicated world bodies,\nwhich is needed for viewers to efficiently select a specific world to view.\nThis is the start of the World=0 first instance."}, {Name: "ReplicasN", Doc: "ReplicasN is the number of body elements within each set of\nreplicated world bodies, which is needed for viewers to efficiently select\na specific world to view."}, {Name: "Bodies", Doc: "Bodies are the rigid body elements (dynamic and static),\nspecifying the constant, non-dynamic properties,\nwhich is initial state for dynamics.\n[body][BodyVarsN]"}, {Name: "Objects", Doc: "Objects is a list of joint indexes for each object, where each object\ncontains all the joints interconnecting an overlapping set of bodies.\nThis is known as an articulation in other physics software.\nJoints must be added in parent -> child order within objects, as joints\nare updated in sequential order within object.\n[object][MaxObjectJoints+1]"}, {Name: "BodyJoints", Doc: "BodyJoints is a list of joint indexes for each dynamic body, for aggregating.\n[dyn body][parent, child][Params.BodyJointsMax]"}, {Name: "Joints", Doc: "Joints is a list of permanent joints connecting bodies,\nwhich do not change (no dynamic variables).\n[joint][JointVarsN]"}, {Name: "JointDoFs", Doc: "JointDoFs is a list of joint DoF parameters, allocated per joint.\n[dof][JointDoFVars]"}, {Name: "BodyCollidePairs", Doc: "BodyCollidePairs are pairs of Body indexes that could potentially collide\nbased on precomputed collision logic, using World, Group, and Joint indexes.\n[BodyCollidePairsN][2]"}, {Name: "Dynamics", Doc: "Dynamics are the dynamic rigid body elements: these actually move.\nThe first set of variables are for initial values, and the second current.\n[body][cur/next][DynamicVarsN]"}, {Name: "BroadContactsN", Doc: "BroadContactsN has number of points of broad contact\nbetween bodies. [1]"}, {Name: "BroadContacts", Doc: "BroadContacts are the results of broad-phase contact processing,\nestablishing possible points of contact between bodies.\n[ContactsMax][BroadContactVarsN]"}, {Name: "ContactsN", Doc: "ContactsN has number of points of narrow (final) contact\nbetween bodies. [1]"}, {Name: "Contacts", Doc: "Contacts are the results of narrow-phase contact processing,\nwhere only actual contacts with fully-specified values are present.\n[ContactsMax][ContactVarsN]"}, {Name: "JointControls", Doc: "JointControls are dynamic joint control inputs, per joint DoF\n(in correspondence with [JointDoFs]). This can be uploaded to the\nGPU at every step.\n[dof][JointControlVarsN]"}}}) var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.PhysicsParams", IDName: "physics-params", Doc: "PhysicsParams are the physics parameters", Directives: []types.Directive{{Tool: "gosl", Directive: "start"}}, Fields: []types.Field{{Name: "Iterations", Doc: "Iterations is the number of integration iterations to perform\nwithin each solver step. Muller et al (2020) report that 1 is best."}, {Name: "Dt", Doc: "Dt is the integration stepsize.\nFor highly kinetic situations (e.g., rapidly moving bouncing balls)\n0.0001 is needed to ensure contact registration. Use SubSteps to\naccomplish a target effective read-out step size."}, {Name: "SubSteps", Doc: "SubSteps is the number of integration steps to take per Step()\nfunction call. These sub steps are taken without any sync to/from\nthe GPU and are therefore much faster."}, {Name: "ControlDt", Doc: "ControlDt is the stepsize for integrating joint control position values\n[JointTargetPos] over time, to avoid sudden strong changes in force.\nFor higher-DoF joints (e.g., Ball), this can be important for stability,\nbut it can also result in under-shoot of the target position."}, {Name: "ControlDtThr", Doc: "ControlDtThr is the threshold on the control delta above which\nControlDt is used. ControlDt is most important for large changes,\nand can result in under-shoot if engaged for small changes."}, {Name: "ContactMargin", Doc: "Contact margin is the extra distance for broadphase collision\naround rigid bodies. This can make some joints potentially unstable if > 0"}, {Name: "ContactRelax", Doc: "ContactRelax is rigid contact relaxation constant.\nHigher values cause errros"}, {Name: "ContactWeighting", Doc: "Contact weighting: balances contact forces?"}, {Name: "Restitution", Doc: "Restitution takes into account bounciness of objects."}, {Name: "JointLinearRelax", Doc: "JointLinearRelax is joint linear relaxation constant."}, {Name: "JointAngularRelax", Doc: "JointAngularRelax is joint angular relaxation constant."}, {Name: "JointLinearComply", Doc: "JointLinearComply is joint linear compliance constant."}, {Name: "JointAngularComply", Doc: "JointAngularComply is joint angular compliance constant."}, {Name: "AngularDamping", Doc: "AngularDamping is damping of angular motion."}, {Name: "SoftRelax", Doc: "SoftRelax is soft-body relaxation constant."}, {Name: "MaxForce", Doc: "MaxForce is the maximum computed force value, which prevents\nrunaway numerical overflow."}, {Name: "MaxDelta", Doc: "MaxDelta is the maximum computed change in position magnitude,\nwhich prevents runaway numerical overflow."}, {Name: "MaxGeomIter", Doc: "MaxGeomIter is number of iterations to perform in shape-based\ngeometry collision computations"}, {Name: "ContactsMax", Doc: "Maximum number of contacts to process at any given point."}, {Name: "Cur", Doc: "Index for the current state (0 or 1, alternates with Next)."}, {Name: "Next", Doc: "Index for the next state (1 or 0, alternates with Cur)."}, {Name: "BodiesN", Doc: "BodiesN is number of rigid bodies."}, {Name: "DynamicsN", Doc: "DynamicsN is number of dynamics bodies."}, {Name: "ObjectsN", Doc: "ObjectsN is number of objects."}, {Name: "MaxObjectJoints", Doc: "MaxObjectJoints is max number of joints per object."}, {Name: "JointsN", Doc: "JointsN is number of joints."}, {Name: "JointDoFsN", Doc: "JointDoFsN is number of joint DoFs."}, {Name: "BodyJointsMax", Doc: "BodyJointsMax is max number of joints per body + 1 for actual n."}, {Name: "BodyCollidePairsN", Doc: "BodyCollidePairsN is the total number of pre-compiled collision pairs\nto examine."}, {Name: "pad"}, {Name: "pad1"}, {Name: "pad2"}, {Name: "Gravity", Doc: "Gravity is the gravity acceleration function"}}}) From 039c4b6c86983427a4ae2d40d74376f3d26ae0ae Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Mon, 5 Jan 2026 12:44:03 -0800 Subject: [PATCH 85/97] physics: sensor framework and example for vestibular sense in virtroom --- physics/builder/object.go | 21 +++++++++++++ physics/dynamics.go | 14 +++++++++ physics/dynamics.goal | 14 +++++++++ physics/examples/virtroom/README.md | 2 +- physics/examples/virtroom/typegen.go | 2 +- physics/examples/virtroom/virtroom.go | 45 ++++++++++++++++++++------- 6 files changed, 84 insertions(+), 14 deletions(-) diff --git a/physics/builder/object.go b/physics/builder/object.go index bd0fc246..3933a706 100644 --- a/physics/builder/object.go +++ b/physics/builder/object.go @@ -22,6 +22,13 @@ type Object struct { // Joints are joints connecting object bodies. // Joint indexes here refer strictly within bodies. Joints []*Joint + + // Sensors are functions that can be configured to report arbitrary values + // on given body element. The output must be stored directly somewhere via + // the closure function: the utility of the sensor function is being able + // to capture all the configuration-time parameters needed to make it work, + // and to have it automatically called on replicated objects. + Sensors []func(obj *Object) } func (ob *Object) Body(idx int) *Body { @@ -38,6 +45,7 @@ func (ob *Object) Joint(idx int) *Joint { func (ob *Object) Copy(so *Object) { ob.Bodies = make([]*Body, len(so.Bodies)) ob.Joints = make([]*Joint, len(so.Joints)) + ob.Sensors = make([]func(obj *Object), len(so.Sensors)) for i := range ob.Bodies { ob.Bodies[i] = &Body{} ob.Body(i).Copy(so.Body(i)) @@ -46,6 +54,7 @@ func (ob *Object) Copy(so *Object) { ob.Joints[i] = &Joint{} ob.Joint(i).Copy(so.Joint(i)) } + copy(ob.Sensors, so.Sensors) } // CopySkins makes new skins for bodies based on those in source object. @@ -146,3 +155,15 @@ func (ob *Object) RotateOnAxisBody(body int, x, y, z, angle float32) { rot := math32.NewQuatAxisAngle(math32.Vec3(x, y, z), math32.DegToRad(angle)) ob.RotateAroundBody(body, rot) } + +//////// Sensors + +func (ob *Object) NewSensor(fun func(obj *Object)) { + ob.Sensors = append(ob.Sensors, fun) +} + +func (ob *Object) RunSensors() { + for _, sf := range ob.Sensors { + sf(ob) + } +} diff --git a/physics/dynamics.go b/physics/dynamics.go index a0c2d504..70e8ebda 100644 --- a/physics/dynamics.go +++ b/physics/dynamics.go @@ -231,3 +231,17 @@ func (ml *Model) TotalKineticEnergy() float32 { } return ke } + +// AngularVelocityAt returns the angular velocity vector of given dynamic body +// index and Next index, relative to given rotation axis at given point +// relative to the structural center of the given dynamic body. +// For example, to get rotation around the XZ plane, axis = (0,1,0) and +// the velocity value will show up in the Z axis for an X-axis point, +// and vice-versa (X for a Z-axis point). +// This uses DynamicAngVel which is computed after each step (into Next). +func AngularVelocityAt(di int32, point, axis math32.Vector3) math32.Vector3 { + params := GetParams(0) + w := DynamicAngVel(di, params.Next) + wp := slmath.Cross3(w.Mul(axis), point) + return wp +} diff --git a/physics/dynamics.goal b/physics/dynamics.goal index 77391cad..70c32438 100644 --- a/physics/dynamics.goal +++ b/physics/dynamics.goal @@ -230,3 +230,17 @@ func (ml *Model) TotalKineticEnergy() float32 { return ke } +// AngularVelocityAt returns the angular velocity vector of given dynamic body +// index and Next index, relative to given rotation axis at given point +// relative to the structural center of the given dynamic body. +// For example, to get rotation around the XZ plane, axis = (0,1,0) and +// the velocity value will show up in the Z axis for an X-axis point, +// and vice-versa (X for a Z-axis point). +// This uses DynamicAngVel which is computed after each step (into Next). +func AngularVelocityAt(di int32, point, axis math32.Vector3) math32.Vector3 { + params := GetParams(0) + w := DynamicAngVel(di, params.Next) + wp := slmath.Cross3(w.Mul(axis), point) + return wp +} + diff --git a/physics/examples/virtroom/README.md b/physics/examples/virtroom/README.md index de4ff342..482fc5db 100644 --- a/physics/examples/virtroom/README.md +++ b/physics/examples/virtroom/README.md @@ -1,4 +1,4 @@ -# xyz/physics example +# physics virtual room navigation This demo shows how to construct and visualize a [physics](../../physics) model of a simple virtual room environment, with a virtual robot ("emer") having a first-person view of the world. diff --git a/physics/examples/virtroom/typegen.go b/physics/examples/virtroom/typegen.go index fbfbb40d..4cf6ab45 100644 --- a/physics/examples/virtroom/typegen.go +++ b/physics/examples/virtroom/typegen.go @@ -6,4 +6,4 @@ import ( "cogentcore.org/core/types" ) -var _ = types.AddType(&types.Type{Name: "main.Env", IDName: "env", Doc: "Env encapsulates the virtual environment", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Methods: []types.Method{{Name: "InitState", Doc: "Initstate reinitializes the physics model state.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "GrabEyeImg", Doc: "GrabEyeImg takes a snapshot from the perspective of Emer's right eye", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "ModelStep", Doc: "ModelStep does one step of the physics model.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "StepForward", Doc: "StepForward moves Emer forward in current facing direction one step,\nand takes GrabEyeImg", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "StepBackward", Doc: "StepBackward moves Emer backward in current facing direction one step, and takes GrabEyeImg", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "RotBodyLeft", Doc: "RotBodyLeft rotates emer left and takes GrabEyeImg", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "RotBodyRight", Doc: "RotBodyRight rotates emer right and takes GrabEyeImg", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "RotHeadLeft", Doc: "RotHeadLeft rotates emer left and takes GrabEyeImg", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "RotHeadRight", Doc: "RotHeadRight rotates emer right and takes GrabEyeImg", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}}, Fields: []types.Field{{Name: "Emer", Doc: "Emer state"}, {Name: "Stiff", Doc: "Stiffness for actions"}, {Name: "MoveStep", Doc: "how far to move every step"}, {Name: "RotStep", Doc: "how far to rotate every step"}, {Name: "ModelSteps", Doc: "number of model steps to take"}, {Name: "Width", Doc: "width of room"}, {Name: "Depth", Doc: "depth of room"}, {Name: "Height", Doc: "height of room"}, {Name: "Thick", Doc: "thickness of walls of room"}, {Name: "DepthVals", Doc: "current depth map"}, {Name: "Camera", Doc: "offscreen render camera settings"}, {Name: "DepthMap", Doc: "color map to use for rendering depth map"}, {Name: "Physics", Doc: "The core physics elements: Model, Builder, Scene"}, {Name: "SceneEditor", Doc: "3D visualization of the Scene"}, {Name: "EyeRImg", Doc: "snapshot image"}, {Name: "DepthImage", Doc: "depth map image"}}}) +var _ = types.AddType(&types.Type{Name: "main.Env", IDName: "env", Doc: "Env encapsulates the virtual environment", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Methods: []types.Method{{Name: "InitState", Doc: "Initstate reinitializes the physics model state.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "GrabEyeImage", Doc: "GrabEyeImage takes a snapshot from the perspective of Emer's right eye", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "ModelStep", Doc: "ModelStep does one step of the physics model.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "StepForward", Doc: "StepForward moves Emer forward in current facing direction one step", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "StepBackward", Doc: "StepBackward moves Emer backward in current facing direction one step.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "RotBodyLeft", Doc: "RotBodyLeft rotates emer left.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "RotBodyRight", Doc: "RotBodyRight rotates emer right.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "RotHeadLeft", Doc: "RotHeadLeft rotates emer left.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "RotHeadRight", Doc: "RotHeadRight rotates emer right.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}}, Fields: []types.Field{{Name: "Emer", Doc: "Emer state"}, {Name: "Stiff", Doc: "Stiffness for actions"}, {Name: "MoveStep", Doc: "how far to move every step"}, {Name: "RotStep", Doc: "how far to rotate every step"}, {Name: "ModelSteps", Doc: "number of model steps to take"}, {Name: "Width", Doc: "width of room"}, {Name: "Depth", Doc: "depth of room"}, {Name: "Height", Doc: "height of room"}, {Name: "Thick", Doc: "thickness of walls of room"}, {Name: "DepthVals", Doc: "current depth map"}, {Name: "Camera", Doc: "offscreen render camera settings"}, {Name: "DepthMap", Doc: "color map to use for rendering depth map"}, {Name: "Physics", Doc: "The core physics elements: Model, Builder, Scene"}, {Name: "SceneEditor", Doc: "3D visualization of the Scene"}, {Name: "EyeRImg", Doc: "snapshot image"}, {Name: "DepthImage", Doc: "depth map image"}}}) diff --git a/physics/examples/virtroom/virtroom.go b/physics/examples/virtroom/virtroom.go index 8b31559f..7a8d9202 100644 --- a/physics/examples/virtroom/virtroom.go +++ b/physics/examples/virtroom/virtroom.go @@ -7,6 +7,7 @@ package main //go:generate core generate import ( + "fmt" "image" "math/rand/v2" "os" @@ -50,6 +51,10 @@ type Emer struct { // if true, emer is angry: changes face color Angry bool + // VestibHRightEar is the horizontal rotation vestibular signal measured + // at the right ear, averaged over the steps. + VestibHRightEar float32 + // height of emer Height float32 @@ -185,8 +190,8 @@ func (ev *Env) RenderEyeImg() image.Image { return ev.Physics.Scene.RenderFrom(ev.Emer.EyeR.Skin, &ev.Camera, 0) } -// GrabEyeImg takes a snapshot from the perspective of Emer's right eye -func (ev *Env) GrabEyeImg() { //types:add +// GrabEyeImage takes a snapshot from the perspective of Emer's right eye +func (ev *Env) GrabEyeImage() { //types:add img := ev.RenderEyeImg() if img != nil { ev.EyeRImg.SetImage(img) @@ -199,6 +204,11 @@ func (ev *Env) GrabEyeImg() { //types:add // } } +// Sensors reads sensors at various key points on body. +func (ev *Env) Sensors() { + ev.Emer.Obj.RunSensors() +} + // ViewDepth updates depth bitmap with depth data func (ev *Env) ViewDepth(depth []float32) { cmap := colormap.AvailableMaps[string(ev.DepthMap)] @@ -217,7 +227,13 @@ func (ev *Env) UpdateView() { // ModelStep does one step of the physics model. func (ev *Env) ModelStep() { //types:add - ev.Physics.Step(ev.ModelSteps) + ev.Emer.VestibHRightEar = 0 + for range ev.ModelSteps { + ev.Physics.Step(1) + ev.Sensors() + } + ev.Emer.VestibHRightEar /= float32(ev.ModelSteps) + fmt.Println("vestibH right ear:", ev.Emer.VestibHRightEar) ev.Emer.Angry = false ctN := physics.ContactsN.Value(0) for ci := range ctN { @@ -233,12 +249,11 @@ func (ev *Env) ModelStep() { //types:add ev.Emer.XZ.AddTargetAngle(2, rot, ev.Stiff) } } - ev.GrabEyeImg() + ev.GrabEyeImage() ev.UpdateView() } -// StepForward moves Emer forward in current facing direction one step, -// and takes GrabEyeImg +// StepForward moves Emer forward in current facing direction one step func (ev *Env) StepForward() { //types:add // doesn't integrate well with joints.. // ev.Emer.MoveOnAxisBody(0, 0, 0, 1, -ev.MoveStep) @@ -249,7 +264,7 @@ func (ev *Env) StepForward() { //types:add ev.ModelStep() } -// StepBackward moves Emer backward in current facing direction one step, and takes GrabEyeImg +// StepBackward moves Emer backward in current facing direction one step. func (ev *Env) StepBackward() { //types:add ang := math32.Pi*.5 - ev.Emer.XZ.DoF(2).Current.Pos // ang := float32(math32.Pi * .5) @@ -257,25 +272,25 @@ func (ev *Env) StepBackward() { //types:add ev.ModelStep() } -// RotBodyLeft rotates emer left and takes GrabEyeImg +// RotBodyLeft rotates emer left. func (ev *Env) RotBodyLeft() { //types:add ev.Emer.XZ.AddTargetAngle(2, ev.RotStep, ev.Stiff) ev.ModelStep() } -// RotBodyRight rotates emer right and takes GrabEyeImg +// RotBodyRight rotates emer right. func (ev *Env) RotBodyRight() { //types:add ev.Emer.XZ.AddTargetAngle(2, -ev.RotStep, ev.Stiff) ev.ModelStep() } -// RotHeadLeft rotates emer left and takes GrabEyeImg +// RotHeadLeft rotates emer left. func (ev *Env) RotHeadLeft() { //types:add ev.Emer.Neck.AddTargetAngle(0, ev.RotStep, ev.Stiff) ev.ModelStep() } -// RotHeadRight rotates emer right and takes GrabEyeImg +// RotHeadRight rotates emer right. func (ev *Env) RotHeadRight() { //types:add ev.Emer.Neck.AddTargetAngle(0, -ev.RotStep, ev.Stiff) ev.ModelStep() @@ -340,6 +355,12 @@ func (ev *Env) MakeEmer(wl *builder.World, em *Emer, name string) { em.Neck.NoLinearRotation = true // obj.NewJointFixed(emr, head, math32.Vec3(0, hh, 0), math32.Vec3(0, -headsz, 0)) + obj.NewSensor(func(obj *builder.Object) { + hd := obj.Body(1) + av := physics.AngularVelocityAt(hd.DynamicIndex, math32.Vec3(headsz, 0, 0), math32.Vec3(0, 1, 0)) + em.VestibHRightEar += av.Z // shows up in Z + }) + eyeoff := math32.Vec3(-headsz*.6, headsz*.1, -(headsz + eyesz*.3)) bd := obj.NewDynamicSkin(sc, name+"_eye-l", physics.Box, "green", mass*.001, math32.Vec3(eyesz, eyesz*.5, eyesz*.2), headPos.Add(eyeoff), rot) // bd.Group = 0 @@ -422,7 +443,7 @@ func (ev *Env) MakeToolbar(p *tree.Plan) { w.SetFunc(ev.InitState).SetText("Init").SetIcon(icons.Update) }) tree.Add(p, func(w *core.FuncButton) { - w.SetFunc(ev.GrabEyeImg).SetText("Grab Image").SetIcon(icons.Image) + w.SetFunc(ev.GrabEyeImage).SetText("Grab Image").SetIcon(icons.Image) }) tree.Add(p, func(w *core.Separator) {}) From cbc3202cdfa0b2adcc90bb4aaf427e939d7b1d74 Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Mon, 5 Jan 2026 13:38:24 -0800 Subject: [PATCH 86/97] physics: grab images automatically iterates over replicas --- physics/builder/builder.go | 3 ++- physics/examples/virtroom/virtroom.go | 2 +- physics/model.go | 21 +++++++---------- physics/model.goal | 21 +++++++---------- physics/phyxyz/editor.go | 2 +- physics/phyxyz/scene.go | 34 +++++++++++++++++++-------- 6 files changed, 44 insertions(+), 39 deletions(-) diff --git a/physics/builder/builder.go b/physics/builder/builder.go index 4e23b6bd..8e1c2c38 100644 --- a/physics/builder/builder.go +++ b/physics/builder/builder.go @@ -89,7 +89,8 @@ func (bl *Builder) Build(ml *physics.Model, sc *phyxyz.Scene) { } if repN > 0 { ml.ReplicasStart = repSt - ml.ReplicasN = repN + ml.ReplicasN = int32(bl.ReplicasN) + ml.ReplicaBodiesN = repN } } diff --git a/physics/examples/virtroom/virtroom.go b/physics/examples/virtroom/virtroom.go index 7a8d9202..68d58a6f 100644 --- a/physics/examples/virtroom/virtroom.go +++ b/physics/examples/virtroom/virtroom.go @@ -187,7 +187,7 @@ func (ev *Env) RenderEyeImg() image.Image { if ev.Emer.EyeR == nil { return nil } - return ev.Physics.Scene.RenderFrom(ev.Emer.EyeR.Skin, &ev.Camera, 0) + return ev.Physics.Scene.RenderFrom(ev.Emer.EyeR.Skin, &ev.Camera)[0] } // GrabEyeImage takes a snapshot from the perspective of Emer's right eye diff --git a/physics/model.go b/physics/model.go index 5ec4d6e2..9d652324 100644 --- a/physics/model.go +++ b/physics/model.go @@ -46,10 +46,14 @@ type Model struct { // This is the start of the World=0 first instance. ReplicasStart int32 `edit:"-"` - // ReplicasN is the number of body elements within each set of + // ReplicasN is the number of replicated worlds. + // Total bodies from ReplicasStart should be ReplicasN * ReplicaBodiesN. + ReplicasN int32 `edit:"-"` + + // ReplicaBodiesN is the number of body elements within each set of // replicated world bodies, which is needed for viewers to efficiently select // a specific world to view. - ReplicasN int32 `edit:"-"` + ReplicaBodiesN int32 `edit:"-"` // Bodies are the rigid body elements (dynamic and static), // specifying the constant, non-dynamic properties, @@ -246,15 +250,6 @@ func (ml *Model) ToGPUInfra() { ToGPU(ParamsVar, BodiesVar, ObjectsVar, JointsVar, JointDoFsVar, BodyJointsVar, BodyCollidePairsVar, DynamicsVar, BroadContactsNVar, BroadContactsVar, ContactsNVar, ContactsVar, JointControlsVar) } -// ReplicaWorldsN returns the number of replicated worlds. 0 if none. -func (ml *Model) ReplicaWorldsN() int32 { - if ml.ReplicasN == 0 { - return 0 - } - nbody := int32(ml.Bodies.DimSize(0)) - return (nbody - ml.ReplicasStart) / ml.ReplicasN -} - // ReplicasIndexes returns the body and dynamics (if dynamic) indexes // for given replica world and source body index, if ReplicasN is > 0. // Otherwise, returns bi and corresponding dynamic index. @@ -262,10 +257,10 @@ func (ml *Model) ReplicasIndexes(bi, replica int32) (bodyIdx, dynIdx int32) { if ml.ReplicasN == 0 { return bi, GetBodyDynamic(bi) } - if bi < ml.ReplicasStart || bi >= ml.ReplicasStart+ml.ReplicasN { + if bi < ml.ReplicasStart { return bi, GetBodyDynamic(bi) } - bodyIdx = (bi - ml.ReplicasStart) + ml.ReplicasStart + replica*ml.ReplicasN + bodyIdx = (bi - ml.ReplicasStart) + ml.ReplicasStart + replica*ml.ReplicaBodiesN dynIdx = GetBodyDynamic(bodyIdx) return } diff --git a/physics/model.goal b/physics/model.goal index d9ee575a..4813dab6 100644 --- a/physics/model.goal +++ b/physics/model.goal @@ -44,10 +44,14 @@ type Model struct { // This is the start of the World=0 first instance. ReplicasStart int32 `edit:"-"` - // ReplicasN is the number of body elements within each set of + // ReplicasN is the number of replicated worlds. + // Total bodies from ReplicasStart should be ReplicasN * ReplicaBodiesN. + ReplicasN int32 `edit:"-"` + + // ReplicaBodiesN is the number of body elements within each set of // replicated world bodies, which is needed for viewers to efficiently select // a specific world to view. - ReplicasN int32 `edit:"-"` + ReplicaBodiesN int32 `edit:"-"` // Bodies are the rigid body elements (dynamic and static), // specifying the constant, non-dynamic properties, @@ -244,15 +248,6 @@ func (ml *Model) ToGPUInfra() { ToGPU(ParamsVar, BodiesVar, ObjectsVar, JointsVar, JointDoFsVar, BodyJointsVar, BodyCollidePairsVar, DynamicsVar, BroadContactsNVar, BroadContactsVar, ContactsNVar, ContactsVar, JointControlsVar) } -// ReplicaWorldsN returns the number of replicated worlds. 0 if none. -func (ml *Model) ReplicaWorldsN() int32 { - if ml.ReplicasN == 0 { - return 0 - } - nbody := int32(ml.Bodies.DimSize(0)) - return (nbody - ml.ReplicasStart) / ml.ReplicasN -} - // ReplicasIndexes returns the body and dynamics (if dynamic) indexes // for given replica world and source body index, if ReplicasN is > 0. // Otherwise, returns bi and corresponding dynamic index. @@ -260,10 +255,10 @@ func (ml *Model) ReplicasIndexes(bi, replica int32) (bodyIdx, dynIdx int32) { if ml.ReplicasN == 0 { return bi, GetBodyDynamic(bi) } - if bi < ml.ReplicasStart || bi >= ml.ReplicasStart+ml.ReplicasN { + if bi < ml.ReplicasStart { return bi, GetBodyDynamic(bi) } - bodyIdx = (bi - ml.ReplicasStart) + ml.ReplicasStart + replica*ml.ReplicasN + bodyIdx = (bi - ml.ReplicasStart) + ml.ReplicasStart + replica*ml.ReplicaBodiesN dynIdx = GetBodyDynamic(bodyIdx) return } diff --git a/physics/phyxyz/editor.go b/physics/phyxyz/editor.go index d0c2b12e..1ee1ea04 100644 --- a/physics/phyxyz/editor.go +++ b/physics/phyxyz/editor.go @@ -276,7 +276,7 @@ func (pe *Editor) MakeToolbar(p *tree.Plan) { w.Styler(func(s *styles.Style) { replN := int32(0) if physics.CurModel != nil && pe.Scene != nil { - replN = physics.CurModel.ReplicaWorldsN() + replN = physics.CurModel.ReplicasN pe.Scene.ReplicasView = replN > 0 } w.SetMax(float32(replN - 1)) diff --git a/physics/phyxyz/scene.go b/physics/phyxyz/scene.go index 1b718907..1095b1aa 100644 --- a/physics/phyxyz/scene.go +++ b/physics/phyxyz/scene.go @@ -103,20 +103,14 @@ func (sc *Scene) UpdateFromPhysics() { } // RenderFrom does an offscreen render using given [Skin] -// for the camera position and orientation, returning the render image. +// for the camera position and orientation, returning the render image(s) +// for each replicated world (1 if no replicas). // Current scene camera is saved and restored. -// If ReplicasView is set, then the given replica will be used for rendering. -func (sc *Scene) RenderFrom(sk *Skin, cam *Camera, replica int) image.Image { +func (sc *Scene) RenderFrom(sk *Skin, cam *Camera) []image.Image { xysc := sc.Scene camnm := "scene-renderfrom-save" xysc.SaveCamera(camnm) rep := sc.ReplicasIndex - sc.ReplicasIndex = replica - defer func() { - sc.ReplicasIndex = rep - xysc.SetCamera(camnm) - xysc.UseMainFrame() - }() xysc.Camera.FOV = cam.FOV xysc.Camera.Near = cam.Near @@ -126,7 +120,27 @@ func (sc *Scene) RenderFrom(sk *Skin, cam *Camera, replica int) image.Image { xysc.Camera.Pose.Scale.Set(1, 1, 1) xysc.UseAltFrame(cam.Size) - return xysc.RenderGrabImage() + + ml := physics.CurModel + var imgs []image.Image + if sc.ReplicasView { + imgs = make([]image.Image, ml.ReplicasN) + for i := range ml.ReplicasN { + sc.ReplicasIndex = int(i) + sc.UpdateFromPhysics() + img := xysc.RenderGrabImage() + imgs[i] = img + } + sc.ReplicasIndex = rep + sc.UpdateFromPhysics() + } else { + img := xysc.RenderGrabImage() + imgs = []image.Image{img} + } + + xysc.SetCamera(camnm) + xysc.UseMainFrame() + return imgs } // DepthImage returns the current rendered depth image From c33cd572dd3a222b25db310447f195387327d191 Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Mon, 5 Jan 2026 22:28:37 -0800 Subject: [PATCH 87/97] physics: update to latest core main! and add RunSensors at higher levels. --- go.mod | 2 +- go.sum | 4 ++-- physics/builder/builder.go | 7 +++++++ physics/builder/object.go | 4 ++++ physics/builder/typegen.go | 10 +++++++++- physics/builder/world.go | 7 +++++++ physics/typegen.go | 2 +- yaegilab/labsymbols/cogentcore_org-lab-physics.go | 5 +++++ 8 files changed, 36 insertions(+), 5 deletions(-) diff --git a/go.mod b/go.mod index a25b59e0..b608b869 100644 --- a/go.mod +++ b/go.mod @@ -10,7 +10,7 @@ go 1.23.4 // https://github.com/googleapis/go-genproto/issues/1015 require ( - cogentcore.org/core v0.3.13-0.20251231082551-665c5f304b02 + cogentcore.org/core v0.3.13 github.com/alecthomas/assert/v2 v2.6.0 github.com/cogentcore/readline v0.1.3 github.com/cogentcore/yaegi v0.0.0-20250622201820-b7838bdd95eb diff --git a/go.sum b/go.sum index 5d04868a..f651c779 100644 --- a/go.sum +++ b/go.sum @@ -1,5 +1,5 @@ -cogentcore.org/core v0.3.13-0.20251231082551-665c5f304b02 h1:PhP95iKVXeVXIS2bnPi9hEz2F5xSN4hfcF2Lo7Y7Ytw= -cogentcore.org/core v0.3.13-0.20251231082551-665c5f304b02/go.mod h1:eDHnTCy1sBhAKN9NPsSCnBW3VAnwQBNA9nbGMo9r+Xs= +cogentcore.org/core v0.3.13 h1:+e7+SqlywIIcDQO4dP0klqaL0BBTbCjO4AXqROQtJlU= +cogentcore.org/core v0.3.13/go.mod h1:eDHnTCy1sBhAKN9NPsSCnBW3VAnwQBNA9nbGMo9r+Xs= github.com/Bios-Marcel/wastebasket/v2 v2.0.3 h1:TkoDPcSqluhLGE+EssHu7UGmLgUEkWg7kNyHyyJ3Q9g= github.com/Bios-Marcel/wastebasket/v2 v2.0.3/go.mod h1:769oPCv6eH7ugl90DYIsWwjZh4hgNmMS3Zuhe1bH6KU= github.com/BurntSushi/toml v0.3.1/go.mod h1:xHWCNGjB5oqiDr8zfno3MHue2Ht5sIBksp03qcyfWMU= diff --git a/physics/builder/builder.go b/physics/builder/builder.go index 8e1c2c38..0fe7f7eb 100644 --- a/physics/builder/builder.go +++ b/physics/builder/builder.go @@ -105,6 +105,13 @@ func (bl *Builder) InitState() { } } +// RunSensors runs the sensor functions for this Builder. +func (bl *Builder) RunSensors() { + for _, wl := range bl.Worlds { + wl.RunSensors() + } +} + // ReplicateWorld makes copies of given world to form an X,Y grid of // worlds with given optional offsets (Y, X) added between world objects. // Note that worldIdx is the index in Worlds, not the world number. diff --git a/physics/builder/object.go b/physics/builder/object.go index 3933a706..d2637bea 100644 --- a/physics/builder/object.go +++ b/physics/builder/object.go @@ -158,10 +158,14 @@ func (ob *Object) RotateOnAxisBody(body int, x, y, z, angle float32) { //////// Sensors +// NewSensor adds a new sensor function for this object. +// The closure function can capture local variables at the time +// of configuration, and write results wherever and however it is useful. func (ob *Object) NewSensor(fun func(obj *Object)) { ob.Sensors = append(ob.Sensors, fun) } +// RunSensors runs the sensor functions for this object. func (ob *Object) RunSensors() { for _, sf := range ob.Sensors { sf(ob) diff --git a/physics/builder/typegen.go b/physics/builder/typegen.go index 79ec1f0a..0e387dc3 100644 --- a/physics/builder/typegen.go +++ b/physics/builder/typegen.go @@ -210,7 +210,7 @@ func (t *DoF) SetInit(v Controls) *DoF { t.Init = v; return t } // Current are the current control values (based on method calls). func (t *DoF) SetCurrent(v Controls) *DoF { t.Current = v; return t } -var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics/builder.Object", IDName: "object", Doc: "Object is an object within the [World].\nEach object is a coherent collection of bodies, typically\nconnected by joints. This is an organizational convenience\nfor positioning elements; has no physical implications.", Fields: []types.Field{{Name: "Bodies", Doc: "Bodies are the bodies in the object."}, {Name: "Joints", Doc: "Joints are joints connecting object bodies.\nJoint indexes here refer strictly within bodies."}}}) +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics/builder.Object", IDName: "object", Doc: "Object is an object within the [World].\nEach object is a coherent collection of bodies, typically\nconnected by joints. This is an organizational convenience\nfor positioning elements; has no physical implications.", Fields: []types.Field{{Name: "Bodies", Doc: "Bodies are the bodies in the object."}, {Name: "Joints", Doc: "Joints are joints connecting object bodies.\nJoint indexes here refer strictly within bodies."}, {Name: "Sensors", Doc: "Sensors are functions that can be configured to report arbitrary values\non given body element. The output must be stored directly somewhere via\nthe closure function: the utility of the sensor function is being able\nto capture all the configuration-time parameters needed to make it work,\nand to have it automatically called on replicated objects."}}}) // SetBodies sets the [Object.Bodies]: // Bodies are the bodies in the object. @@ -221,6 +221,14 @@ func (t *Object) SetBodies(v ...*Body) *Object { t.Bodies = v; return t } // Joint indexes here refer strictly within bodies. func (t *Object) SetJoints(v ...*Joint) *Object { t.Joints = v; return t } +// SetSensors sets the [Object.Sensors]: +// Sensors are functions that can be configured to report arbitrary values +// on given body element. The output must be stored directly somewhere via +// the closure function: the utility of the sensor function is being able +// to capture all the configuration-time parameters needed to make it work, +// and to have it automatically called on replicated objects. +func (t *Object) SetSensors(v ...func(obj *Object)) *Object { t.Sensors = v; return t } + var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics/builder.Physics", IDName: "physics", Doc: "Physics provides a container and manager for the main physics elements:\n[Builder], [physics.Model], and [phyxyz.Scene]. This is helpful for\nmodels used within other apps (e.g., an AI simulation), whereas\n[phyxyz.Editor] provides a standalone GUI interface for testing models.", Fields: []types.Field{{Name: "Model", Doc: "Model has the physics Model."}, {Name: "Builder", Doc: "Builder for configuring the Model."}, {Name: "Scene", Doc: "Scene for visualizing the Model"}}}) // SetModel sets the [Physics.Model]: diff --git a/physics/builder/world.go b/physics/builder/world.go index 4f3e712a..2a549e56 100644 --- a/physics/builder/world.go +++ b/physics/builder/world.go @@ -65,3 +65,10 @@ func (wl *World) PoseToPhysics() { ob.PoseToPhysics() } } + +// RunSensors runs the sensor functions for this World. +func (wl *World) RunSensors() { + for _, ob := range wl.Objects { + ob.RunSensors() + } +} diff --git a/physics/typegen.go b/physics/typegen.go index 6a00275e..a548c7ff 100644 --- a/physics/typegen.go +++ b/physics/typegen.go @@ -22,7 +22,7 @@ var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.JointVars", var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.JointDoFVars", IDName: "joint-do-f-vars", Doc: "JointDoFVars are joint DoF state variables stored in tensor.Float32,\none for each DoF."}) -var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.Model", IDName: "model", Doc: "Model contains and manages all of the physics elements.", Directives: []types.Directive{{Tool: "go", Directive: "generate", Args: []string{"core", "generate", "-add-types", "-gosl"}}}, Fields: []types.Field{{Name: "GPU", Doc: "GPU determines whether to use GPU (else CPU)."}, {Name: "Params", Doc: "Params are global parameters."}, {Name: "GetContacts", Doc: "GetContacts will download Contacts from the GPU, if processing them on the CPU."}, {Name: "ReportTotalKE", Doc: "ReportTotalKE prints out the total computed kinetic energy in the system after\nevery step."}, {Name: "CurrentWorld", Doc: "CurrentWorld is the [BodyWorld] value to use when creating new bodies.\nSet to -1 to create global elements that interact with everything,\nwhile 0 and positive numbers only interact amongst themselves."}, {Name: "CurrentObject", Doc: "CurrentObject is the Object to use when creating new joints.\nCall NewObject to increment."}, {Name: "CurrentObjectJoint", Doc: "CurrentObjectJoint is the Joint index in CurrentObject\nto use when creating new joints."}, {Name: "ReplicasStart", Doc: "ReplicasStart is the starting body index for replicated world bodies,\nwhich is needed for viewers to efficiently select a specific world to view.\nThis is the start of the World=0 first instance."}, {Name: "ReplicasN", Doc: "ReplicasN is the number of body elements within each set of\nreplicated world bodies, which is needed for viewers to efficiently select\na specific world to view."}, {Name: "Bodies", Doc: "Bodies are the rigid body elements (dynamic and static),\nspecifying the constant, non-dynamic properties,\nwhich is initial state for dynamics.\n[body][BodyVarsN]"}, {Name: "Objects", Doc: "Objects is a list of joint indexes for each object, where each object\ncontains all the joints interconnecting an overlapping set of bodies.\nThis is known as an articulation in other physics software.\nJoints must be added in parent -> child order within objects, as joints\nare updated in sequential order within object.\n[object][MaxObjectJoints+1]"}, {Name: "BodyJoints", Doc: "BodyJoints is a list of joint indexes for each dynamic body, for aggregating.\n[dyn body][parent, child][Params.BodyJointsMax]"}, {Name: "Joints", Doc: "Joints is a list of permanent joints connecting bodies,\nwhich do not change (no dynamic variables).\n[joint][JointVarsN]"}, {Name: "JointDoFs", Doc: "JointDoFs is a list of joint DoF parameters, allocated per joint.\n[dof][JointDoFVars]"}, {Name: "BodyCollidePairs", Doc: "BodyCollidePairs are pairs of Body indexes that could potentially collide\nbased on precomputed collision logic, using World, Group, and Joint indexes.\n[BodyCollidePairsN][2]"}, {Name: "Dynamics", Doc: "Dynamics are the dynamic rigid body elements: these actually move.\nThe first set of variables are for initial values, and the second current.\n[body][cur/next][DynamicVarsN]"}, {Name: "BroadContactsN", Doc: "BroadContactsN has number of points of broad contact\nbetween bodies. [1]"}, {Name: "BroadContacts", Doc: "BroadContacts are the results of broad-phase contact processing,\nestablishing possible points of contact between bodies.\n[ContactsMax][BroadContactVarsN]"}, {Name: "ContactsN", Doc: "ContactsN has number of points of narrow (final) contact\nbetween bodies. [1]"}, {Name: "Contacts", Doc: "Contacts are the results of narrow-phase contact processing,\nwhere only actual contacts with fully-specified values are present.\n[ContactsMax][ContactVarsN]"}, {Name: "JointControls", Doc: "JointControls are dynamic joint control inputs, per joint DoF\n(in correspondence with [JointDoFs]). This can be uploaded to the\nGPU at every step.\n[dof][JointControlVarsN]"}}}) +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.Model", IDName: "model", Doc: "Model contains and manages all of the physics elements.", Directives: []types.Directive{{Tool: "go", Directive: "generate", Args: []string{"core", "generate", "-add-types", "-gosl"}}}, Fields: []types.Field{{Name: "GPU", Doc: "GPU determines whether to use GPU (else CPU)."}, {Name: "Params", Doc: "Params are global parameters."}, {Name: "GetContacts", Doc: "GetContacts will download Contacts from the GPU, if processing them on the CPU."}, {Name: "ReportTotalKE", Doc: "ReportTotalKE prints out the total computed kinetic energy in the system after\nevery step."}, {Name: "CurrentWorld", Doc: "CurrentWorld is the [BodyWorld] value to use when creating new bodies.\nSet to -1 to create global elements that interact with everything,\nwhile 0 and positive numbers only interact amongst themselves."}, {Name: "CurrentObject", Doc: "CurrentObject is the Object to use when creating new joints.\nCall NewObject to increment."}, {Name: "CurrentObjectJoint", Doc: "CurrentObjectJoint is the Joint index in CurrentObject\nto use when creating new joints."}, {Name: "ReplicasStart", Doc: "ReplicasStart is the starting body index for replicated world bodies,\nwhich is needed for viewers to efficiently select a specific world to view.\nThis is the start of the World=0 first instance."}, {Name: "ReplicasN", Doc: "ReplicasN is the number of replicated worlds.\nTotal bodies from ReplicasStart should be ReplicasN * ReplicaBodiesN."}, {Name: "ReplicaBodiesN", Doc: "ReplicaBodiesN is the number of body elements within each set of\nreplicated world bodies, which is needed for viewers to efficiently select\na specific world to view."}, {Name: "Bodies", Doc: "Bodies are the rigid body elements (dynamic and static),\nspecifying the constant, non-dynamic properties,\nwhich is initial state for dynamics.\n[body][BodyVarsN]"}, {Name: "Objects", Doc: "Objects is a list of joint indexes for each object, where each object\ncontains all the joints interconnecting an overlapping set of bodies.\nThis is known as an articulation in other physics software.\nJoints must be added in parent -> child order within objects, as joints\nare updated in sequential order within object.\n[object][MaxObjectJoints+1]"}, {Name: "BodyJoints", Doc: "BodyJoints is a list of joint indexes for each dynamic body, for aggregating.\n[dyn body][parent, child][Params.BodyJointsMax]"}, {Name: "Joints", Doc: "Joints is a list of permanent joints connecting bodies,\nwhich do not change (no dynamic variables).\n[joint][JointVarsN]"}, {Name: "JointDoFs", Doc: "JointDoFs is a list of joint DoF parameters, allocated per joint.\n[dof][JointDoFVars]"}, {Name: "BodyCollidePairs", Doc: "BodyCollidePairs are pairs of Body indexes that could potentially collide\nbased on precomputed collision logic, using World, Group, and Joint indexes.\n[BodyCollidePairsN][2]"}, {Name: "Dynamics", Doc: "Dynamics are the dynamic rigid body elements: these actually move.\nThe first set of variables are for initial values, and the second current.\n[body][cur/next][DynamicVarsN]"}, {Name: "BroadContactsN", Doc: "BroadContactsN has number of points of broad contact\nbetween bodies. [1]"}, {Name: "BroadContacts", Doc: "BroadContacts are the results of broad-phase contact processing,\nestablishing possible points of contact between bodies.\n[ContactsMax][BroadContactVarsN]"}, {Name: "ContactsN", Doc: "ContactsN has number of points of narrow (final) contact\nbetween bodies. [1]"}, {Name: "Contacts", Doc: "Contacts are the results of narrow-phase contact processing,\nwhere only actual contacts with fully-specified values are present.\n[ContactsMax][ContactVarsN]"}, {Name: "JointControls", Doc: "JointControls are dynamic joint control inputs, per joint DoF\n(in correspondence with [JointDoFs]). This can be uploaded to the\nGPU at every step.\n[dof][JointControlVarsN]"}}}) var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.PhysicsParams", IDName: "physics-params", Doc: "PhysicsParams are the physics parameters", Directives: []types.Directive{{Tool: "gosl", Directive: "start"}}, Fields: []types.Field{{Name: "Iterations", Doc: "Iterations is the number of integration iterations to perform\nwithin each solver step. Muller et al (2020) report that 1 is best."}, {Name: "Dt", Doc: "Dt is the integration stepsize.\nFor highly kinetic situations (e.g., rapidly moving bouncing balls)\n0.0001 is needed to ensure contact registration. Use SubSteps to\naccomplish a target effective read-out step size."}, {Name: "SubSteps", Doc: "SubSteps is the number of integration steps to take per Step()\nfunction call. These sub steps are taken without any sync to/from\nthe GPU and are therefore much faster."}, {Name: "ControlDt", Doc: "ControlDt is the stepsize for integrating joint control position values\n[JointTargetPos] over time, to avoid sudden strong changes in force.\nFor higher-DoF joints (e.g., Ball), this can be important for stability,\nbut it can also result in under-shoot of the target position."}, {Name: "ControlDtThr", Doc: "ControlDtThr is the threshold on the control delta above which\nControlDt is used. ControlDt is most important for large changes,\nand can result in under-shoot if engaged for small changes."}, {Name: "ContactMargin", Doc: "Contact margin is the extra distance for broadphase collision\naround rigid bodies. This can make some joints potentially unstable if > 0"}, {Name: "ContactRelax", Doc: "ContactRelax is rigid contact relaxation constant.\nHigher values cause errros"}, {Name: "ContactWeighting", Doc: "Contact weighting: balances contact forces?"}, {Name: "Restitution", Doc: "Restitution takes into account bounciness of objects."}, {Name: "JointLinearRelax", Doc: "JointLinearRelax is joint linear relaxation constant."}, {Name: "JointAngularRelax", Doc: "JointAngularRelax is joint angular relaxation constant."}, {Name: "JointLinearComply", Doc: "JointLinearComply is joint linear compliance constant."}, {Name: "JointAngularComply", Doc: "JointAngularComply is joint angular compliance constant."}, {Name: "AngularDamping", Doc: "AngularDamping is damping of angular motion."}, {Name: "SoftRelax", Doc: "SoftRelax is soft-body relaxation constant."}, {Name: "MaxForce", Doc: "MaxForce is the maximum computed force value, which prevents\nrunaway numerical overflow."}, {Name: "MaxDelta", Doc: "MaxDelta is the maximum computed change in position magnitude,\nwhich prevents runaway numerical overflow."}, {Name: "MaxGeomIter", Doc: "MaxGeomIter is number of iterations to perform in shape-based\ngeometry collision computations"}, {Name: "ContactsMax", Doc: "Maximum number of contacts to process at any given point."}, {Name: "Cur", Doc: "Index for the current state (0 or 1, alternates with Next)."}, {Name: "Next", Doc: "Index for the next state (1 or 0, alternates with Cur)."}, {Name: "BodiesN", Doc: "BodiesN is number of rigid bodies."}, {Name: "DynamicsN", Doc: "DynamicsN is number of dynamics bodies."}, {Name: "ObjectsN", Doc: "ObjectsN is number of objects."}, {Name: "MaxObjectJoints", Doc: "MaxObjectJoints is max number of joints per object."}, {Name: "JointsN", Doc: "JointsN is number of joints."}, {Name: "JointDoFsN", Doc: "JointDoFsN is number of joint DoFs."}, {Name: "BodyJointsMax", Doc: "BodyJointsMax is max number of joints per body + 1 for actual n."}, {Name: "BodyCollidePairsN", Doc: "BodyCollidePairsN is the total number of pre-compiled collision pairs\nto examine."}, {Name: "pad"}, {Name: "pad1"}, {Name: "pad2"}, {Name: "Gravity", Doc: "Gravity is the gravity acceleration function"}}}) diff --git a/yaegilab/labsymbols/cogentcore_org-lab-physics.go b/yaegilab/labsymbols/cogentcore_org-lab-physics.go index 03b53fa5..b4b4e7b0 100644 --- a/yaegilab/labsymbols/cogentcore_org-lab-physics.go +++ b/yaegilab/labsymbols/cogentcore_org-lab-physics.go @@ -14,6 +14,7 @@ func init() { // function, constant and variable definitions "AddBroadContacts": reflect.ValueOf(physics.AddBroadContacts), "AngularCorrection": reflect.ValueOf(physics.AngularCorrection), + "AngularVelocityAt": reflect.ValueOf(physics.AngularVelocityAt), "Ball": reflect.ValueOf(physics.Ball), "Bodies": reflect.ValueOf(&physics.Bodies).Elem(), "BodiesVar": reflect.ValueOf(physics.BodiesVar), @@ -234,6 +235,7 @@ func init() { "GetJointAngularDoFN": reflect.ValueOf(physics.GetJointAngularDoFN), "GetJointEnabled": reflect.ValueOf(physics.GetJointEnabled), "GetJointLinearDoFN": reflect.ValueOf(physics.GetJointLinearDoFN), + "GetJointNoLinearRotation": reflect.ValueOf(physics.GetJointNoLinearRotation), "GetJointParentFixed": reflect.ValueOf(physics.GetJointParentFixed), "GetJointTargetPos": reflect.ValueOf(physics.GetJointTargetPos), "GetJointTargetVel": reflect.ValueOf(physics.GetJointTargetVel), @@ -300,6 +302,7 @@ func init() { "JointLinLambdaY": reflect.ValueOf(physics.JointLinLambdaY), "JointLinLambdaZ": reflect.ValueOf(physics.JointLinLambdaZ), "JointLinearDoFN": reflect.ValueOf(physics.JointLinearDoFN), + "JointNoLinearRotation": reflect.ValueOf(physics.JointNoLinearRotation), "JointPForce": reflect.ValueOf(physics.JointPForce), "JointPForceX": reflect.ValueOf(physics.JointPForceX), "JointPForceY": reflect.ValueOf(physics.JointPForceY), @@ -451,6 +454,7 @@ func init() { "SetJointEnabled": reflect.ValueOf(physics.SetJointEnabled), "SetJointLinLambda": reflect.ValueOf(physics.SetJointLinLambda), "SetJointLinearDoFN": reflect.ValueOf(physics.SetJointLinearDoFN), + "SetJointNoLinearRotation": reflect.ValueOf(physics.SetJointNoLinearRotation), "SetJointPForce": reflect.ValueOf(physics.SetJointPForce), "SetJointPPos": reflect.ValueOf(physics.SetJointPPos), "SetJointPQuat": reflect.ValueOf(physics.SetJointPQuat), @@ -469,6 +473,7 @@ func init() { "StepBodyContactDeltas": reflect.ValueOf(physics.StepBodyContactDeltas), "StepBodyContacts": reflect.ValueOf(physics.StepBodyContacts), "StepBodyDeltas": reflect.ValueOf(physics.StepBodyDeltas), + "StepBodyKinetics": reflect.ValueOf(physics.StepBodyKinetics), "StepInit": reflect.ValueOf(physics.StepInit), "StepIntegrateBodies": reflect.ValueOf(physics.StepIntegrateBodies), "StepJointForces": reflect.ValueOf(physics.StepJointForces), From 0557c97a5cea3bfa7d6733b4b61eda6aaf9c3a9b Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Tue, 6 Jan 2026 07:36:58 -0800 Subject: [PATCH 88/97] physics: remove test1 from examples and put target at 0 in collision --- physics/examples/collision/collision.go | 2 +- physics/examples/test1/test1.go | 105 ------------------------ 2 files changed, 1 insertion(+), 106 deletions(-) delete mode 100644 physics/examples/test1/test1.go diff --git a/physics/examples/collision/collision.go b/physics/examples/collision/collision.go index d88c5327..eb8d38db 100644 --- a/physics/examples/collision/collision.go +++ b/physics/examples/collision/collision.go @@ -106,7 +106,7 @@ func main() { physics.SetBodyFrictionRolling(ba.BodyIndex, cl.FrictionRolling) physics.SetBodyFrictionTortion(ba.BodyIndex, cl.FrictionTortion) - bb := sc.NewDynamic(ml, "B", cl.ShapeB, "red", cl.MassB, math32.Vec3(cl.SizeB, 2*cl.SizeB, cl.SizeB), math32.Vec3(5, hhB, cl.ZposB), rot) + bb := sc.NewDynamic(ml, "B", cl.ShapeB, "red", cl.MassB, math32.Vec3(cl.SizeB, 2*cl.SizeB, cl.SizeB), math32.Vec3(0, hhB, cl.ZposB), rot) physics.SetBodyFriction(bb.BodyIndex, cl.Friction) physics.SetBodyFrictionRolling(bb.BodyIndex, cl.FrictionRolling) physics.SetBodyFrictionTortion(bb.BodyIndex, cl.FrictionTortion) diff --git a/physics/examples/test1/test1.go b/physics/examples/test1/test1.go deleted file mode 100644 index a8492e9e..00000000 --- a/physics/examples/test1/test1.go +++ /dev/null @@ -1,105 +0,0 @@ -// Copyright (c) 2025, Cogent Core. All rights reserved. -// Use of this source code is governed by a BSD-style -// license that can be found in the LICENSE file. - -package main - -//go:generate core generate -add-types - -import ( - "cogentcore.org/core/core" - "cogentcore.org/core/math32" - _ "cogentcore.org/lab/gosl/slbool/slboolcore" // include to get gui views - "cogentcore.org/lab/physics" - "cogentcore.org/lab/physics/phyxyz" -) - -// Test has sim params -type Test struct { - // Height to start from - Height float32 - - // Size of test item - Size float32 - - // Mass of test item - Mass float32 - - Bounce float32 - Friction float32 - FrictionTortion float32 - FrictionRolling float32 -} - -func (b *Test) Defaults() { - b.Height = 50 - b.Size = 0.5 - b.Mass = 0.1 - - b.Bounce = 0.5 - b.Friction = 0 - b.FrictionTortion = 0 - b.FrictionRolling = 0 -} - -func main() { - b := core.NewBody("test1").SetTitle("Physics Test1") - ed := phyxyz.NewEditor(b) - ed.CameraPos = math32.Vec3(0, 3, 3) - - bs := &Test{} - bs.Defaults() - - ed.SetUserParams(bs) - - ed.SetConfigFunc(func() { - ml := ed.Model - ml.GPU = false - sc := ed.Scene - rot := math32.NewQuatIdentity() - // thick := float32(0.1) - fl := sc.NewBody(ml, "floor", physics.Plane, "#D0D0D080", math32.Vec3(10, 0, 10), - math32.Vec3(0, 0, 0), rot) - fl.SetBodyBounce(bs.Bounce) - - height := float32(bs.Size) - width := height * .4 - depth := height * .15 - _, _ = width, depth - // b1 := wr.NewDynamic(ml, "body", physics.Box, "purple", 1.0, math32.Vec3(height, height, depth), - // math32.Vec3(0, height*2, 0), rot) - // b1 := sc.NewDynamic(ml, "body", physics.Sphere, "purple", 1.0, math32.Vec3(height, height, height), math32.Vec3(0, height*bs.Height, 0), rot) - b1 := sc.NewDynamic(ml, "body", physics.Box, "purple", 1.0, math32.Vec3(width, height, depth), math32.Vec3(0, bs.Size, 0), rot) - physics.SetBodyGroup(b1.BodyIndex, 0) - - // rleft := math32.NewQuatAxisAngle(math32.Vec3(0, 0, 1), -math32.Pi/2) - // b2 := sc.NewDynamic(ml, "nose", physics.Capsule, "blue", .001, math32.Vec3(0.1*depth, 0.1*height, 0.1*depth), math32.Vec3(-depth, 2*bs.Size, 0), rleft) - // b1.SetBodyBounce(bs.Bounce) - _ = b1 - - // bj := ml.NewJointRevolute(-1, b1.DynamicIndex, math32.Vec3(0, 0, 0), math32.Vec3(0, -height, 0), math32.Vec3(0, 1, 0)) - // bj := ml.NewJointPrismatic(-1, b1.DynamicIndex, math32.Vec3(0, 0, 0), math32.Vec3(0, -height, 0), math32.Vec3(1, 0, 0)) - // // // physics.SetJointControlForce(bj, 0, .1) - // physics.SetJointTargetPos(bj, 0, 1, 1) - // physics.SetJointTargetVel(bj, 0, 0, 1) - // ml.NewJointFixed(b1.DynamicIndex, b2.DynamicIndex, math32.Vec3(-depth, bs.Size, 0), math32.Vec3(-0.1*depth, 0, 0)) - bj := ml.NewJointBall(-1, b1.DynamicIndex, math32.Vec3(0, 0, 0), math32.Vec3(0, -height, 0)) - physics.SetJointTargetPos(bj, 0, 0, 1000) - physics.SetJointTargetVel(bj, 0, 0, 20) - physics.SetJointTargetPos(bj, 1, 0, 1000) - physics.SetJointTargetVel(bj, 1, 0, 20) - physics.SetJointTargetPos(bj, 2, 0, 1000) - physics.SetJointTargetVel(bj, 2, 0, 20) - }) - - // ed.SetControlFunc(func(timeStep int) { - // if timeStep >= ps.ForceOn && timeStep < ps.ForceOff { - // fmt.Println(timeStep, "\tforce on:", ps.Force) - // physics.SetJointControlForce(botJoint.JointIndex, 0, ps.Force) - // } else { - // physics.SetJointControlForce(botJoint.JointIndex, 0, 0) - // } - // }) - - b.RunMainWindow() -} From 3ae83536e5c5c712623845e8fec571805753fa6c Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Sun, 11 Jan 2026 13:18:17 -0800 Subject: [PATCH 89/97] physics: replicate worlds updates to support accessing joints and bodies in builder. --- physics/builder/body.go | 17 +++++-- physics/builder/builder.go | 68 ++++++++++++++++++++------- physics/builder/builder_test.go | 55 ++++++++++++++++++++++ physics/builder/joint.go | 17 ++++++- physics/builder/object.go | 12 +++++ physics/builder/typegen.go | 62 ++++++++++++++++++++---- physics/builder/world.go | 23 +++++++-- physics/dynamics.go | 14 ++++++ physics/dynamics.goal | 14 ++++++ physics/examples/virtroom/virtroom.go | 20 ++++---- physics/model.go | 46 ++++++++++++------ physics/model.goal | 46 ++++++++++++------ physics/phyxyz/skin.go | 2 +- physics/phyxyz/typegen.go | 11 +++-- physics/typegen.go | 2 +- 15 files changed, 330 insertions(+), 79 deletions(-) create mode 100644 physics/builder/builder_test.go diff --git a/physics/builder/body.go b/physics/builder/body.go index e82f42d0..e10af34b 100644 --- a/physics/builder/body.go +++ b/physics/builder/body.go @@ -12,9 +12,18 @@ import ( // Body is a rigid body. type Body struct { - // ObjectIndex is the index of body within parent [Object], - // which is used for id in [Builder] context. - ObjectIndex int + // World is the world number for physics: -1 = globals, else positive + // are distinct non-interacting worlds. + World int + + // WorldIndex is the index of world within builder Worlds list. + WorldIndex int + + // Object is the index within World's Objects list. + Object int + + // ObjectBody is the index within the Object's Bodies list. + ObjectBody int // Shape of the body. Shape physics.Shapes @@ -85,7 +94,7 @@ type Body struct { // Use this for Static elements; NewDynamic for dynamic elements. func (ob *Object) NewBody(shape physics.Shapes, hsize, pos math32.Vector3, rot math32.Quat) *Body { idx := len(ob.Bodies) - bd := &Body{ObjectIndex: idx, Shape: shape, HSize: hsize} + bd := &Body{World: ob.World, WorldIndex: ob.WorldIndex, Object: ob.Object, ObjectBody: idx, Shape: shape, HSize: hsize} bd.Pose.Pos = pos bd.Pose.Quat = rot bd.Group = -1 // default static diff --git a/physics/builder/builder.go b/physics/builder/builder.go index 0fe7f7eb..2795a9d8 100644 --- a/physics/builder/builder.go +++ b/physics/builder/builder.go @@ -56,25 +56,27 @@ func (bl *Builder) NewWorld() *World { if idx > 0 { wn = bl.Worlds[idx-1].World + 1 } - bl.Worlds = append(bl.Worlds, &World{World: wn}) + bl.Worlds = append(bl.Worlds, &World{World: wn, WorldIndex: idx}) return bl.Worlds[idx] } // Build builds a physics model, with optional [phyxyz.Scene] for // visualization (using Skin elements created for bodies). func (bl *Builder) Build(ml *physics.Model, sc *phyxyz.Scene) { - repSt := int32(0) - repN := int32(0) + bSt := int32(-1) + bN := int32(0) + jSt := int32(-1) + jN := int32(0) for wi, wl := range bl.Worlds { // fmt.Println("\n######## World:", wl.World) for _, ob := range wl.Objects { // fmt.Println("\n\t#### Object") - for bbi, bd := range ob.Bodies { + for _, bd := range ob.Bodies { bd.NewPhysicsBody(ml, wl.World) if bl.ReplicasN > 0 && wi == bl.ReplicasStart { - repN++ - if bbi == 0 { - repSt = bd.BodyIndex + bN++ + if bSt < 0 { + bSt = bd.BodyIndex } } } @@ -84,13 +86,21 @@ func (bl *Builder) Build(ml *physics.Model, sc *phyxyz.Scene) { ml.NewObject() for _, jd := range ob.Joints { jd.NewPhysicsJoint(ml, ob) + if bl.ReplicasN > 0 && wi == bl.ReplicasStart { + jN++ + if jSt < 0 { + jSt = jd.JointIndex + } + } } } } - if repN > 0 { - ml.ReplicasStart = repSt + if bN > 0 { ml.ReplicasN = int32(bl.ReplicasN) - ml.ReplicaBodiesN = repN + ml.ReplicaBodiesStart = bSt + ml.ReplicaBodiesN = bN + ml.ReplicaJointsStart = jSt + ml.ReplicaJointsN = jN } } @@ -129,15 +139,16 @@ func (bl *Builder) ReplicateWorld(sc *phyxyz.Scene, worldIdx, nY, nX int, offs . if len(offs) > 1 { Xoff = offs[1] } + for y := range nY { for x := range nX { if x == 0 && y == 0 { continue } nw := bl.NewWorld() - wi := nw.World + wi := nw.WorldIndex nw.Copy(src) - nw.World = wi + nw.SetWorldIndex(wi) off := Yoff.MulScalar(float32(y)).Add(Xoff.MulScalar(float32(x))) nw.Move(off) if sc != nil { @@ -145,8 +156,33 @@ func (bl *Builder) ReplicateWorld(sc *phyxyz.Scene, worldIdx, nY, nX int, offs . } } } - if sc == nil { - bl.ReplicasStart = worldIdx - bl.ReplicasN = nY * nX - } + bl.ReplicasStart = worldIdx + bl.ReplicasN = nY * nX +} + +// ReplicaWorld returns the replica World at given replica index, +// Where replica is index into replicated worlds (0 = original). +func (bl *Builder) ReplicaWorld(replica int) *World { + return bl.Worlds[bl.ReplicasStart+replica] +} + +// ReplicaObject returns the replica corresponding to given [Object], +// Where replica is index into replicated worlds (0 = original). +func (bl *Builder) ReplicaObject(ob *Object, replica int) *Object { + wl := bl.ReplicaWorld(replica) + return wl.Object(ob.Object) +} + +// ReplicaBody returns the replica corresponding to given [Body], +// Where replica is index into replicated worlds (0 = original). +func (bl *Builder) ReplicaBody(bd *Body, replica int) *Body { + wl := bl.ReplicaWorld(replica) + return wl.Object(bd.Object).Body(bd.ObjectBody) +} + +// ReplicaJoint returns the replica corresponding to given [Joint], +// Where replica is index into replicated worlds (0 = original). +func (bl *Builder) ReplicaJoint(bd *Joint, replica int) *Joint { + wl := bl.ReplicaWorld(replica) + return wl.Object(bd.Object).Joint(bd.ObjectJoint) } diff --git a/physics/builder/builder_test.go b/physics/builder/builder_test.go new file mode 100644 index 00000000..fef0a839 --- /dev/null +++ b/physics/builder/builder_test.go @@ -0,0 +1,55 @@ +// Copyright (c) 2025, Cogent Core. All rights reserved. +// Use of this source code is governed by a BSD-style +// license that can be found in the LICENSE file. + +package builder + +import ( + "testing" + + "cogentcore.org/core/math32" + "cogentcore.org/lab/physics" + "github.com/stretchr/testify/assert" +) + +func TestReplicate(t *testing.T) { + rot := math32.NewQuatIdentity() + bl := NewBuilder() + wls := bl.NewGlobalWorld() + ob := wls.NewObject() + ob.NewBody(physics.Box, math32.Vec3(1, 2, 3), math32.Vec3(0, 2, 0), rot) + ob.NewBody(physics.Capsule, math32.Vec3(1, 2, 3), math32.Vec3(2, 2, 0), rot) + + wld := bl.NewWorld() + obd := wld.NewObject() + bdd := obd.NewDynamic(physics.Box, 0.5, math32.Vec3(1, 2, 3), math32.Vec3(0, 2, 0), rot) + bj := obd.NewJointPlaneXZ(nil, bdd, math32.Vec3(0, 0, 0), math32.Vec3(0, -2, 0)) + + bl.ReplicateWorld(nil, 1, 4, 2) + + assert.Equal(t, 8, bl.ReplicasN) + assert.Equal(t, 1, bl.ReplicasStart) + + for wi, wl := range bl.Worlds { + assert.Equal(t, wi, wl.WorldIndex) + for oi, ob := range wl.Objects { + assert.Equal(t, wi, ob.WorldIndex) + assert.Equal(t, oi, ob.Object) + for bi, bd := range ob.Bodies { + assert.Equal(t, wi, bd.WorldIndex) + assert.Equal(t, oi, bd.Object) + assert.Equal(t, bi, bd.ObjectBody) + } + } + } + assert.Equal(t, wls, bl.World(0)) + assert.Equal(t, wld, bl.World(1)) + assert.Equal(t, wld, bl.ReplicaWorld(0)) + assert.Equal(t, obd, bl.ReplicaObject(obd, 0)) + assert.Equal(t, bdd, bl.ReplicaBody(bdd, 0)) + assert.Equal(t, bj, bl.ReplicaJoint(bj, 0)) + + assert.Equal(t, 3, bl.ReplicaBody(bdd, 2).WorldIndex) + assert.Equal(t, 0, bl.ReplicaBody(bdd, 2).Object) + assert.Equal(t, 0, bl.ReplicaBody(bdd, 2).ObjectBody) +} diff --git a/physics/builder/joint.go b/physics/builder/joint.go index ce3528f6..5d695e5d 100644 --- a/physics/builder/joint.go +++ b/physics/builder/joint.go @@ -12,6 +12,19 @@ import ( // Joint describes a joint between two bodies. type Joint struct { + // World is the world number for physics: -1 = globals, else positive + // are distinct non-interacting worlds. + World int + + // WorldIndex is the index of world within builder Worlds list. + WorldIndex int + + // Object is the index within World's Objects list. + Object int + + // ObjectJoint is the index within Object's Joints list. + ObjectJoint int + // Parent is index within an Object for parent body. // -1 for world-anchored parent. Parent int @@ -130,10 +143,10 @@ func (df *DoF) Copy(sd *DoF) { func (ob *Object) newJoint(typ physics.JointTypes, parent, child *Body, ppos, cpos math32.Vector3, linDoF, angDoF int) *Joint { pidx := -1 if parent != nil { - pidx = parent.ObjectIndex + pidx = parent.ObjectBody } idx := len(ob.Joints) - ob.Joints = append(ob.Joints, &Joint{Parent: pidx, Child: child.ObjectIndex, Type: typ, LinearDoFN: linDoF, AngularDoFN: angDoF}) + ob.Joints = append(ob.Joints, &Joint{World: ob.World, WorldIndex: ob.WorldIndex, Object: ob.Object, ObjectJoint: idx, Parent: pidx, Child: child.ObjectBody, Type: typ, LinearDoFN: linDoF, AngularDoFN: angDoF}) jd := ob.Joint(idx) jd.PPose.Pos = ppos jd.PPose.Quat = math32.NewQuatIdentity() diff --git a/physics/builder/object.go b/physics/builder/object.go index d2637bea..c02d4d86 100644 --- a/physics/builder/object.go +++ b/physics/builder/object.go @@ -16,6 +16,16 @@ import ( // connected by joints. This is an organizational convenience // for positioning elements; has no physical implications. type Object struct { + // World is the world number for physics: -1 = globals, else positive + // are distinct non-interacting worlds. + World int + + // WorldIndex is the index of world within builder Worlds list. + WorldIndex int + + // Object is the index within World's Objects list. + Object int + // Bodies are the bodies in the object. Bodies []*Body @@ -43,6 +53,8 @@ func (ob *Object) Joint(idx int) *Joint { // (The objects will be identical after, regardless of current starting // condition). func (ob *Object) Copy(so *Object) { + ob.World = so.World + ob.Object = so.Object ob.Bodies = make([]*Body, len(so.Bodies)) ob.Joints = make([]*Joint, len(so.Joints)) ob.Sensors = make([]func(obj *Object), len(so.Sensors)) diff --git a/physics/builder/typegen.go b/physics/builder/typegen.go index 0e387dc3..018adff3 100644 --- a/physics/builder/typegen.go +++ b/physics/builder/typegen.go @@ -10,12 +10,24 @@ import ( "cogentcore.org/lab/physics/phyxyz" ) -var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics/builder.Body", IDName: "body", Doc: "Body is a rigid body.", Fields: []types.Field{{Name: "ObjectIndex", Doc: "ObjectIndex is the index of body within parent [Object],\nwhich is used for id in [Builder] context."}, {Name: "Shape", Doc: "Shape of the body."}, {Name: "Dynamic", Doc: "Dynamic makes this a dynamic body."}, {Name: "Group", Doc: "Group partitions bodies within worlds into different groups\nfor collision detection. 0 does not collide with anything.\nNegative numbers are global within a world, except they don't\ncollide amongst themselves (all non-dynamic bodies should go\nin -1 because they don't collide amongst each-other, but do\npotentially collide with dynamics).\nPositive numbers only collide amongst themselves, and with\nnegative groups, but not other positive groups. To avoid\nunwanted collisions, put bodies into separate groups.\nThere is an automatic constraint that the two objects\nwithin a single joint do not collide with each other, so this\ndoes not need to be handled here."}, {Name: "HSize", Doc: "HSize is the half-size (e.g., radius) of the body.\nValues depend on shape type: X is generally radius,\nY is half-height."}, {Name: "Thick", Doc: "Thick is the thickness of the body, as a hollow shape.\nIf 0, then it is a solid shape (default)."}, {Name: "Mass", Doc: "Mass of the object. Only relevant for Dynamic bodies."}, {Name: "Pose", Doc: "Pose has the position and rotation."}, {Name: "Com", Doc: "Com is the center-of-mass offset from the Pose.Pos."}, {Name: "Bounce", Doc: "Bounce specifies the COR or coefficient of restitution (0..1),\nwhich determines how elastic the collision is,\ni.e., final velocity / initial velocity."}, {Name: "Friction", Doc: "Friction is the standard coefficient for linear friction (mu)."}, {Name: "FrictionTortion", Doc: "FrictionTortion is resistance to spinning at the contact point."}, {Name: "FrictionRolling", Doc: "FrictionRolling is resistance to rolling motion at contact."}, {Name: "Skin", Doc: "Optional [phyxyz.Skin] for visualizing the body."}, {Name: "BodyIndex", Doc: "BodyIndex is the index of this body in the [physics.Model] Bodies list,\nonce built."}, {Name: "DynamicIndex", Doc: "DynamicIndex is the index of this dynamic body in the\n[physics.Model] Dynamics list, once built."}}}) +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics/builder.Body", IDName: "body", Doc: "Body is a rigid body.", Fields: []types.Field{{Name: "World", Doc: "World is the world number for physics: -1 = globals, else positive\nare distinct non-interacting worlds."}, {Name: "WorldIndex", Doc: "WorldIndex is the index of world within builder Worlds list."}, {Name: "Object", Doc: "Object is the index within World's Objects list."}, {Name: "ObjectBody", Doc: "ObjectBody is the index within the Object's Bodies list."}, {Name: "Shape", Doc: "Shape of the body."}, {Name: "Dynamic", Doc: "Dynamic makes this a dynamic body."}, {Name: "Group", Doc: "Group partitions bodies within worlds into different groups\nfor collision detection. 0 does not collide with anything.\nNegative numbers are global within a world, except they don't\ncollide amongst themselves (all non-dynamic bodies should go\nin -1 because they don't collide amongst each-other, but do\npotentially collide with dynamics).\nPositive numbers only collide amongst themselves, and with\nnegative groups, but not other positive groups. To avoid\nunwanted collisions, put bodies into separate groups.\nThere is an automatic constraint that the two objects\nwithin a single joint do not collide with each other, so this\ndoes not need to be handled here."}, {Name: "HSize", Doc: "HSize is the half-size (e.g., radius) of the body.\nValues depend on shape type: X is generally radius,\nY is half-height."}, {Name: "Thick", Doc: "Thick is the thickness of the body, as a hollow shape.\nIf 0, then it is a solid shape (default)."}, {Name: "Mass", Doc: "Mass of the object. Only relevant for Dynamic bodies."}, {Name: "Pose", Doc: "Pose has the position and rotation."}, {Name: "Com", Doc: "Com is the center-of-mass offset from the Pose.Pos."}, {Name: "Bounce", Doc: "Bounce specifies the COR or coefficient of restitution (0..1),\nwhich determines how elastic the collision is,\ni.e., final velocity / initial velocity."}, {Name: "Friction", Doc: "Friction is the standard coefficient for linear friction (mu)."}, {Name: "FrictionTortion", Doc: "FrictionTortion is resistance to spinning at the contact point."}, {Name: "FrictionRolling", Doc: "FrictionRolling is resistance to rolling motion at contact."}, {Name: "Skin", Doc: "Optional [phyxyz.Skin] for visualizing the body."}, {Name: "BodyIndex", Doc: "BodyIndex is the index of this body in the [physics.Model] Bodies list,\nonce built."}, {Name: "DynamicIndex", Doc: "DynamicIndex is the index of this dynamic body in the\n[physics.Model] Dynamics list, once built."}}}) -// SetObjectIndex sets the [Body.ObjectIndex]: -// ObjectIndex is the index of body within parent [Object], -// which is used for id in [Builder] context. -func (t *Body) SetObjectIndex(v int) *Body { t.ObjectIndex = v; return t } +// SetWorld sets the [Body.World]: +// World is the world number for physics: -1 = globals, else positive +// are distinct non-interacting worlds. +func (t *Body) SetWorld(v int) *Body { t.World = v; return t } + +// SetWorldIndex sets the [Body.WorldIndex]: +// WorldIndex is the index of world within builder Worlds list. +func (t *Body) SetWorldIndex(v int) *Body { t.WorldIndex = v; return t } + +// SetObject sets the [Body.Object]: +// Object is the index within World's Objects list. +func (t *Body) SetObject(v int) *Body { t.Object = v; return t } + +// SetObjectBody sets the [Body.ObjectBody]: +// ObjectBody is the index within the Object's Bodies list. +func (t *Body) SetObjectBody(v int) *Body { t.ObjectBody = v; return t } // SetShape sets the [Body.Shape]: // Shape of the body. @@ -111,7 +123,24 @@ func (t *Builder) SetReplicasStart(v int) *Builder { t.ReplicasStart = v; return // Set by ReplicateWorld, and used to set corresponding value in Model. func (t *Builder) SetReplicasN(v int) *Builder { t.ReplicasN = v; return t } -var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics/builder.Joint", IDName: "joint", Doc: "Joint describes a joint between two bodies.", Fields: []types.Field{{Name: "Parent", Doc: "Parent is index within an Object for parent body.\n-1 for world-anchored parent."}, {Name: "Child", Doc: "Parent is index within an Object for parent body."}, {Name: "Type", Doc: "Type is the type of the joint."}, {Name: "PPose", Doc: "PPose is the parent position and orientation of the joint\nin the parent's body-centered coordinates."}, {Name: "CPose", Doc: "CPose is the child position and orientation of the joint\nin the parent's body-centered coordinates."}, {Name: "ParentFixed", Doc: "ParentFixed does not update the parent side of the joint."}, {Name: "NoLinearRotation", Doc: "NoLinearRotation ignores the rotational (angular) effects of\nlinear joint position constraints (i.e., Coriolis and centrifugal forces)\nwhich can otherwise interfere with rotational position constraints in\njoints with both linear and angular DoFs\n(e.g., [PlaneXZ], for which this is on by default)."}, {Name: "LinearDoFN", Doc: "LinearDoFN is the number of linear degrees of freedom (3 max)."}, {Name: "AngularDoFN", Doc: "AngularDoFN is the number of linear degrees of freedom (3 max)."}, {Name: "DoFs", Doc: "DoFs are the degrees-of-freedom for this joint."}, {Name: "JointIndex", Doc: "JointIndex is the index of this joint in [physics.Joints] when built."}}}) +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics/builder.Joint", IDName: "joint", Doc: "Joint describes a joint between two bodies.", Fields: []types.Field{{Name: "World", Doc: "World is the world number for physics: -1 = globals, else positive\nare distinct non-interacting worlds."}, {Name: "WorldIndex", Doc: "WorldIndex is the index of world within builder Worlds list."}, {Name: "Object", Doc: "Object is the index within World's Objects list."}, {Name: "ObjectJoint", Doc: "ObjectJoint is the index within Object's Joints list."}, {Name: "Parent", Doc: "Parent is index within an Object for parent body.\n-1 for world-anchored parent."}, {Name: "Child", Doc: "Parent is index within an Object for parent body."}, {Name: "Type", Doc: "Type is the type of the joint."}, {Name: "PPose", Doc: "PPose is the parent position and orientation of the joint\nin the parent's body-centered coordinates."}, {Name: "CPose", Doc: "CPose is the child position and orientation of the joint\nin the parent's body-centered coordinates."}, {Name: "ParentFixed", Doc: "ParentFixed does not update the parent side of the joint."}, {Name: "NoLinearRotation", Doc: "NoLinearRotation ignores the rotational (angular) effects of\nlinear joint position constraints (i.e., Coriolis and centrifugal forces)\nwhich can otherwise interfere with rotational position constraints in\njoints with both linear and angular DoFs\n(e.g., [PlaneXZ], for which this is on by default)."}, {Name: "LinearDoFN", Doc: "LinearDoFN is the number of linear degrees of freedom (3 max)."}, {Name: "AngularDoFN", Doc: "AngularDoFN is the number of linear degrees of freedom (3 max)."}, {Name: "DoFs", Doc: "DoFs are the degrees-of-freedom for this joint."}, {Name: "JointIndex", Doc: "JointIndex is the index of this joint in [physics.Joints] when built."}}}) + +// SetWorld sets the [Joint.World]: +// World is the world number for physics: -1 = globals, else positive +// are distinct non-interacting worlds. +func (t *Joint) SetWorld(v int) *Joint { t.World = v; return t } + +// SetWorldIndex sets the [Joint.WorldIndex]: +// WorldIndex is the index of world within builder Worlds list. +func (t *Joint) SetWorldIndex(v int) *Joint { t.WorldIndex = v; return t } + +// SetObject sets the [Joint.Object]: +// Object is the index within World's Objects list. +func (t *Joint) SetObject(v int) *Joint { t.Object = v; return t } + +// SetObjectJoint sets the [Joint.ObjectJoint]: +// ObjectJoint is the index within Object's Joints list. +func (t *Joint) SetObjectJoint(v int) *Joint { t.ObjectJoint = v; return t } // SetParent sets the [Joint.Parent]: // Parent is index within an Object for parent body. @@ -210,7 +239,20 @@ func (t *DoF) SetInit(v Controls) *DoF { t.Init = v; return t } // Current are the current control values (based on method calls). func (t *DoF) SetCurrent(v Controls) *DoF { t.Current = v; return t } -var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics/builder.Object", IDName: "object", Doc: "Object is an object within the [World].\nEach object is a coherent collection of bodies, typically\nconnected by joints. This is an organizational convenience\nfor positioning elements; has no physical implications.", Fields: []types.Field{{Name: "Bodies", Doc: "Bodies are the bodies in the object."}, {Name: "Joints", Doc: "Joints are joints connecting object bodies.\nJoint indexes here refer strictly within bodies."}, {Name: "Sensors", Doc: "Sensors are functions that can be configured to report arbitrary values\non given body element. The output must be stored directly somewhere via\nthe closure function: the utility of the sensor function is being able\nto capture all the configuration-time parameters needed to make it work,\nand to have it automatically called on replicated objects."}}}) +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics/builder.Object", IDName: "object", Doc: "Object is an object within the [World].\nEach object is a coherent collection of bodies, typically\nconnected by joints. This is an organizational convenience\nfor positioning elements; has no physical implications.", Fields: []types.Field{{Name: "World", Doc: "World is the world number for physics: -1 = globals, else positive\nare distinct non-interacting worlds."}, {Name: "WorldIndex", Doc: "WorldIndex is the index of world within builder Worlds list."}, {Name: "Object", Doc: "Object is the index within World's Objects list."}, {Name: "Bodies", Doc: "Bodies are the bodies in the object."}, {Name: "Joints", Doc: "Joints are joints connecting object bodies.\nJoint indexes here refer strictly within bodies."}, {Name: "Sensors", Doc: "Sensors are functions that can be configured to report arbitrary values\non given body element. The output must be stored directly somewhere via\nthe closure function: the utility of the sensor function is being able\nto capture all the configuration-time parameters needed to make it work,\nand to have it automatically called on replicated objects."}}}) + +// SetWorld sets the [Object.World]: +// World is the world number for physics: -1 = globals, else positive +// are distinct non-interacting worlds. +func (t *Object) SetWorld(v int) *Object { t.World = v; return t } + +// SetWorldIndex sets the [Object.WorldIndex]: +// WorldIndex is the index of world within builder Worlds list. +func (t *Object) SetWorldIndex(v int) *Object { t.WorldIndex = v; return t } + +// SetObject sets the [Object.Object]: +// Object is the index within World's Objects list. +func (t *Object) SetObject(v int) *Object { t.Object = v; return t } // SetBodies sets the [Object.Bodies]: // Bodies are the bodies in the object. @@ -253,11 +295,11 @@ func (t *Pose) SetPos(v math32.Vector3) *Pose { t.Pos = v; return t } // Quat is the rotation specified as a quaternion. func (t *Pose) SetQuat(v math32.Quat) *Pose { t.Quat = v; return t } -var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics/builder.World", IDName: "world", Doc: "World is one world within the Builder.", Fields: []types.Field{{Name: "World", Doc: "World is the world index. -1 = globals, else positive.. are distinct\nnon-interacting worlds."}, {Name: "Objects", Doc: "Objects are the objects within the [World].\nEach object is a coherent collection of bodies, typically\nconnected by joints. This is an organizational convenience\nfor positioning elements; has no physical implications."}}}) +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics/builder.World", IDName: "world", Doc: "World is one world within the Builder.", Fields: []types.Field{{Name: "World", Doc: "World is the world number for physics: -1 = globals, else positive\nare distinct non-interacting worlds."}, {Name: "WorldIndex", Doc: "WorldIndex is the index of world within builder Worlds list."}, {Name: "Objects", Doc: "Objects are the objects within the [World].\nEach object is a coherent collection of bodies, typically\nconnected by joints. This is an organizational convenience\nfor positioning elements; has no physical implications."}}}) // SetWorld sets the [World.World]: -// World is the world index. -1 = globals, else positive.. are distinct -// non-interacting worlds. +// World is the world number for physics: -1 = globals, else positive +// are distinct non-interacting worlds. func (t *World) SetWorld(v int) *World { t.World = v; return t } // SetObjects sets the [World.Objects]: diff --git a/physics/builder/world.go b/physics/builder/world.go index 2a549e56..b4603e1f 100644 --- a/physics/builder/world.go +++ b/physics/builder/world.go @@ -11,10 +11,13 @@ import ( // World is one world within the Builder. type World struct { - // World is the world index. -1 = globals, else positive.. are distinct - // non-interacting worlds. + // World is the world number for physics: -1 = globals, else positive + // are distinct non-interacting worlds. World int + // WorldIndex is the index of world within builder Worlds list. + WorldIndex int `set:"-"` + // Objects are the objects within the [World]. // Each object is a coherent collection of bodies, typically // connected by joints. This is an organizational convenience @@ -28,7 +31,7 @@ func (wl *World) Object(idx int) *Object { func (wl *World) NewObject() *Object { idx := len(wl.Objects) - wl.Objects = append(wl.Objects, &Object{}) + wl.Objects = append(wl.Objects, &Object{World: wl.World, WorldIndex: wl.WorldIndex, Object: idx}) return wl.Objects[idx] } @@ -51,6 +54,20 @@ func (wl *World) CopySkins(sc *phyxyz.Scene, ow *World) { } } +// SetWorldIndex sets the WorldIndex for this and all children. +func (wl *World) SetWorldIndex(wi int) { + wl.WorldIndex = wi + for _, ob := range wl.Objects { + ob.WorldIndex = wi + for _, bd := range ob.Bodies { + bd.WorldIndex = wi + } + for _, jd := range ob.Joints { + jd.WorldIndex = wi + } + } +} + // Move moves all objects in world by given delta. func (wl *World) Move(delta math32.Vector3) { for _, ob := range wl.Objects { diff --git a/physics/dynamics.go b/physics/dynamics.go index 70e8ebda..fcc3d76a 100644 --- a/physics/dynamics.go +++ b/physics/dynamics.go @@ -245,3 +245,17 @@ func AngularVelocityAt(di int32, point, axis math32.Vector3) math32.Vector3 { wp := slmath.Cross3(w.Mul(axis), point) return wp } + +// AngularAccelAt returns the angular acceleration vector of given dynamic body +// index and Next index, relative to given rotation axis at given point +// relative to the structural center of the given dynamic body. +// For example, to get rotation around the XZ plane, axis = (0,1,0) and +// the acceleration value will show up in the Z axis for an X-axis point, +// and vice-versa (X for a Z-axis point). +// This uses DynamicAngAcc which is computed after each step (into Next). +func AngularAccelAt(di int32, point, axis math32.Vector3) math32.Vector3 { + params := GetParams(0) + w := DynamicAngAcc(di, params.Next) + wp := slmath.Cross3(w.Mul(axis), point) + return wp +} diff --git a/physics/dynamics.goal b/physics/dynamics.goal index 70c32438..212c51d3 100644 --- a/physics/dynamics.goal +++ b/physics/dynamics.goal @@ -244,3 +244,17 @@ func AngularVelocityAt(di int32, point, axis math32.Vector3) math32.Vector3 { return wp } +// AngularAccelAt returns the angular acceleration vector of given dynamic body +// index and Next index, relative to given rotation axis at given point +// relative to the structural center of the given dynamic body. +// For example, to get rotation around the XZ plane, axis = (0,1,0) and +// the acceleration value will show up in the Z axis for an X-axis point, +// and vice-versa (X for a Z-axis point). +// This uses DynamicAngAcc which is computed after each step (into Next). +func AngularAccelAt(di int32, point, axis math32.Vector3) math32.Vector3 { + params := GetParams(0) + w := DynamicAngAcc(di, params.Next) + wp := slmath.Cross3(w.Mul(axis), point) + return wp +} + diff --git a/physics/examples/virtroom/virtroom.go b/physics/examples/virtroom/virtroom.go index 68d58a6f..a95c7a33 100644 --- a/physics/examples/virtroom/virtroom.go +++ b/physics/examples/virtroom/virtroom.go @@ -47,6 +47,7 @@ func main() { b.RunMainWindow() } +// Emer is the robot agent in the environment. type Emer struct { // if true, emer is angry: changes face color Angry bool @@ -55,15 +56,12 @@ type Emer struct { // at the right ear, averaged over the steps. VestibHRightEar float32 - // height of emer + // full height of emer Height float32 // emer object Obj *builder.Object `display:"-"` - // Revolute joint controlling orientation. - Orient *builder.Joint - // PlaneXZ joint for controlling 2D position. XZ *builder.Joint @@ -145,7 +143,7 @@ func (ev *Env) Defaults() { ev.Camera.FOV = 90 } -func (ev *Env) MakeWorld(sc *xyz.Scene) { +func (ev *Env) MakeModel(sc *xyz.Scene) { ev.Physics.Model = physics.NewModel() ev.Physics.Builder = builder.NewBuilder() ev.Physics.Model.GPU = false @@ -161,7 +159,7 @@ func (ev *Env) MakeWorld(sc *xyz.Scene) { ev.MakeRoom(wl, "room1", ev.Width, ev.Depth, ev.Height, ev.Thick) ew := ev.Physics.Builder.NewWorld() ev.MakeEmer(ew, &ev.Emer, "emer") - // ev.Physics.Builder.ReplicateWorld(1, 8, 2) + // vw.Physics.Builder.ReplicateWorld(vw.Physics.Scene, 1, 1, 8) // 1x8 ev.Physics.Build() // params := physics.GetParams(0) // params.ControlDt = 1 @@ -305,8 +303,8 @@ func (ev *Env) MakeRoom(wl *builder.World, name string, width, depth, height, th ht := thick / 2 obj := wl.NewObject() sc := ev.Physics.Scene - obj.NewBodySkin(sc, name+"_floor", physics.Box, "grey", math32.Vec3(hw, ht, hd), - math32.Vec3(0, -ht, 0), rot) + obj.NewBodySkin(sc, name+"_floor", physics.Plane, "grey", math32.Vec3(hw, 0, hd), + math32.Vec3(0, 0, 0), rot) obj.NewBodySkin(sc, name+"_back-wall", physics.Box, "blue", math32.Vec3(hw, hh, ht), math32.Vec3(0, hh, -hd), rot) obj.NewBodySkin(sc, name+"_left-wall", physics.Box, "red", math32.Vec3(ht, hh, hd), @@ -367,7 +365,7 @@ func (ev *Env) MakeEmer(wl *builder.World, em *Emer, name string) { ej := obj.NewJointFixed(head, bd, eyeoff, math32.Vec3(0, 0, -eyesz*.3)) ej.ParentFixed = true - eyeoff = math32.Vec3(headsz*.6, headsz*.1, -(headsz + eyesz*.3)) + eyeoff.X = headsz * .6 em.EyeR = obj.NewDynamicSkin(sc, name+"_eye-r", physics.Box, "green", mass*.001, math32.Vec3(eyesz, eyesz*.5, eyesz*.2), headPos.Add(eyeoff), rot) // em.EyeR.Group = 0 ej = obj.NewJointFixed(head, em.EyeR, eyeoff, math32.Vec3(0, 0, -eyesz*.3)) @@ -394,7 +392,7 @@ func (ev *Env) ConfigGUI() *core.Body { ev.SceneEditor = xyzcore.NewSceneEditor(scfr) ev.SceneEditor.UpdateWidget() sc := ev.SceneEditor.SceneXYZ() - ev.MakeWorld(sc) + ev.MakeModel(sc) // local toolbar for manipulating emer // etb.Maker(phyxyz.MakeStateToolbar(&ev.Emer.Rel, func() { @@ -506,7 +504,7 @@ func (ev *Env) NoGUIRun() { panic(err) } sc := phyxyz.NoDisplayScene(gp, dev) - ev.MakeWorld(sc) + ev.MakeModel(sc) img := ev.RenderEyeImg() if img != nil { diff --git a/physics/model.go b/physics/model.go index 9d652324..d4d7bccf 100644 --- a/physics/model.go +++ b/physics/model.go @@ -41,20 +41,30 @@ type Model struct { // to use when creating new joints. CurrentObjectJoint int `edit:"-"` - // ReplicasStart is the starting body index for replicated world bodies, - // which is needed for viewers to efficiently select a specific world to view. - // This is the start of the World=0 first instance. - ReplicasStart int32 `edit:"-"` - // ReplicasN is the number of replicated worlds. // Total bodies from ReplicasStart should be ReplicasN * ReplicaBodiesN. ReplicasN int32 `edit:"-"` + // ReplicaBodiesStart is the starting body index for replicated world bodies, + // which is needed to efficiently select a body from a specific world. + // This is the start of the World=0 first instance. + ReplicaBodiesStart int32 `edit:"-"` + // ReplicaBodiesN is the number of body elements within each set of - // replicated world bodies, which is needed for viewers to efficiently select - // a specific world to view. + // replicated world bodies, which is needed to efficiently select + // a body from a specific world. ReplicaBodiesN int32 `edit:"-"` + // ReplicaJointsStart is the starting joint index for replicated world joints, + // which is needed to efficiently select a joint from a specific world. + // This is the start of the World=0 first instance. + ReplicaJointsStart int32 `edit:"-"` + + // ReplicaJointsN is the number of joint elements within each set of + // replicated world joints, which is needed to efficiently select + // a joint from a specific world. + ReplicaJointsN int32 `edit:"-"` + // Bodies are the rigid body elements (dynamic and static), // specifying the constant, non-dynamic properties, // which is initial state for dynamics. @@ -250,17 +260,25 @@ func (ml *Model) ToGPUInfra() { ToGPU(ParamsVar, BodiesVar, ObjectsVar, JointsVar, JointDoFsVar, BodyJointsVar, BodyCollidePairsVar, DynamicsVar, BroadContactsNVar, BroadContactsVar, ContactsNVar, ContactsVar, JointControlsVar) } -// ReplicasIndexes returns the body and dynamics (if dynamic) indexes +// ReplicasBodyIndexes returns the body and dynamics (if dynamic) indexes // for given replica world and source body index, if ReplicasN is > 0. // Otherwise, returns bi and corresponding dynamic index. -func (ml *Model) ReplicasIndexes(bi, replica int32) (bodyIdx, dynIdx int32) { - if ml.ReplicasN == 0 { +func (ml *Model) ReplicasBodyIndexes(bi, replica int32) (bodyIdx, dynIdx int32) { + if ml.ReplicasN == 0 || bi < ml.ReplicaBodiesStart { return bi, GetBodyDynamic(bi) } - if bi < ml.ReplicasStart { - return bi, GetBodyDynamic(bi) - } - bodyIdx = (bi - ml.ReplicasStart) + ml.ReplicasStart + replica*ml.ReplicaBodiesN + bodyIdx = (bi - ml.ReplicaBodiesStart) + ml.ReplicaBodiesStart + replica*ml.ReplicaBodiesN dynIdx = GetBodyDynamic(bodyIdx) return } + +// ReplicasJointIndex returns the joint indexe for given replica +// world and source body index, if ReplicasN is > 0. +// Otherwise, returns bi and corresponding dynamic index. +func (ml *Model) ReplicasJointIndex(ji, replica int32) int32 { + if ml.ReplicasN == 0 || ji < ml.ReplicaJointsStart { + return ji + } + nji := (ji - ml.ReplicaJointsStart) + ml.ReplicaJointsStart + replica*ml.ReplicaJointsN + return nji +} diff --git a/physics/model.goal b/physics/model.goal index 4813dab6..065154a5 100644 --- a/physics/model.goal +++ b/physics/model.goal @@ -39,20 +39,30 @@ type Model struct { // to use when creating new joints. CurrentObjectJoint int `edit:"-"` - // ReplicasStart is the starting body index for replicated world bodies, - // which is needed for viewers to efficiently select a specific world to view. - // This is the start of the World=0 first instance. - ReplicasStart int32 `edit:"-"` - // ReplicasN is the number of replicated worlds. // Total bodies from ReplicasStart should be ReplicasN * ReplicaBodiesN. ReplicasN int32 `edit:"-"` + // ReplicaBodiesStart is the starting body index for replicated world bodies, + // which is needed to efficiently select a body from a specific world. + // This is the start of the World=0 first instance. + ReplicaBodiesStart int32 `edit:"-"` + // ReplicaBodiesN is the number of body elements within each set of - // replicated world bodies, which is needed for viewers to efficiently select - // a specific world to view. + // replicated world bodies, which is needed to efficiently select + // a body from a specific world. ReplicaBodiesN int32 `edit:"-"` + // ReplicaJointsStart is the starting joint index for replicated world joints, + // which is needed to efficiently select a joint from a specific world. + // This is the start of the World=0 first instance. + ReplicaJointsStart int32 `edit:"-"` + + // ReplicaJointsN is the number of joint elements within each set of + // replicated world joints, which is needed to efficiently select + // a joint from a specific world. + ReplicaJointsN int32 `edit:"-"` + // Bodies are the rigid body elements (dynamic and static), // specifying the constant, non-dynamic properties, // which is initial state for dynamics. @@ -248,17 +258,25 @@ func (ml *Model) ToGPUInfra() { ToGPU(ParamsVar, BodiesVar, ObjectsVar, JointsVar, JointDoFsVar, BodyJointsVar, BodyCollidePairsVar, DynamicsVar, BroadContactsNVar, BroadContactsVar, ContactsNVar, ContactsVar, JointControlsVar) } -// ReplicasIndexes returns the body and dynamics (if dynamic) indexes +// ReplicasBodyIndexes returns the body and dynamics (if dynamic) indexes // for given replica world and source body index, if ReplicasN is > 0. // Otherwise, returns bi and corresponding dynamic index. -func (ml *Model) ReplicasIndexes(bi, replica int32) (bodyIdx, dynIdx int32) { - if ml.ReplicasN == 0 { +func (ml *Model) ReplicasBodyIndexes(bi, replica int32) (bodyIdx, dynIdx int32) { + if ml.ReplicasN == 0 || bi < ml.ReplicaBodiesStart { return bi, GetBodyDynamic(bi) } - if bi < ml.ReplicasStart { - return bi, GetBodyDynamic(bi) - } - bodyIdx = (bi - ml.ReplicasStart) + ml.ReplicasStart + replica*ml.ReplicaBodiesN + bodyIdx = (bi - ml.ReplicaBodiesStart) + ml.ReplicaBodiesStart + replica*ml.ReplicaBodiesN dynIdx = GetBodyDynamic(bodyIdx) return } + +// ReplicasJointIndex returns the joint indexe for given replica +// world and source body index, if ReplicasN is > 0. +// Otherwise, returns bi and corresponding dynamic index. +func (ml *Model) ReplicasJointIndex(ji, replica int32) int32 { + if ml.ReplicasN == 0 || ji < ml.ReplicaJointsStart { + return ji + } + nji := (ji - ml.ReplicaJointsStart) + ml.ReplicaJointsStart + replica*ml.ReplicaJointsN + return nji +} diff --git a/physics/phyxyz/skin.go b/physics/phyxyz/skin.go index 28923ab0..b42b1cf0 100644 --- a/physics/phyxyz/skin.go +++ b/physics/phyxyz/skin.go @@ -78,7 +78,7 @@ func (sk *Skin) UpdateFromPhysics(sc *Scene) { di := int32(sk.DynamicIndex) bi := int32(sk.BodyIndex) if sc.ReplicasView { - bi, di = physics.CurModel.ReplicasIndexes(bi, int32(sc.ReplicasIndex)) + bi, di = physics.CurModel.ReplicasBodyIndexes(bi, int32(sc.ReplicasIndex)) } if di >= 0 { sk.Pos = physics.DynamicPos(di, params.Cur) diff --git a/physics/phyxyz/typegen.go b/physics/phyxyz/typegen.go index 8e8ceb66..edfc779b 100644 --- a/physics/phyxyz/typegen.go +++ b/physics/phyxyz/typegen.go @@ -12,11 +12,12 @@ import ( var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics/phyxyz.Camera", IDName: "camera", Doc: "Camera defines the properties of a camera needed for rendering from a node.", Fields: []types.Field{{Name: "Size", Doc: "size of image to record"}, {Name: "FOV", Doc: "field of view in degrees"}, {Name: "Near", Doc: "near plane z coordinate"}, {Name: "Far", Doc: "far plane z coordinate"}, {Name: "MaxD", Doc: "maximum distance for depth maps. Anything above is 1.\nThis is independent of Near / Far rendering (though must be < Far)\nand is for normalized depth maps."}, {Name: "LogD", Doc: "use the natural log of 1 + depth for normalized depth values in display etc."}, {Name: "MSample", Doc: "number of multi-samples to use for antialising -- 4 is best and default."}, {Name: "UpDir", Doc: "up direction for camera. Defaults to positive Y axis,\nand is reset by call to LookAt method."}}}) -var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics/phyxyz.Editor", IDName: "editor", Doc: "Editor provides a basic viewer and parameter controller widget\nfor exploring physics models.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Embeds: []types.Field{{Name: "Frame"}}, Fields: []types.Field{{Name: "Model", Doc: "Model has the physics simulation."}, {Name: "Scene", Doc: "Scene has the 3D GUI visualization."}, {Name: "UserParams", Doc: "UserParams is a struct with parameters for configuring the physics sim.\nThese are displayed in the editor."}, {Name: "ConfigFunc", Doc: "ConfigFunc is the function that configures the [physics.Model]."}, {Name: "ControlFunc", Doc: "ControlFunc is the function that sets control parameters,\nbased on the current timestep (in milliseconds, converted from physics time)."}, {Name: "CameraPos", Doc: "CameraPos provides the default initial camera position, looking at the origin.\nSet this to larger numbers to zoom out, and smaller numbers to zoom in.\nDefaults to math32.Vec3(0, 25, 20)."}, {Name: "isRunning", Doc: "IsRunning is true if currently running sim."}, {Name: "stop", Doc: "Stop triggers topping of running."}, {Name: "TimeStep", Doc: "TimeStep is current time step in physics update cycles."}, {Name: "editor", Doc: "editor is the xyz GUI visualization widget."}, {Name: "toolbar", Doc: "Toolbar is the top toolbar."}, {Name: "splits", Doc: "Splits is the container for elements."}, {Name: "userParamsForm", Doc: "UserParamsForm has the user's config parameters."}, {Name: "paramsForm", Doc: "ParamsForm has the Physics parameters."}}}) +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics/phyxyz.Editor", IDName: "editor", Doc: "Editor provides a basic viewer and parameter controller widget\nfor exploring physics models. It creates and manages its own\n[physics.Model] and [phyxyz.Scene].", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Embeds: []types.Field{{Name: "Frame"}}, Fields: []types.Field{{Name: "Model", Doc: "Model has the physics simulation."}, {Name: "Scene", Doc: "Scene has the 3D GUI visualization."}, {Name: "UserParams", Doc: "UserParams is a struct with parameters for configuring the physics sim.\nThese are displayed in the editor."}, {Name: "ConfigFunc", Doc: "ConfigFunc is the function that configures the [physics.Model]."}, {Name: "ControlFunc", Doc: "ControlFunc is the function that sets control parameters,\nbased on the current timestep (in milliseconds, converted from physics time)."}, {Name: "CameraPos", Doc: "CameraPos provides the default initial camera position, looking at the origin.\nSet this to larger numbers to zoom out, and smaller numbers to zoom in.\nDefaults to math32.Vec3(0, 25, 20)."}, {Name: "Replica", Doc: "Replica is the replica world to view, if replicas are present in model."}, {Name: "isRunning", Doc: "IsRunning is true if currently running sim."}, {Name: "stop", Doc: "Stop triggers topping of running."}, {Name: "TimeStep", Doc: "TimeStep is current time step in physics update cycles."}, {Name: "editor", Doc: "editor is the xyz GUI visualization widget."}, {Name: "toolbar", Doc: "Toolbar is the top toolbar."}, {Name: "splits", Doc: "Splits is the container for elements."}, {Name: "userParamsForm", Doc: "UserParamsForm has the user's config parameters."}, {Name: "paramsForm", Doc: "ParamsForm has the Physics parameters."}}}) // NewEditor returns a new [Editor] with the given optional parent: // Editor provides a basic viewer and parameter controller widget -// for exploring physics models. +// for exploring physics models. It creates and manages its own +// [physics.Model] and [phyxyz.Scene]. func NewEditor(parent ...tree.Node) *Editor { return tree.New[Editor](parent...) } // SetModel sets the [Editor.Model]: @@ -47,11 +48,15 @@ func (t *Editor) SetControlFunc(v func(timeStep int)) *Editor { t.ControlFunc = // Defaults to math32.Vec3(0, 25, 20). func (t *Editor) SetCameraPos(v math32.Vector3) *Editor { t.CameraPos = v; return t } +// SetReplica sets the [Editor.Replica]: +// Replica is the replica world to view, if replicas are present in model. +func (t *Editor) SetReplica(v int) *Editor { t.Replica = v; return t } + // SetTimeStep sets the [Editor.TimeStep]: // TimeStep is current time step in physics update cycles. func (t *Editor) SetTimeStep(v int) *Editor { t.TimeStep = v; return t } -var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics/phyxyz.Scene", IDName: "scene", Doc: "Scene displays a [physics.Model] using a [xyz.Scene].\nOne Scene can be used for multiple different [physics.Model]s which\nis more efficient when running multiple in parallel.\nInitial construction of the physics and visualization happens here.", Fields: []types.Field{{Name: "Scene", Doc: "Scene is the [xyz.Scene] object for visualizing."}, {Name: "Root", Doc: "Root is the root Group node in the Scene under which the world is rendered."}, {Name: "Skins", Doc: "Skins are the view elements for each body in [physics.Model]."}}}) +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics/phyxyz.Scene", IDName: "scene", Doc: "Scene displays a [physics.Model] using a [xyz.Scene].\nOne Scene can be used for multiple different [physics.Model]s which\nis more efficient when running multiple in parallel.\nInitial construction of the physics and visualization happens here.", Fields: []types.Field{{Name: "Scene", Doc: "Scene is the [xyz.Scene] object for visualizing."}, {Name: "Root", Doc: "Root is the root Group node in the Scene under which the world is rendered."}, {Name: "Skins", Doc: "Skins are the view elements for each body in [physics.Model]."}, {Name: "ReplicasView", Doc: "ReplicasView enables viewing of different replicated worlds\nusing the same skins."}, {Name: "ReplicasIndex", Doc: "ReplicasIndex is the replicated world to view."}}}) var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics/phyxyz.Skin", IDName: "skin", Doc: "Skin has visualization functions for physics elements.", Directives: []types.Directive{{Tool: "types", Directive: "add", Args: []string{"-setters"}}}, Fields: []types.Field{{Name: "Name", Doc: "Name is a name for element (index always appended, so it is unique)."}, {Name: "Shape", Doc: "Shape is the physical shape of the element."}, {Name: "Color", Doc: "Color is the color of the element."}, {Name: "HSize", Doc: "HSize is the half-size (e.g., radius) of the body.\nValues depend on shape type: X is generally radius,\nY is half-height."}, {Name: "Pos", Doc: "Pos is the position."}, {Name: "Quat", Doc: "Quat is the rotation as a quaternion."}, {Name: "NewSkin", Doc: "NewSkin is a function that returns a new [xyz.Node]\nto represent this element. If nil, uses appropriate defaults."}, {Name: "InitSkin", Doc: "InitSkin is a function that initializes a new [xyz.Node]\nthat represents this element. If nil, uses appropriate defaults."}, {Name: "BodyIndex", Doc: "BodyIndex is the index of the body in [physics.Bodies]"}, {Name: "DynamicIndex", Doc: "DynamicIndex is the index in [physics.Dynamics] (-1 if not dynamic)."}}}) diff --git a/physics/typegen.go b/physics/typegen.go index a548c7ff..bfda31ba 100644 --- a/physics/typegen.go +++ b/physics/typegen.go @@ -22,7 +22,7 @@ var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.JointVars", var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.JointDoFVars", IDName: "joint-do-f-vars", Doc: "JointDoFVars are joint DoF state variables stored in tensor.Float32,\none for each DoF."}) -var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.Model", IDName: "model", Doc: "Model contains and manages all of the physics elements.", Directives: []types.Directive{{Tool: "go", Directive: "generate", Args: []string{"core", "generate", "-add-types", "-gosl"}}}, Fields: []types.Field{{Name: "GPU", Doc: "GPU determines whether to use GPU (else CPU)."}, {Name: "Params", Doc: "Params are global parameters."}, {Name: "GetContacts", Doc: "GetContacts will download Contacts from the GPU, if processing them on the CPU."}, {Name: "ReportTotalKE", Doc: "ReportTotalKE prints out the total computed kinetic energy in the system after\nevery step."}, {Name: "CurrentWorld", Doc: "CurrentWorld is the [BodyWorld] value to use when creating new bodies.\nSet to -1 to create global elements that interact with everything,\nwhile 0 and positive numbers only interact amongst themselves."}, {Name: "CurrentObject", Doc: "CurrentObject is the Object to use when creating new joints.\nCall NewObject to increment."}, {Name: "CurrentObjectJoint", Doc: "CurrentObjectJoint is the Joint index in CurrentObject\nto use when creating new joints."}, {Name: "ReplicasStart", Doc: "ReplicasStart is the starting body index for replicated world bodies,\nwhich is needed for viewers to efficiently select a specific world to view.\nThis is the start of the World=0 first instance."}, {Name: "ReplicasN", Doc: "ReplicasN is the number of replicated worlds.\nTotal bodies from ReplicasStart should be ReplicasN * ReplicaBodiesN."}, {Name: "ReplicaBodiesN", Doc: "ReplicaBodiesN is the number of body elements within each set of\nreplicated world bodies, which is needed for viewers to efficiently select\na specific world to view."}, {Name: "Bodies", Doc: "Bodies are the rigid body elements (dynamic and static),\nspecifying the constant, non-dynamic properties,\nwhich is initial state for dynamics.\n[body][BodyVarsN]"}, {Name: "Objects", Doc: "Objects is a list of joint indexes for each object, where each object\ncontains all the joints interconnecting an overlapping set of bodies.\nThis is known as an articulation in other physics software.\nJoints must be added in parent -> child order within objects, as joints\nare updated in sequential order within object.\n[object][MaxObjectJoints+1]"}, {Name: "BodyJoints", Doc: "BodyJoints is a list of joint indexes for each dynamic body, for aggregating.\n[dyn body][parent, child][Params.BodyJointsMax]"}, {Name: "Joints", Doc: "Joints is a list of permanent joints connecting bodies,\nwhich do not change (no dynamic variables).\n[joint][JointVarsN]"}, {Name: "JointDoFs", Doc: "JointDoFs is a list of joint DoF parameters, allocated per joint.\n[dof][JointDoFVars]"}, {Name: "BodyCollidePairs", Doc: "BodyCollidePairs are pairs of Body indexes that could potentially collide\nbased on precomputed collision logic, using World, Group, and Joint indexes.\n[BodyCollidePairsN][2]"}, {Name: "Dynamics", Doc: "Dynamics are the dynamic rigid body elements: these actually move.\nThe first set of variables are for initial values, and the second current.\n[body][cur/next][DynamicVarsN]"}, {Name: "BroadContactsN", Doc: "BroadContactsN has number of points of broad contact\nbetween bodies. [1]"}, {Name: "BroadContacts", Doc: "BroadContacts are the results of broad-phase contact processing,\nestablishing possible points of contact between bodies.\n[ContactsMax][BroadContactVarsN]"}, {Name: "ContactsN", Doc: "ContactsN has number of points of narrow (final) contact\nbetween bodies. [1]"}, {Name: "Contacts", Doc: "Contacts are the results of narrow-phase contact processing,\nwhere only actual contacts with fully-specified values are present.\n[ContactsMax][ContactVarsN]"}, {Name: "JointControls", Doc: "JointControls are dynamic joint control inputs, per joint DoF\n(in correspondence with [JointDoFs]). This can be uploaded to the\nGPU at every step.\n[dof][JointControlVarsN]"}}}) +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.Model", IDName: "model", Doc: "Model contains and manages all of the physics elements.", Directives: []types.Directive{{Tool: "go", Directive: "generate", Args: []string{"core", "generate", "-add-types", "-gosl"}}}, Fields: []types.Field{{Name: "GPU", Doc: "GPU determines whether to use GPU (else CPU)."}, {Name: "Params", Doc: "Params are global parameters."}, {Name: "GetContacts", Doc: "GetContacts will download Contacts from the GPU, if processing them on the CPU."}, {Name: "ReportTotalKE", Doc: "ReportTotalKE prints out the total computed kinetic energy in the system after\nevery step."}, {Name: "CurrentWorld", Doc: "CurrentWorld is the [BodyWorld] value to use when creating new bodies.\nSet to -1 to create global elements that interact with everything,\nwhile 0 and positive numbers only interact amongst themselves."}, {Name: "CurrentObject", Doc: "CurrentObject is the Object to use when creating new joints.\nCall NewObject to increment."}, {Name: "CurrentObjectJoint", Doc: "CurrentObjectJoint is the Joint index in CurrentObject\nto use when creating new joints."}, {Name: "ReplicasN", Doc: "ReplicasN is the number of replicated worlds.\nTotal bodies from ReplicasStart should be ReplicasN * ReplicaBodiesN."}, {Name: "ReplicaBodiesStart", Doc: "ReplicaBodiesStart is the starting body index for replicated world bodies,\nwhich is needed to efficiently select a body from a specific world.\nThis is the start of the World=0 first instance."}, {Name: "ReplicaBodiesN", Doc: "ReplicaBodiesN is the number of body elements within each set of\nreplicated world bodies, which is needed to efficiently select\na body from a specific world."}, {Name: "ReplicaJointsStart", Doc: "ReplicaJointsStart is the starting joint index for replicated world joints,\nwhich is needed to efficiently select a joint from a specific world.\nThis is the start of the World=0 first instance."}, {Name: "ReplicaJointsN", Doc: "ReplicaJointsN is the number of joint elements within each set of\nreplicated world joints, which is needed to efficiently select\na joint from a specific world."}, {Name: "Bodies", Doc: "Bodies are the rigid body elements (dynamic and static),\nspecifying the constant, non-dynamic properties,\nwhich is initial state for dynamics.\n[body][BodyVarsN]"}, {Name: "Objects", Doc: "Objects is a list of joint indexes for each object, where each object\ncontains all the joints interconnecting an overlapping set of bodies.\nThis is known as an articulation in other physics software.\nJoints must be added in parent -> child order within objects, as joints\nare updated in sequential order within object.\n[object][MaxObjectJoints+1]"}, {Name: "BodyJoints", Doc: "BodyJoints is a list of joint indexes for each dynamic body, for aggregating.\n[dyn body][parent, child][Params.BodyJointsMax]"}, {Name: "Joints", Doc: "Joints is a list of permanent joints connecting bodies,\nwhich do not change (no dynamic variables).\n[joint][JointVarsN]"}, {Name: "JointDoFs", Doc: "JointDoFs is a list of joint DoF parameters, allocated per joint.\n[dof][JointDoFVars]"}, {Name: "BodyCollidePairs", Doc: "BodyCollidePairs are pairs of Body indexes that could potentially collide\nbased on precomputed collision logic, using World, Group, and Joint indexes.\n[BodyCollidePairsN][2]"}, {Name: "Dynamics", Doc: "Dynamics are the dynamic rigid body elements: these actually move.\nThe first set of variables are for initial values, and the second current.\n[body][cur/next][DynamicVarsN]"}, {Name: "BroadContactsN", Doc: "BroadContactsN has number of points of broad contact\nbetween bodies. [1]"}, {Name: "BroadContacts", Doc: "BroadContacts are the results of broad-phase contact processing,\nestablishing possible points of contact between bodies.\n[ContactsMax][BroadContactVarsN]"}, {Name: "ContactsN", Doc: "ContactsN has number of points of narrow (final) contact\nbetween bodies. [1]"}, {Name: "Contacts", Doc: "Contacts are the results of narrow-phase contact processing,\nwhere only actual contacts with fully-specified values are present.\n[ContactsMax][ContactVarsN]"}, {Name: "JointControls", Doc: "JointControls are dynamic joint control inputs, per joint DoF\n(in correspondence with [JointDoFs]). This can be uploaded to the\nGPU at every step.\n[dof][JointControlVarsN]"}}}) var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics.PhysicsParams", IDName: "physics-params", Doc: "PhysicsParams are the physics parameters", Directives: []types.Directive{{Tool: "gosl", Directive: "start"}}, Fields: []types.Field{{Name: "Iterations", Doc: "Iterations is the number of integration iterations to perform\nwithin each solver step. Muller et al (2020) report that 1 is best."}, {Name: "Dt", Doc: "Dt is the integration stepsize.\nFor highly kinetic situations (e.g., rapidly moving bouncing balls)\n0.0001 is needed to ensure contact registration. Use SubSteps to\naccomplish a target effective read-out step size."}, {Name: "SubSteps", Doc: "SubSteps is the number of integration steps to take per Step()\nfunction call. These sub steps are taken without any sync to/from\nthe GPU and are therefore much faster."}, {Name: "ControlDt", Doc: "ControlDt is the stepsize for integrating joint control position values\n[JointTargetPos] over time, to avoid sudden strong changes in force.\nFor higher-DoF joints (e.g., Ball), this can be important for stability,\nbut it can also result in under-shoot of the target position."}, {Name: "ControlDtThr", Doc: "ControlDtThr is the threshold on the control delta above which\nControlDt is used. ControlDt is most important for large changes,\nand can result in under-shoot if engaged for small changes."}, {Name: "ContactMargin", Doc: "Contact margin is the extra distance for broadphase collision\naround rigid bodies. This can make some joints potentially unstable if > 0"}, {Name: "ContactRelax", Doc: "ContactRelax is rigid contact relaxation constant.\nHigher values cause errros"}, {Name: "ContactWeighting", Doc: "Contact weighting: balances contact forces?"}, {Name: "Restitution", Doc: "Restitution takes into account bounciness of objects."}, {Name: "JointLinearRelax", Doc: "JointLinearRelax is joint linear relaxation constant."}, {Name: "JointAngularRelax", Doc: "JointAngularRelax is joint angular relaxation constant."}, {Name: "JointLinearComply", Doc: "JointLinearComply is joint linear compliance constant."}, {Name: "JointAngularComply", Doc: "JointAngularComply is joint angular compliance constant."}, {Name: "AngularDamping", Doc: "AngularDamping is damping of angular motion."}, {Name: "SoftRelax", Doc: "SoftRelax is soft-body relaxation constant."}, {Name: "MaxForce", Doc: "MaxForce is the maximum computed force value, which prevents\nrunaway numerical overflow."}, {Name: "MaxDelta", Doc: "MaxDelta is the maximum computed change in position magnitude,\nwhich prevents runaway numerical overflow."}, {Name: "MaxGeomIter", Doc: "MaxGeomIter is number of iterations to perform in shape-based\ngeometry collision computations"}, {Name: "ContactsMax", Doc: "Maximum number of contacts to process at any given point."}, {Name: "Cur", Doc: "Index for the current state (0 or 1, alternates with Next)."}, {Name: "Next", Doc: "Index for the next state (1 or 0, alternates with Cur)."}, {Name: "BodiesN", Doc: "BodiesN is number of rigid bodies."}, {Name: "DynamicsN", Doc: "DynamicsN is number of dynamics bodies."}, {Name: "ObjectsN", Doc: "ObjectsN is number of objects."}, {Name: "MaxObjectJoints", Doc: "MaxObjectJoints is max number of joints per object."}, {Name: "JointsN", Doc: "JointsN is number of joints."}, {Name: "JointDoFsN", Doc: "JointDoFsN is number of joint DoFs."}, {Name: "BodyJointsMax", Doc: "BodyJointsMax is max number of joints per body + 1 for actual n."}, {Name: "BodyCollidePairsN", Doc: "BodyCollidePairsN is the total number of pre-compiled collision pairs\nto examine."}, {Name: "pad"}, {Name: "pad1"}, {Name: "pad2"}, {Name: "Gravity", Doc: "Gravity is the gravity acceleration function"}}}) From 4f12ee287b546a006c44b827182f76913d74001d Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Sun, 11 Jan 2026 14:58:09 -0800 Subject: [PATCH 90/97] physics: docs update --- docs/content/physics.md | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/docs/content/physics.md b/docs/content/physics.md index c469abc3..91fd9009 100644 --- a/docs/content/physics.md +++ b/docs/content/physics.md @@ -254,7 +254,7 @@ To optimize the collision detection computation, it is important to organize bod There is also a special constraint where the parent and child on a same joint do not collide, as this often happens and would lead to weird behavior. -There is also an `Object` index for each body, that is used for external manipulation and control purposes, but does not affect collision or physics. +The `NewBody` and `NewDynamic` methods automatically use the `Model.CurrentWorld` index by default, or you can directly use `SetBodyWorld` to assign a specific world index. ## Shapes @@ -302,8 +302,6 @@ A major advantage of using `builder` is to take advantage of the `ReplicateWorld The compute efficiency of the GPU goes up with the more elements that are processed in parallel, amortizing the memory transfer overhead and leveraging the parallel cores. Furthermore, in AI applications for example, models can be trained in parallel on different instances of the same environment, with each instance having its own random initial starting point and trajectory over time. All of these instances can be simulated in one `physics.Model` by using the `World` index on the bodies, with the shared static environment living in World -1, and the elements of each instance (e.g., a simulated robot) living in its own separate world. -The `NewBody` and `NewDynamic` methods automatically use the `Model.CurrentWorld` index by default, or you can directly use `SetBodyWorld` to assign a specific world index. - The [[doc:physics/builder.Builder.ReplicateWorld]] method creates N replicas of an existing world, including all associated joints. This can only be called once, as it records the start and N-per-world of each such replicated world, which allows the `phyxyz` `Editor` to efficiently view a specific world. Thus, under this scenario, you create world 0 and then replicate it, then modify the initial positions and orientations accordingly, using the Object-based methods. ## Sensors From cf0d61904a54d72651cabf2a566f438452907c47 Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Tue, 13 Jan 2026 23:39:06 -0800 Subject: [PATCH 91/97] physics: Replicas indexes robust to calling with higher indexes --- physics/examples/virtroom/virtroom.go | 2 +- physics/model.go | 14 ++++++++++---- physics/model.goal | 14 ++++++++++---- 3 files changed, 21 insertions(+), 9 deletions(-) diff --git a/physics/examples/virtroom/virtroom.go b/physics/examples/virtroom/virtroom.go index a95c7a33..98dd0bad 100644 --- a/physics/examples/virtroom/virtroom.go +++ b/physics/examples/virtroom/virtroom.go @@ -162,7 +162,7 @@ func (ev *Env) MakeModel(sc *xyz.Scene) { // vw.Physics.Builder.ReplicateWorld(vw.Physics.Scene, 1, 1, 8) // 1x8 ev.Physics.Build() // params := physics.GetParams(0) - // params.ControlDt = 1 + // params.ControlDt = 0.1 // params.Gravity.Y = 0 // params.MaxForce = 1.0e3 // params.AngularDamping = 0.5 diff --git a/physics/model.go b/physics/model.go index d4d7bccf..04a7a6d0 100644 --- a/physics/model.go +++ b/physics/model.go @@ -264,10 +264,13 @@ func (ml *Model) ToGPUInfra() { // for given replica world and source body index, if ReplicasN is > 0. // Otherwise, returns bi and corresponding dynamic index. func (ml *Model) ReplicasBodyIndexes(bi, replica int32) (bodyIdx, dynIdx int32) { - if ml.ReplicasN == 0 || bi < ml.ReplicaBodiesStart { + start := ml.ReplicaBodiesStart + n := ml.ReplicaBodiesN + if ml.ReplicasN == 0 || bi < start { return bi, GetBodyDynamic(bi) } - bodyIdx = (bi - ml.ReplicaBodiesStart) + ml.ReplicaBodiesStart + replica*ml.ReplicaBodiesN + rbi := (bi - start) % n + bodyIdx = start + rbi + replica*n dynIdx = GetBodyDynamic(bodyIdx) return } @@ -276,9 +279,12 @@ func (ml *Model) ReplicasBodyIndexes(bi, replica int32) (bodyIdx, dynIdx int32) // world and source body index, if ReplicasN is > 0. // Otherwise, returns bi and corresponding dynamic index. func (ml *Model) ReplicasJointIndex(ji, replica int32) int32 { - if ml.ReplicasN == 0 || ji < ml.ReplicaJointsStart { + start := ml.ReplicaJointsStart + n := ml.ReplicaJointsN + if ml.ReplicasN == 0 || ji < start { return ji } - nji := (ji - ml.ReplicaJointsStart) + ml.ReplicaJointsStart + replica*ml.ReplicaJointsN + rji := (ji - start) % n + nji := start + rji + replica*n return nji } diff --git a/physics/model.goal b/physics/model.goal index 065154a5..f460e9b3 100644 --- a/physics/model.goal +++ b/physics/model.goal @@ -262,10 +262,13 @@ func (ml *Model) ToGPUInfra() { // for given replica world and source body index, if ReplicasN is > 0. // Otherwise, returns bi and corresponding dynamic index. func (ml *Model) ReplicasBodyIndexes(bi, replica int32) (bodyIdx, dynIdx int32) { - if ml.ReplicasN == 0 || bi < ml.ReplicaBodiesStart { + start := ml.ReplicaBodiesStart + n := ml.ReplicaBodiesN + if ml.ReplicasN == 0 || bi < start { return bi, GetBodyDynamic(bi) } - bodyIdx = (bi - ml.ReplicaBodiesStart) + ml.ReplicaBodiesStart + replica*ml.ReplicaBodiesN + rbi := (bi - start) % n + bodyIdx = start + rbi + replica*n dynIdx = GetBodyDynamic(bodyIdx) return } @@ -274,9 +277,12 @@ func (ml *Model) ReplicasBodyIndexes(bi, replica int32) (bodyIdx, dynIdx int32) // world and source body index, if ReplicasN is > 0. // Otherwise, returns bi and corresponding dynamic index. func (ml *Model) ReplicasJointIndex(ji, replica int32) int32 { - if ml.ReplicasN == 0 || ji < ml.ReplicaJointsStart { + start := ml.ReplicaJointsStart + n := ml.ReplicaJointsN + if ml.ReplicasN == 0 || ji < start { return ji } - nji := (ji - ml.ReplicaJointsStart) + ml.ReplicaJointsStart + replica*ml.ReplicaJointsN + rji := (ji - start) % n + nji := start + rji + replica*n return nji } From 7a11751d0716add557610d202c1b231a6bde969f Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Wed, 14 Jan 2026 13:14:09 -0800 Subject: [PATCH 92/97] physics: several updates to fix RenderFrom and having a completely separate xyz.Scene view of a physics world. --- physics/builder/builder.go | 15 +++++++++++++++ physics/builder/physics.go | 4 +++- physics/phyxyz/editor.go | 2 +- physics/phyxyz/scene.go | 11 ++++++++++- physics/phyxyz/skin.go | 1 - 5 files changed, 29 insertions(+), 4 deletions(-) diff --git a/physics/builder/builder.go b/physics/builder/builder.go index 2795a9d8..1211df68 100644 --- a/physics/builder/builder.go +++ b/physics/builder/builder.go @@ -160,6 +160,21 @@ func (bl *Builder) ReplicateWorld(sc *phyxyz.Scene, worldIdx, nY, nX int, offs . bl.ReplicasN = nY * nX } +// CloneSkins copies existing Body skins into the given [phyxyz.Scene], +// thereby configuring the given scene to view the physics model for this builder. +func (bl *Builder) CloneSkins(sc *phyxyz.Scene) { + for _, wl := range bl.Worlds { + for _, ob := range wl.Objects { + for _, bd := range ob.Bodies { + if bd.Skin == nil { + continue + } + sc.AddSkinClone(bd.Skin) + } + } + } +} + // ReplicaWorld returns the replica World at given replica index, // Where replica is index into replicated worlds (0 = original). func (bl *Builder) ReplicaWorld(replica int) *World { diff --git a/physics/builder/physics.go b/physics/builder/physics.go index b7ca3f92..be417527 100644 --- a/physics/builder/physics.go +++ b/physics/builder/physics.go @@ -49,7 +49,9 @@ func (ph *Physics) InitState() { func (ph *Physics) Step(n int) { for range n { ph.Model.Step() - ph.Scene.Update() + if ph.Scene != nil { + ph.Scene.Update() + } } } diff --git a/physics/phyxyz/editor.go b/physics/phyxyz/editor.go index 1ee1ea04..3b4eb6ac 100644 --- a/physics/phyxyz/editor.go +++ b/physics/phyxyz/editor.go @@ -280,7 +280,7 @@ func (pe *Editor) MakeToolbar(p *tree.Plan) { pe.Scene.ReplicasView = replN > 0 } w.SetMax(float32(replN - 1)) - s.SetEnabled(replN > 0) + s.SetEnabled(replN > 1) }) w.OnChange(func(e events.Event) { pe.Scene.ReplicasIndex = pe.Replica diff --git a/physics/phyxyz/scene.go b/physics/phyxyz/scene.go index 1095b1aa..b7446672 100644 --- a/physics/phyxyz/scene.go +++ b/physics/phyxyz/scene.go @@ -127,7 +127,9 @@ func (sc *Scene) RenderFrom(sk *Skin, cam *Camera) []image.Image { imgs = make([]image.Image, ml.ReplicasN) for i := range ml.ReplicasN { sc.ReplicasIndex = int(i) - sc.UpdateFromPhysics() + sc.Update() // full Update needed, beyond just UpdateFromPhysics. + xysc.Camera.Pose.Pos = sk.Pos + xysc.Camera.Pose.Quat = sk.Quat img := xysc.RenderGrabImage() imgs[i] = img } @@ -153,3 +155,10 @@ func (sc *Scene) NewSkin(shape physics.Shapes, name, clr string, hsize math32.Ve sc.Skins = append(sc.Skins, sk) return sk } + +// AddSkinClone adds a cloned version of given skin. +func (sc *Scene) AddSkinClone(sk *Skin) { + nsk := &Skin{} + *nsk = *sk + sc.Skins = append(sc.Skins, sk) +} diff --git a/physics/phyxyz/skin.go b/physics/phyxyz/skin.go index b42b1cf0..18ef7655 100644 --- a/physics/phyxyz/skin.go +++ b/physics/phyxyz/skin.go @@ -87,7 +87,6 @@ func (sk *Skin) UpdateFromPhysics(sc *Scene) { sk.Pos = physics.BodyPos(bi) sk.Quat = physics.BodyQuat(bi) } - // fmt.Println("skin:", sk.BodyIndex, sk.DynamicIndex, sk.Name, sk.Pos, sk.Quat) } // UpdatePose updates the xyz node pose from skin. From d1cc0f0f7776237acd0c9e6ab2529691a382984b Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Fri, 16 Jan 2026 10:44:39 -0800 Subject: [PATCH 93/97] physics: update to go1.25 and fix random numbers in test, minor fix in updating gui in physics editor --- go.mod | 71 +++++++-------- go.sum | 171 +++++++++++++++++------------------- physics/phyxyz/editor.go | 5 ++ plot/plots/table_test.go | 8 +- stats/cluster/clust_test.go | 4 +- 5 files changed, 123 insertions(+), 136 deletions(-) diff --git a/go.mod b/go.mod index b608b869..95123049 100644 --- a/go.mod +++ b/go.mod @@ -1,56 +1,52 @@ module cogentcore.org/lab -go 1.23.4 +go 1.25.6 // after go mod tidy, you need to run: -// go get google.golang.org/genproto@v0.0.0-20250804133106-a7a43d27e69b -// when updating go version: // go get google.golang.org/genproto@latest // otherwise there will be an ambiguous import warning when building baremetal // https://github.com/googleapis/go-genproto/issues/1015 require ( - cogentcore.org/core v0.3.13 - github.com/alecthomas/assert/v2 v2.6.0 + cogentcore.org/core v0.3.14-0.20260116180322-746be48a586f github.com/cogentcore/readline v0.1.3 - github.com/cogentcore/yaegi v0.0.0-20250622201820-b7838bdd95eb + github.com/cogentcore/yaegi v0.0.0-20260116172027-700fbf8949f3 github.com/mitchellh/go-homedir v1.1.0 github.com/nsf/termbox-go v1.1.1 - github.com/stretchr/testify v1.10.0 - golang.org/x/exp v0.0.0-20231110203233-9a3e6036ecaa - golang.org/x/tools v0.34.0 - gonum.org/v1/gonum v0.15.0 - google.golang.org/grpc v1.74.2 - google.golang.org/protobuf v1.36.6 + github.com/stretchr/testify v1.11.1 + golang.org/x/exp v0.0.0-20260112195511-716be5621a96 + golang.org/x/tools v0.41.0 + gonum.org/v1/gonum v0.17.0 + google.golang.org/grpc v1.76.0 + google.golang.org/protobuf v1.36.11 ) require ( + codeberg.org/go-pdf/fpdf v0.11.0 // indirect github.com/Bios-Marcel/wastebasket/v2 v2.0.3 // indirect github.com/Masterminds/vcs v1.13.3 // indirect github.com/adrg/strutil v0.3.1 // indirect - github.com/alecthomas/chroma/v2 v2.13.0 // indirect - github.com/alecthomas/repr v0.4.0 // indirect - github.com/anthonynsimon/bild v0.13.0 // indirect + github.com/alecthomas/chroma/v2 v2.22.0 // indirect + github.com/anthonynsimon/bild v0.14.0 // indirect github.com/aymanbagabas/go-osc52/v2 v2.0.1 // indirect github.com/aymerick/douceur v0.2.0 // indirect - github.com/bramvdbogaerde/go-scp v1.4.0 // indirect - github.com/chewxy/math32 v1.10.1 // indirect + github.com/bramvdbogaerde/go-scp v1.6.0 // indirect + github.com/chewxy/math32 v1.11.1 // indirect github.com/cogentcore/webgpu v0.23.0 // indirect github.com/davecgh/go-spew v1.1.2-0.20180830191138-d8f796af33cc // indirect - github.com/dlclark/regexp2 v1.11.0 // indirect - github.com/ericchiang/css v1.3.0 // indirect - github.com/fsnotify/fsnotify v1.8.0 // indirect + github.com/dlclark/regexp2 v1.11.5 // indirect + github.com/ericchiang/css v1.4.0 // indirect + github.com/fsnotify/fsnotify v1.9.0 // indirect github.com/go-fonts/latin-modern v0.3.3 // indirect - github.com/go-gl/glfw/v3.3/glfw v0.0.0-20240506104042-037f3cc74f2a // indirect - github.com/go-text/typesetting v0.3.1-0.20250402122313-7a0f05577ff5 // indirect + github.com/go-gl/glfw/v3.3/glfw v0.0.0-20250301202403-da16c1255728 // indirect + github.com/go-text/typesetting v0.3.2 // indirect github.com/gobwas/glob v0.2.3 // indirect - github.com/gomarkdown/markdown v0.0.0-20250311123330-531bef5e742b // indirect + github.com/gomarkdown/markdown v0.0.0-20250810172220-2e2c11897d1a // indirect github.com/gorilla/css v1.0.1 // indirect github.com/h2non/filetype v1.1.3 // indirect github.com/hack-pad/go-indexeddb v0.3.2 // indirect - github.com/hack-pad/hackpadfs v0.2.1 // indirect + github.com/hack-pad/hackpadfs v0.2.4 // indirect github.com/hack-pad/safejs v0.1.1 // indirect - github.com/hexops/gotextdiff v1.0.3 // indirect github.com/jeandeaual/go-locale v0.0.0-20250612000132-0ef82f21eade // indirect github.com/jinzhu/copier v0.4.0 // indirect github.com/kr/text v0.2.0 // indirect @@ -59,23 +55,22 @@ require ( github.com/mattn/go-runewidth v0.0.15 // indirect github.com/mattn/go-shellwords v1.0.12 // indirect github.com/muesli/termenv v0.16.0 // indirect - github.com/pelletier/go-toml/v2 v2.1.2-0.20240227203013-2b69615b5d55 // indirect + github.com/pelletier/go-toml/v2 v2.2.4 // indirect github.com/pmezard/go-difflib v1.0.1-0.20181226105442-5d4384ee4fb2 // indirect github.com/rivo/uniseg v0.4.7 // indirect github.com/rogpeppe/go-internal v1.13.1 // indirect - github.com/tdewolff/parse/v2 v2.7.19 // indirect - github.com/tdewolff/test v1.0.11-0.20240106005702-7de5f7df4739 // indirect - golang.org/x/crypto v0.40.0 // indirect - golang.org/x/image v0.25.0 // indirect - golang.org/x/mod v0.25.0 // indirect - golang.org/x/net v0.42.0 // indirect - golang.org/x/sync v0.16.0 // indirect - golang.org/x/sys v0.34.0 // indirect - golang.org/x/text v0.27.0 // indirect - google.golang.org/genproto v0.0.0-20250804133106-a7a43d27e69b // indirect - google.golang.org/genproto/googleapis/rpc v0.0.0-20250728155136-f173205681a0 // indirect + github.com/tdewolff/parse/v2 v2.8.5 // indirect + golang.org/x/crypto v0.47.0 // indirect + golang.org/x/image v0.35.0 // indirect + golang.org/x/mod v0.32.0 // indirect + golang.org/x/net v0.49.0 // indirect + golang.org/x/sync v0.19.0 // indirect + golang.org/x/sys v0.40.0 // indirect + golang.org/x/text v0.33.0 // indirect + google.golang.org/genproto v0.0.0-20260114163908-3f89685c29c3 // indirect + google.golang.org/genproto/googleapis/rpc v0.0.0-20251222181119-0a764e51fe1b // indirect gopkg.in/yaml.v3 v3.0.1 // indirect modernc.org/knuth v0.5.4 // indirect modernc.org/token v1.1.0 // indirect - star-tex.org/x/tex v0.6.0 // indirect + star-tex.org/x/tex v0.7.1 // indirect ) diff --git a/go.sum b/go.sum index f651c779..e810df60 100644 --- a/go.sum +++ b/go.sum @@ -1,69 +1,66 @@ -cogentcore.org/core v0.3.13 h1:+e7+SqlywIIcDQO4dP0klqaL0BBTbCjO4AXqROQtJlU= -cogentcore.org/core v0.3.13/go.mod h1:eDHnTCy1sBhAKN9NPsSCnBW3VAnwQBNA9nbGMo9r+Xs= +codeberg.org/go-pdf/fpdf v0.11.0 h1:n3I8WISQ1cr0S2rvx9DOlE/GypbcimMWqLpel3slHmY= +codeberg.org/go-pdf/fpdf v0.11.0/go.mod h1:Y0DGRAdZ0OmnZPvjbMp/1bYxmIPxm0ws4tfoPOc4LjU= +cogentcore.org/core v0.3.14-0.20260116180322-746be48a586f h1:vB/qftLxPr4FoYd+mwWAm2drvbfEcjMBXeHagGxIzX8= +cogentcore.org/core v0.3.14-0.20260116180322-746be48a586f/go.mod h1:BBGmzMSmyLImTGJcwNAzABxxaXG+E5niNZbZMPAWwaU= +git.sr.ht/~sbinet/cmpimg v0.1.0 h1:E0zPRk2muWuCqSKSVZIWsgtU9pjsw3eKHi8VmQeScxo= +git.sr.ht/~sbinet/cmpimg v0.1.0/go.mod h1:FU12psLbF4TfNXkKH2ZZQ29crIqoiqTZmeQ7dkp/pxE= github.com/Bios-Marcel/wastebasket/v2 v2.0.3 h1:TkoDPcSqluhLGE+EssHu7UGmLgUEkWg7kNyHyyJ3Q9g= github.com/Bios-Marcel/wastebasket/v2 v2.0.3/go.mod h1:769oPCv6eH7ugl90DYIsWwjZh4hgNmMS3Zuhe1bH6KU= -github.com/BurntSushi/toml v0.3.1/go.mod h1:xHWCNGjB5oqiDr8zfno3MHue2Ht5sIBksp03qcyfWMU= github.com/Masterminds/vcs v1.13.3 h1:IIA2aBdXvfbIM+yl/eTnL4hb1XwdpvuQLglAix1gweE= github.com/Masterminds/vcs v1.13.3/go.mod h1:TiE7xuEjl1N4j016moRd6vezp6e6Lz23gypeXfzXeW8= github.com/adrg/strutil v0.3.1 h1:OLvSS7CSJO8lBii4YmBt8jiK9QOtB9CzCzwl4Ic/Fz4= github.com/adrg/strutil v0.3.1/go.mod h1:8h90y18QLrs11IBffcGX3NW/GFBXCMcNg4M7H6MspPA= -github.com/alecthomas/assert/v2 v2.6.0 h1:o3WJwILtexrEUk3cUVal3oiQY2tfgr/FHWiz/v2n4FU= -github.com/alecthomas/assert/v2 v2.6.0/go.mod h1:Bze95FyfUr7x34QZrjL+XP+0qgp/zg8yS+TtBj1WA3k= -github.com/alecthomas/chroma/v2 v2.13.0 h1:VP72+99Fb2zEcYM0MeaWJmV+xQvz5v5cxRHd+ooU1lI= -github.com/alecthomas/chroma/v2 v2.13.0/go.mod h1:BUGjjsD+ndS6eX37YgTchSEG+Jg9Jv1GiZs9sqPqztk= -github.com/alecthomas/repr v0.4.0 h1:GhI2A8MACjfegCPVq9f1FLvIBS+DrQ2KQBFZP1iFzXc= -github.com/alecthomas/repr v0.4.0/go.mod h1:Fr0507jx4eOXV7AlPV6AVZLYrLIuIeSOWtW57eE/O/4= -github.com/anthonynsimon/bild v0.13.0 h1:mN3tMaNds1wBWi1BrJq0ipDBhpkooYfu7ZFSMhXt1C8= -github.com/anthonynsimon/bild v0.13.0/go.mod h1:tpzzp0aYkAsMi1zmfhimaDyX1xjn2OUc1AJZK/TF0AE= -github.com/armon/consul-api v0.0.0-20180202201655-eb2c6b5be1b6/go.mod h1:grANhF5doyWs3UAsr3K4I6qtAmlQcZDesFNEHPZAzj8= +github.com/alecthomas/assert/v2 v2.11.0 h1:2Q9r3ki8+JYXvGsDyBXwH3LcJ+WK5D0gc5E8vS6K3D0= +github.com/alecthomas/assert/v2 v2.11.0/go.mod h1:Bze95FyfUr7x34QZrjL+XP+0qgp/zg8yS+TtBj1WA3k= +github.com/alecthomas/chroma/v2 v2.22.0 h1:PqEhf+ezz5F5owoDeOUKFzW+W3ZJDShNCaHg4sZuItI= +github.com/alecthomas/chroma/v2 v2.22.0/go.mod h1:NqVhfBR0lte5Ouh3DcthuUCTUpDC9cxBOfyMbMQPs3o= +github.com/alecthomas/repr v0.5.2 h1:SU73FTI9D1P5UNtvseffFSGmdNci/O6RsqzeXJtP0Qs= +github.com/alecthomas/repr v0.5.2/go.mod h1:Fr0507jx4eOXV7AlPV6AVZLYrLIuIeSOWtW57eE/O/4= +github.com/anthonynsimon/bild v0.14.0 h1:IFRkmKdNdqmexXHfEU7rPlAmdUZ8BDZEGtGHDnGWync= +github.com/anthonynsimon/bild v0.14.0/go.mod h1:hcvEAyBjTW69qkKJTfpcDQ83sSZHxwOunsseDfeQhUs= github.com/aymanbagabas/go-osc52/v2 v2.0.1 h1:HwpRHbFMcZLEVr42D4p7XBqjyuxQH5SMiErDT4WkJ2k= github.com/aymanbagabas/go-osc52/v2 v2.0.1/go.mod h1:uYgXzlJ7ZpABp8OJ+exZzJJhRNQ2ASbcXHWsFqH8hp8= github.com/aymerick/douceur v0.2.0 h1:Mv+mAeH1Q+n9Fr+oyamOlAkUNPWPlA8PPGR0QAaYuPk= github.com/aymerick/douceur v0.2.0/go.mod h1:wlT5vV2O3h55X9m7iVYN0TBM0NH/MmbLnd30/FjWUq4= -github.com/bramvdbogaerde/go-scp v1.4.0 h1:jKMwpwCbcX1KyvDbm/PDJuXcMuNVlLGi0Q0reuzjyKY= -github.com/bramvdbogaerde/go-scp v1.4.0/go.mod h1:on2aH5AxaFb2G0N5Vsdy6B0Ml7k9HuHSwfo1y0QzAbQ= -github.com/chewxy/math32 v1.10.1 h1:LFpeY0SLJXeaiej/eIp2L40VYfscTvKh/FSEZ68uMkU= -github.com/chewxy/math32 v1.10.1/go.mod h1:dOB2rcuFrCn6UHrze36WSLVPKtzPMRAQvBvUwkSsLqs= +github.com/bramvdbogaerde/go-scp v1.6.0 h1:lDh0lUuz1dbIhJqlKLwWT7tzIRONCp1Mtx3pgQVaLQo= +github.com/bramvdbogaerde/go-scp v1.6.0/go.mod h1:on2aH5AxaFb2G0N5Vsdy6B0Ml7k9HuHSwfo1y0QzAbQ= +github.com/chewxy/math32 v1.11.1 h1:b7PGHlp8KjylDoU8RrcEsRuGZhJuz8haxnKfuMMRqy8= +github.com/chewxy/math32 v1.11.1/go.mod h1:dOB2rcuFrCn6UHrze36WSLVPKtzPMRAQvBvUwkSsLqs= github.com/cogentcore/readline v0.1.3 h1:tYmjP3XHvsGwhsDLkAp+vBhkERmLFENZfftyPOR/PBE= github.com/cogentcore/readline v0.1.3/go.mod h1:IHVtJHSKXspK7CMg3OC/bbPEXxO++dFlug/vsPktvas= github.com/cogentcore/webgpu v0.23.0 h1:hrjnnuDZAPSRsqBjQAsJOyg2COGztIkBbxL87r0Q9KE= github.com/cogentcore/webgpu v0.23.0/go.mod h1:ciqaxChrmRRMU1SnI5OE12Cn3QWvOKO+e5nSy+N9S1o= -github.com/cogentcore/yaegi v0.0.0-20250622201820-b7838bdd95eb h1:vXYqPLO36pRyyk1cVILVlk+slDI+Q7N4bgeWlh1sjA0= -github.com/cogentcore/yaegi v0.0.0-20250622201820-b7838bdd95eb/go.mod h1:+MGpZ0srBmeJ7aaOLTdVss8WLolt0/y/plVHLpxgd3A= -github.com/coreos/etcd v3.3.10+incompatible/go.mod h1:uF7uidLiAD3TWHmW31ZFd/JWoc32PjwdhPthX9715RE= -github.com/coreos/go-etcd v2.0.0+incompatible/go.mod h1:Jez6KQU2B/sWsbdaef3ED8NzMklzPG4d5KIOhIy30Tk= -github.com/coreos/go-semver v0.2.0/go.mod h1:nnelYz7RCh+5ahJtPPxZlU+153eP4D4r3EedlOD2RNk= -github.com/cpuguy83/go-md2man v1.0.10/go.mod h1:SmD6nW6nTyfqj6ABTjUi3V3JVMnlJmwcJI5acqYI6dE= +github.com/cogentcore/yaegi v0.0.0-20260116172027-700fbf8949f3 h1:y3Djpt/g3QTjFdj8cpvy/r8FsZsEa7PqHGjgsKXbta0= +github.com/cogentcore/yaegi v0.0.0-20260116172027-700fbf8949f3/go.mod h1:XkOm++pRmWlk85p+hw71ZItfTeRdzqG23+2xjP9eb+M= github.com/creack/pty v1.1.9/go.mod h1:oKZEueFk5CKHvIhNR5MUki03XCEU+Q6VDXinZuGJ33E= github.com/davecgh/go-spew v1.1.0/go.mod h1:J7Y8YcW2NihsgmVo/mv3lAwl/skON4iLHjSsI+c5H38= github.com/davecgh/go-spew v1.1.1/go.mod h1:J7Y8YcW2NihsgmVo/mv3lAwl/skON4iLHjSsI+c5H38= github.com/davecgh/go-spew v1.1.2-0.20180830191138-d8f796af33cc h1:U9qPSI2PIWSS1VwoXQT9A3Wy9MM3WgvqSxFWenqJduM= github.com/davecgh/go-spew v1.1.2-0.20180830191138-d8f796af33cc/go.mod h1:J7Y8YcW2NihsgmVo/mv3lAwl/skON4iLHjSsI+c5H38= -github.com/dlclark/regexp2 v1.11.0 h1:G/nrcoOa7ZXlpoa/91N3X7mM3r8eIlMBBJZvsz/mxKI= -github.com/dlclark/regexp2 v1.11.0/go.mod h1:DHkYz0B9wPfa6wondMfaivmHpzrQ3v9q8cnmRbL6yW8= -github.com/ericchiang/css v1.3.0 h1:e0vS+vpujMjtT3/SYu7qTHn1LVzXWcLCCDjlfq3YlLY= -github.com/ericchiang/css v1.3.0/go.mod h1:sVSdL+MFR9Q4cKJMQzpIkHIDOLiK+7Wmjjhq7D+MubA= -github.com/fsnotify/fsnotify v1.4.7/go.mod h1:jwhsz4b93w/PPRr/qN1Yymfu8t87LnFCMoQvtojpjFo= -github.com/fsnotify/fsnotify v1.8.0 h1:dAwr6QBTBZIkG8roQaJjGof0pp0EeF+tNV7YBP3F/8M= -github.com/fsnotify/fsnotify v1.8.0/go.mod h1:8jBTzvmWwFyi3Pb8djgCCO5IBqzKJ/Jwo8TRcHyHii0= +github.com/dlclark/regexp2 v1.11.5 h1:Q/sSnsKerHeCkc/jSTNq1oCm7KiVgUMZRDUoRu0JQZQ= +github.com/dlclark/regexp2 v1.11.5/go.mod h1:DHkYz0B9wPfa6wondMfaivmHpzrQ3v9q8cnmRbL6yW8= +github.com/ericchiang/css v1.4.0 h1:OlkWiPGHZpWIthKa2YBSAh00XwOT1PUtaoye9bKkTqw= +github.com/ericchiang/css v1.4.0/go.mod h1:sVSdL+MFR9Q4cKJMQzpIkHIDOLiK+7Wmjjhq7D+MubA= +github.com/fsnotify/fsnotify v1.9.0 h1:2Ml+OJNzbYCTzsxtv8vKSFD9PbJjmhYF14k/jKC7S9k= +github.com/fsnotify/fsnotify v1.9.0/go.mod h1:8jBTzvmWwFyi3Pb8djgCCO5IBqzKJ/Jwo8TRcHyHii0= github.com/go-fonts/latin-modern v0.3.3 h1:g2xNgI8yzdNzIVm+qvbMryB6yGPe0pSMss8QT3QwlJ0= github.com/go-fonts/latin-modern v0.3.3/go.mod h1:tHaiWDGze4EPB0Go4cLT5M3QzRY3peya09Z/8KSCrpY= -github.com/go-gl/glfw/v3.3/glfw v0.0.0-20240506104042-037f3cc74f2a h1:vxnBhFDDT+xzxf1jTJKMKZw3H0swfWk9RpWbBbDK5+0= -github.com/go-gl/glfw/v3.3/glfw v0.0.0-20240506104042-037f3cc74f2a/go.mod h1:tQ2UAYgL5IevRw8kRxooKSPJfGvJ9fJQFa0TUsXzTg8= +github.com/go-gl/glfw/v3.3/glfw v0.0.0-20250301202403-da16c1255728 h1:RkGhqHxEVAvPM0/R+8g7XRwQnHatO0KAuVcwHo8q9W8= +github.com/go-gl/glfw/v3.3/glfw v0.0.0-20250301202403-da16c1255728/go.mod h1:SyRD8YfuKk+ZXlDqYiqe1qMSqjNgtHzBTG810KUagMc= github.com/go-logr/logr v1.4.3 h1:CjnDlHq8ikf6E492q6eKboGOC0T8CDaOvkHCIg8idEI= github.com/go-logr/logr v1.4.3/go.mod h1:9T104GzyrTigFIr8wt5mBrctHMim0Nb2HLGrmQ40KvY= github.com/go-logr/stdr v1.2.2 h1:hSWxHoqTgW2S2qGc0LTAI563KZ5YKYRhT3MFKZMbjag= github.com/go-logr/stdr v1.2.2/go.mod h1:mMo/vtBO5dYbehREoey6XUKy/eSumjCCveDpRre4VKE= -github.com/go-text/typesetting v0.3.1-0.20250402122313-7a0f05577ff5 h1:ChaHVT66Mk9SwP0bdWEKwikYd709GSFjGxWKPeZsE14= -github.com/go-text/typesetting v0.3.1-0.20250402122313-7a0f05577ff5/go.mod h1:qjZLkhRgOEYMhU9eHBr3AR4sfnGJvOXNLt8yRAySFuY= -github.com/go-text/typesetting-utils v0.0.0-20241103174707-87a29e9e6066 h1:qCuYC+94v2xrb1PoS4NIDe7DGYtLnU2wWiQe9a1B1c0= -github.com/go-text/typesetting-utils v0.0.0-20241103174707-87a29e9e6066/go.mod h1:DDxDdQEnB70R8owOx3LVpEFvpMK9eeH1o2r0yZhFI9o= +github.com/go-text/typesetting v0.3.2 h1:OUOFxp9Rx5PiO0/rh2IY+5gmyXjXsVG8+LfEyk9NMcE= +github.com/go-text/typesetting v0.3.2/go.mod h1:vIRUT25mLQaSh4C8H/lIsKppQz/Gdb8Pu/tNwpi52ts= +github.com/go-text/typesetting-utils v0.0.0-20250618110550-c820a94c77b8 h1:4KCscI9qYWMGTuz6BpJtbUSRzcBrUSSE0ENMJbNSrFs= +github.com/go-text/typesetting-utils v0.0.0-20250618110550-c820a94c77b8/go.mod h1:3/62I4La/HBRX9TcTpBj4eipLiwzf+vhI+7whTc9V7o= github.com/gobwas/glob v0.2.3 h1:A4xDbljILXROh+kObIiy5kIaPYD8e96x1tgBhUI5J+Y= github.com/gobwas/glob v0.2.3/go.mod h1:d3Ez4x06l9bZtSvzIay5+Yzi0fmZzPgnTbPcKjJAkT8= github.com/golang/protobuf v1.5.4 h1:i7eJL8qZTpSEXOPTxNKhASYpMn+8e5Q6AdndVa1dWek= github.com/golang/protobuf v1.5.4/go.mod h1:lnTiLA8Wa4RWRcIUkrtSVa5nRhsEGBg48fD6rSs7xps= -github.com/gomarkdown/markdown v0.0.0-20250311123330-531bef5e742b h1:EY/KpStFl60qA17CptGXhwfZ+k1sFNJIUNR8DdbcuUk= -github.com/gomarkdown/markdown v0.0.0-20250311123330-531bef5e742b/go.mod h1:JDGcbDT52eL4fju3sZ4TeHGsQwhG9nbDV21aMyhwPoA= +github.com/gomarkdown/markdown v0.0.0-20250810172220-2e2c11897d1a h1:l7A0loSszR5zHd/qK53ZIHMO8b3bBSmENnQ6eKnUT0A= +github.com/gomarkdown/markdown v0.0.0-20250810172220-2e2c11897d1a/go.mod h1:JDGcbDT52eL4fju3sZ4TeHGsQwhG9nbDV21aMyhwPoA= github.com/google/go-cmp v0.5.6/go.mod h1:v8dTdLbMG2kIc/vJvl+f65V22dbkXbowE6jgT/gNBxE= github.com/google/go-cmp v0.7.0 h1:wk8382ETsv4JYUZwIsn6YpYiWiBsYLSJiTsyBybVuN8= github.com/google/go-cmp v0.7.0/go.mod h1:pXiqmnSA92OHEEa9HXL2W4E7lf9JzCmGVUdgjX3N/iU= @@ -75,14 +72,12 @@ github.com/h2non/filetype v1.1.3 h1:FKkx9QbD7HR/zjK1Ia5XiBsq9zdLi5Kf3zGyFTAFkGg= github.com/h2non/filetype v1.1.3/go.mod h1:319b3zT68BvV+WRj7cwy856M2ehB3HqNOt6sy1HndBY= github.com/hack-pad/go-indexeddb v0.3.2 h1:DTqeJJYc1usa45Q5r52t01KhvlSN02+Oq+tQbSBI91A= github.com/hack-pad/go-indexeddb v0.3.2/go.mod h1:QvfTevpDVlkfomY498LhstjwbPW6QC4VC/lxYb0Kom0= -github.com/hack-pad/hackpadfs v0.2.1 h1:FelFhIhv26gyjujoA/yeFO+6YGlqzmc9la/6iKMIxMw= -github.com/hack-pad/hackpadfs v0.2.1/go.mod h1:khQBuCEwGXWakkmq8ZiFUvUZz84ZkJ2KNwKvChs4OrU= +github.com/hack-pad/hackpadfs v0.2.4 h1:7pmzQGR6JsGq/uB0JWxd3wTBi7I85f46CHGvcfrJsiE= +github.com/hack-pad/hackpadfs v0.2.4/go.mod h1:2XDioLb2NwaQzRYo+cpgNx1iMALzBQ4bQoLhHpArQZM= github.com/hack-pad/safejs v0.1.1 h1:d5qPO0iQ7h2oVtpzGnLExE+Wn9AtytxIfltcS2b9KD8= github.com/hack-pad/safejs v0.1.1/go.mod h1:HdS+bKF1NrE72VoXZeWzxFOVQVUSqZJAG0xNCnb+Tio= -github.com/hashicorp/hcl v1.0.0/go.mod h1:E5yfLk+7swimpb2L/Alb/PJmXilQ/rhwaUYs4T20WEQ= github.com/hexops/gotextdiff v1.0.3 h1:gitA9+qJrrTCsiCl7+kh75nPqQt1cx4ZkudSTLoUqJM= github.com/hexops/gotextdiff v1.0.3/go.mod h1:pSWU5MAI3yDq+fZBTazCSJysOMbxWL1BSow5/V2vxeg= -github.com/inconshreveable/mousetrap v1.0.0/go.mod h1:PxqpIevigyE2G7u3NXJIT2ANytuPF1OarO4DADm73n8= github.com/jeandeaual/go-locale v0.0.0-20250612000132-0ef82f21eade h1:FmusiCI1wHw+XQbvL9M+1r/C3SPqKrmBaIOYwVfQoDE= github.com/jeandeaual/go-locale v0.0.0-20250612000132-0ef82f21eade/go.mod h1:ZDXo8KHryOWSIqnsb/CiDq7hQUYryCgdVnxbj8tDG7o= github.com/jinzhu/copier v0.4.0 h1:w3ciUoD19shMCRargcpm0cm91ytaBhDvuRpz1ODO/U8= @@ -93,7 +88,6 @@ github.com/kr/text v0.2.0 h1:5Nx0Ya0ZqY2ygV366QzturHI13Jq95ApcVaJBhpS+AY= github.com/kr/text v0.2.0/go.mod h1:eLer722TekiGuMkidMxC/pM04lWEeraHUUmBw8l2grE= github.com/lucasb-eyer/go-colorful v1.2.0 h1:1nnpGOrhyZZuNyfu1QjKiUICQ74+3FNCN69Aj6K7nkY= github.com/lucasb-eyer/go-colorful v1.2.0/go.mod h1:R4dSotOR9KMtayYi1e77YzuveK+i7ruzyGqttikkLy0= -github.com/magiconair/properties v1.8.0/go.mod h1:PppfXfuXeibc/6YijjN8zIbojt8czPbwD3XqdrwzmxQ= github.com/mattn/go-isatty v0.0.20 h1:xfD0iDuEKnDkl03q4limB+vH+GxLEtL/jb4xVJSWWEY= github.com/mattn/go-isatty v0.0.20/go.mod h1:W+V8PltTTMOvKvAeJH7IuucS94S2C6jfK/D7dTCTo3Y= github.com/mattn/go-runewidth v0.0.9/go.mod h1:H031xJmbD/WCDINGzjvQ9THkh0rPKHF+m2gUSrubnMI= @@ -103,14 +97,12 @@ github.com/mattn/go-shellwords v1.0.12 h1:M2zGm7EW6UQJvDeQxo4T51eKPurbeFbe8WtebG github.com/mattn/go-shellwords v1.0.12/go.mod h1:EZzvwXDESEeg03EKmM+RmDnNOPKG4lLtQsUlTZDWQ8Y= github.com/mitchellh/go-homedir v1.1.0 h1:lukF9ziXFxDFPkA1vsr5zpc1XuPDn/wFntq5mG+4E0Y= github.com/mitchellh/go-homedir v1.1.0/go.mod h1:SfyaCUpYCn1Vlf4IUYiD9fPX4A5wJrkLzIz1N1q0pr0= -github.com/mitchellh/mapstructure v1.1.2/go.mod h1:FVVH3fgwuzCH5S8UJGiWEs2h04kUh9fWfEaFds41c1Y= github.com/muesli/termenv v0.16.0 h1:S5AlUN9dENB57rsbnkPyfdGuWIlkmzJjbFf0Tf5FWUc= github.com/muesli/termenv v0.16.0/go.mod h1:ZRfOIKPFDYQoDFF4Olj7/QJbW60Ol/kL1pU3VfY/Cnk= github.com/nsf/termbox-go v1.1.1 h1:nksUPLCb73Q++DwbYUBEglYBRPZyoXJdrj5L+TkjyZY= github.com/nsf/termbox-go v1.1.1/go.mod h1:T0cTdVuOwf7pHQNtfhnEbzHbcNyCEcVU4YPpouCbVxo= -github.com/pelletier/go-toml v1.2.0/go.mod h1:5z9KED0ma1S8pY6P1sdut58dfprrGBbd/94hg7ilaic= -github.com/pelletier/go-toml/v2 v2.1.2-0.20240227203013-2b69615b5d55 h1:CJwoX/v1ZWNj0Ofn62jvQDRuH3/hIHMqCQxbkzq2m5Y= -github.com/pelletier/go-toml/v2 v2.1.2-0.20240227203013-2b69615b5d55/go.mod h1:tJU2Z3ZkXwnxa4DPO899bsyIoywizdUvyaeZurnPPDc= +github.com/pelletier/go-toml/v2 v2.2.4 h1:mye9XuhQ6gvn5h28+VilKrrPoQVanw5PMw/TB0t5Ec4= +github.com/pelletier/go-toml/v2 v2.2.4/go.mod h1:2gIqNv+qfxSVS7cM2xJQKtLSTLUE9V8t9Stt+h56mCY= github.com/pmezard/go-difflib v1.0.0/go.mod h1:iKH77koFhYxTK1pcRnkKkqfTogsbg7gZNVY4sRDYZ/4= github.com/pmezard/go-difflib v1.0.1-0.20181226105442-5d4384ee4fb2 h1:Jamvg5psRIccs7FGNTlIRMkT8wgtp5eCXdBlqhYGL6U= github.com/pmezard/go-difflib v1.0.1-0.20181226105442-5d4384ee4fb2/go.mod h1:iKH77koFhYxTK1pcRnkKkqfTogsbg7gZNVY4sRDYZ/4= @@ -119,29 +111,18 @@ github.com/rivo/uniseg v0.4.7 h1:WUdvkW8uEhrYfLC4ZzdpI2ztxP1I582+49Oc5Mq64VQ= github.com/rivo/uniseg v0.4.7/go.mod h1:FN3SvrM+Zdj16jyLfmOkMNblXMcoc8DfTHruCPUcx88= github.com/rogpeppe/go-internal v1.13.1 h1:KvO1DLK/DRN07sQ1LQKScxyZJuNnedQ5/wKSR38lUII= github.com/rogpeppe/go-internal v1.13.1/go.mod h1:uMEvuHeurkdAXX61udpOXGD/AzZDWNMNyH2VO9fmH0o= -github.com/russross/blackfriday v1.5.2/go.mod h1:JO/DiYxRf+HjHt06OyowR9PTA263kcR/rfWxYHBV53g= -github.com/spf13/afero v1.1.2/go.mod h1:j4pytiNVoe2o6bmDsKpLACNPDBIoEAkihy7loJ1B0CQ= -github.com/spf13/cast v1.3.0/go.mod h1:Qx5cxh0v+4UWYiBimWS+eyWzqEqokIECu5etghLkUJE= -github.com/spf13/cobra v0.0.5/go.mod h1:3K3wKZymM7VvHMDS9+Akkh4K60UwM26emMESw8tLCHU= -github.com/spf13/jwalterweatherman v1.0.0/go.mod h1:cQK4TGJAtQXfYWX+Ddv3mKDzgVb68N+wFjFa4jdeBTo= -github.com/spf13/pflag v1.0.3/go.mod h1:DYY7MBk1bdzusC3SYhjObp+wFpr4gzcvqqNjLnInEg4= -github.com/spf13/viper v1.3.2/go.mod h1:ZiWeW+zYFKm7srdB9IoDzzZXaJaI5eL9QjNiN/DMA2s= github.com/stretchr/objx v0.1.0/go.mod h1:HFkY916IF+rwdDfMAkV7OtwuqBVzrE8GR6GFx+wExME= github.com/stretchr/objx v0.4.0/go.mod h1:YvHI0jy2hoMjB+UWwv71VJQ9isScKT/TqJzVSSt89Yw= github.com/stretchr/objx v0.5.0/go.mod h1:Yh+to48EsGEfYuaHDzXPcE3xhTkx73EhmCGUpEOglKo= -github.com/stretchr/testify v1.2.2/go.mod h1:a8OnRcib4nhh0OaRAV+Yts87kKdq0PP7pXfy6kDkUVs= github.com/stretchr/testify v1.7.1/go.mod h1:6Fq8oRcR53rry900zMqJjRRixrwX3KX962/h/Wwjteg= github.com/stretchr/testify v1.8.0/go.mod h1:yNjHg4UonilssWZ8iaSj1OCr/vHnekPRkoO+kdMU+MU= github.com/stretchr/testify v1.8.4/go.mod h1:sz/lmYIOXD/1dqDmKjjqLyZ2RngseejIcXlSw2iwfAo= -github.com/stretchr/testify v1.10.0 h1:Xv5erBjTwe/5IxqUQTdXv5kgmIvbHo3QQyRwhJsOfJA= -github.com/stretchr/testify v1.10.0/go.mod h1:r2ic/lqez/lEtzL7wO/rwa5dbSLXVDPFyf8C91i36aY= -github.com/tdewolff/parse/v2 v2.7.19 h1:7Ljh26yj+gdLFEq/7q9LT4SYyKtwQX4ocNrj45UCePg= -github.com/tdewolff/parse/v2 v2.7.19/go.mod h1:3FbJWZp3XT9OWVN3Hmfp0p/a08v4h8J9W1aghka0soA= -github.com/tdewolff/test v1.0.11-0.20231101010635-f1265d231d52/go.mod h1:6DAvZliBAAnD7rhVgwaM7DE5/d9NMOAJ09SqYqeK4QE= -github.com/tdewolff/test v1.0.11-0.20240106005702-7de5f7df4739 h1:IkjBCtQOOjIn03u/dMQK9g+Iw9ewps4mCl1nB8Sscbo= -github.com/tdewolff/test v1.0.11-0.20240106005702-7de5f7df4739/go.mod h1:XPuWBzvdUzhCuxWO1ojpXsyzsA5bFoS3tO/Q3kFuTG8= -github.com/ugorji/go/codec v0.0.0-20181204163529-d75b2dcb6bc8/go.mod h1:VFNgLljTbGfSG7qAOspJ7OScBnGdDN/yBr0sguwnwf0= -github.com/xordataexchange/crypt v0.0.3-0.20170626215501-b2862e3d0a77/go.mod h1:aYKd//L2LvnjZzWKhF00oedf4jCCReLcmhLdhm1A27Q= +github.com/stretchr/testify v1.11.1 h1:7s2iGBzp5EwR7/aIZr8ao5+dra3wiQyKjjFuvgVKu7U= +github.com/stretchr/testify v1.11.1/go.mod h1:wZwfW3scLgRK+23gO65QZefKpKQRnfz6sD981Nm4B6U= +github.com/tdewolff/parse/v2 v2.8.5 h1:ZmBiA/8Do5Rpk7bDye0jbbDUpXXbCdc3iah4VeUvwYU= +github.com/tdewolff/parse/v2 v2.8.5/go.mod h1:Hwlni2tiVNKyzR1o6nUs4FOF07URA+JLBLd6dlIXYqo= +github.com/tdewolff/test v1.0.11 h1:FdLbwQVHxqG16SlkGveC0JVyrJN62COWTRyUFzfbtBE= +github.com/tdewolff/test v1.0.11/go.mod h1:XPuWBzvdUzhCuxWO1ojpXsyzsA5bFoS3tO/Q3kFuTG8= go.opentelemetry.io/auto/sdk v1.1.0 h1:cH53jehLUN6UFLY71z+NDOiNJqDdPRaXzTel0sJySYA= go.opentelemetry.io/auto/sdk v1.1.0/go.mod h1:3wSPjt5PWp2RhlCcmmOial7AvC4DQqZb7a7wCow3W8A= go.opentelemetry.io/otel v1.36.0 h1:UumtzIklRBY6cI/lllNZlALOF5nNIzJVb16APdvgTXg= @@ -154,52 +135,56 @@ go.opentelemetry.io/otel/sdk/metric v1.36.0 h1:r0ntwwGosWGaa0CrSt8cuNuTcccMXERFw go.opentelemetry.io/otel/sdk/metric v1.36.0/go.mod h1:qTNOhFDfKRwX0yXOqJYegL5WRaW376QbB7P4Pb0qva4= go.opentelemetry.io/otel/trace v1.36.0 h1:ahxWNuqZjpdiFAyrIoQ4GIiAIhxAunQR6MUoKrsNd4w= go.opentelemetry.io/otel/trace v1.36.0/go.mod h1:gQ+OnDZzrybY4k4seLzPAWNwVBBVlF2szhehOBB/tGA= -golang.org/x/crypto v0.0.0-20181203042331-505ab145d0a9/go.mod h1:6SG95UA2DQfeDnfUPMdvaQW0Q7yPrPDi9nlGo2tz2b4= -golang.org/x/crypto v0.40.0 h1:r4x+VvoG5Fm+eJcxMaY8CQM7Lb0l1lsmjGBQ6s8BfKM= -golang.org/x/crypto v0.40.0/go.mod h1:Qr1vMER5WyS2dfPHAlsOj01wgLbsyWtFn/aY+5+ZdxY= -golang.org/x/exp v0.0.0-20231110203233-9a3e6036ecaa h1:FRnLl4eNAQl8hwxVVC17teOw8kdjVDVAiFMtgUdTSRQ= -golang.org/x/exp v0.0.0-20231110203233-9a3e6036ecaa/go.mod h1:zk2irFbV9DP96SEBUUAy67IdHUaZuSnrz1n472HUCLE= -golang.org/x/image v0.0.0-20190703141733-d6a02ce849c9/go.mod h1:FeLwcggjj3mMvU+oOTbSwawSJRM1uh48EjtB4UJZlP0= -golang.org/x/image v0.25.0 h1:Y6uW6rH1y5y/LK1J8BPWZtr6yZ7hrsy6hFrXjgsc2fQ= -golang.org/x/image v0.25.0/go.mod h1:tCAmOEGthTtkalusGp1g3xa2gke8J6c2N565dTyl9Rs= -golang.org/x/mod v0.25.0 h1:n7a+ZbQKQA/Ysbyb0/6IbB1H/X41mKgbhfv7AfG/44w= -golang.org/x/mod v0.25.0/go.mod h1:IXM97Txy2VM4PJ3gI61r1YEk/gAj6zAHN3AdZt6S9Ww= +golang.org/x/crypto v0.47.0 h1:V6e3FRj+n4dbpw86FJ8Fv7XVOql7TEwpHapKoMJ/GO8= +golang.org/x/crypto v0.47.0/go.mod h1:ff3Y9VzzKbwSSEzWqJsJVBnWmRwRSHt/6Op5n9bQc4A= +golang.org/x/exp v0.0.0-20260112195511-716be5621a96 h1:Z/6YuSHTLOHfNFdb8zVZomZr7cqNgTJvA8+Qz75D8gU= +golang.org/x/exp v0.0.0-20260112195511-716be5621a96/go.mod h1:nzimsREAkjBCIEFtHiYkrJyT+2uy9YZJB7H1k68CXZU= +golang.org/x/image v0.35.0 h1:LKjiHdgMtO8z7Fh18nGY6KDcoEtVfsgLDPeLyguqb7I= +golang.org/x/image v0.35.0/go.mod h1:MwPLTVgvxSASsxdLzKrl8BRFuyqMyGhLwmC+TO1Sybk= +golang.org/x/mod v0.32.0 h1:9F4d3PHLljb6x//jOyokMv3eX+YDeepZSEo3mFJy93c= +golang.org/x/mod v0.32.0/go.mod h1:SgipZ/3h2Ci89DlEtEXWUk/HteuRin+HHhN+WbNhguU= golang.org/x/net v0.0.0-20211216030914-fe4d6282115f/go.mod h1:9nx3DQGgdP8bBQD5qxJ1jj9UTztislL4KSBs9R2vV5Y= -golang.org/x/net v0.42.0 h1:jzkYrhi3YQWD6MLBJcsklgQsoAcw89EcZbJw8Z614hs= -golang.org/x/net v0.42.0/go.mod h1:FF1RA5d3u7nAYA4z2TkclSCKh68eSXtiFwcWQpPXdt8= -golang.org/x/sync v0.16.0 h1:ycBJEhp9p4vXvUZNszeOq0kGTPghopOL8q0fq3vstxw= -golang.org/x/sync v0.16.0/go.mod h1:1dzgHSNfp02xaA81J2MS99Qcpr2w7fw1gpm99rleRqA= -golang.org/x/sys v0.0.0-20181205085412-a5c9d58dba9a/go.mod h1:STP8DvDyc/dI5b8T5hshtkjS+E42TnysNCUPdjciGhY= +golang.org/x/net v0.49.0 h1:eeHFmOGUTtaaPSGNmjBKpbng9MulQsJURQUAfUwY++o= +golang.org/x/net v0.49.0/go.mod h1:/ysNB2EvaqvesRkuLAyjI1ycPZlQHM3q01F02UY/MV8= +golang.org/x/sync v0.19.0 h1:vV+1eWNmZ5geRlYjzm2adRgW2/mcpevXNg50YZtPCE4= +golang.org/x/sync v0.19.0/go.mod h1:9KTHXmSnoGruLpwFjVSX0lNNA75CykiMECbovNTZqGI= golang.org/x/sys v0.0.0-20201119102817-f84b799fce68/go.mod h1:h1NjWce9XRLGQEsW7wpKNCjG9DtNlClVuFLEZdDNbEs= golang.org/x/sys v0.0.0-20210423082822-04245dca01da/go.mod h1:h1NjWce9XRLGQEsW7wpKNCjG9DtNlClVuFLEZdDNbEs= golang.org/x/sys v0.6.0/go.mod h1:oPkhp1MJrh7nUepCBck5+mAzfO9JrbApNNgaTdGDITg= -golang.org/x/sys v0.34.0 h1:H5Y5sJ2L2JRdyv7ROF1he/lPdvFsd0mJHFw2ThKHxLA= -golang.org/x/sys v0.34.0/go.mod h1:BJP2sWEmIv4KK5OTEluFJCKSidICx8ciO85XgH3Ak8k= +golang.org/x/sys v0.40.0 h1:DBZZqJ2Rkml6QMQsZywtnjnnGvHza6BTfYFWY9kjEWQ= +golang.org/x/sys v0.40.0/go.mod h1:OgkHotnGiDImocRcuBABYBEXf8A9a87e/uXjp9XT3ks= golang.org/x/term v0.0.0-20201126162022-7de9c90e9dd1/go.mod h1:bj7SfCRtBDWHUb9snDiAeCFNEtKQo2Wmx5Cou7ajbmo= -golang.org/x/term v0.33.0 h1:NuFncQrRcaRvVmgRkvM3j/F00gWIAlcmlB8ACEKmGIg= -golang.org/x/term v0.33.0/go.mod h1:s18+ql9tYWp1IfpV9DmCtQDDSRBUjKaw9M1eAv5UeF0= -golang.org/x/text v0.3.0/go.mod h1:NqM8EUOU14njkJ3fqMW+pc6Ldnwhi/IjpwHt7yyuwOQ= +golang.org/x/term v0.39.0 h1:RclSuaJf32jOqZz74CkPA9qFuVTX7vhLlpfj/IGWlqY= +golang.org/x/term v0.39.0/go.mod h1:yxzUCTP/U+FzoxfdKmLaA0RV1WgE0VY7hXBwKtY/4ww= golang.org/x/text v0.3.6/go.mod h1:5Zoc/QRtKVWzQhOtBMvqHzDpF6irO9z98xDceosuGiQ= -golang.org/x/text v0.27.0 h1:4fGWRpyh641NLlecmyl4LOe6yDdfaYNrGb2zdfo4JV4= -golang.org/x/text v0.27.0/go.mod h1:1D28KMCvyooCX9hBiosv5Tz/+YLxj0j7XhWjpSUF7CU= +golang.org/x/text v0.33.0 h1:B3njUFyqtHDUI5jMn1YIr5B0IE2U0qck04r6d4KPAxE= +golang.org/x/text v0.33.0/go.mod h1:LuMebE6+rBincTi9+xWTY8TztLzKHc/9C1uBCG27+q8= golang.org/x/tools v0.0.0-20180917221912-90fa682c2a6e/go.mod h1:n7NCudcB/nEzxVGmLbDWY5pfWTLqBcC2KZ6jyYvM4mQ= -golang.org/x/tools v0.34.0 h1:qIpSLOxeCYGg9TrcJokLBG4KFA6d795g0xkBkiESGlo= -golang.org/x/tools v0.34.0/go.mod h1:pAP9OwEaY1CAW3HOmg3hLZC5Z0CCmzjAF2UQMSqNARg= +golang.org/x/tools v0.41.0 h1:a9b8iMweWG+S0OBnlU36rzLp20z1Rp10w+IY2czHTQc= +golang.org/x/tools v0.41.0/go.mod h1:XSY6eDqxVNiYgezAVqqCeihT4j1U2CCsqvH3WhQpnlg= golang.org/x/xerrors v0.0.0-20191204190536-9bdfabe68543/go.mod h1:I/5z698sn9Ka8TeJc9MKroUUfqBBauWjQqLJ2OPfmY0= gonum.org/v1/gonum v0.15.0 h1:2lYxjRbTYyxkJxlhC+LvJIx3SsANPdRybu1tGj9/OrQ= gonum.org/v1/gonum v0.15.0/go.mod h1:xzZVBJBtS+Mz4q0Yl2LJTk+OxOg4jiXZ7qBoM0uISGo= -google.golang.org/genproto v0.0.0-20250804133106-a7a43d27e69b h1:eZTgydvqZO44zyTZAvMaSyAxccZZdraiSAGvqOczVvk= -google.golang.org/genproto v0.0.0-20250804133106-a7a43d27e69b/go.mod h1:suyz2QBHQKlGIF92HEEsCfO1SwxXdk7PFLz+Zd9Uah4= +gonum.org/v1/gonum v0.16.0/go.mod h1:fef3am4MQ93R2HHpKnLk4/Tbh/s0+wqD5nfa6Pnwy4E= +gonum.org/v1/gonum v0.17.0 h1:VbpOemQlsSMrYmn7T2OUvQ4dqxQXU+ouZFQsZOx50z4= +gonum.org/v1/gonum v0.17.0/go.mod h1:El3tOrEuMpv2UdMrbNlKEh9vd86bmQ6vqIcDwxEOc1E= +google.golang.org/genproto v0.0.0-20260114163908-3f89685c29c3 h1:rUamZFBwsWVWg4Yb7iTbwYp81XVHUvOXNdrFCoYRRNE= +google.golang.org/genproto v0.0.0-20260114163908-3f89685c29c3/go.mod h1:wE6SUYr3iNtF/D0GxVAjT+0CbDFktQNssYs9PVptCt4= google.golang.org/genproto/googleapis/rpc v0.0.0-20250728155136-f173205681a0 h1:MAKi5q709QWfnkkpNQ0M12hYJ1+e8qYVDyowc4U1XZM= google.golang.org/genproto/googleapis/rpc v0.0.0-20250728155136-f173205681a0/go.mod h1:qQ0YXyHHx3XkvlzUtpXDkS29lDSafHMZBAZDc03LQ3A= +google.golang.org/genproto/googleapis/rpc v0.0.0-20251222181119-0a764e51fe1b h1:Mv8VFug0MP9e5vUxfBcE3vUkV6CImK3cMNMIDFjmzxU= +google.golang.org/genproto/googleapis/rpc v0.0.0-20251222181119-0a764e51fe1b/go.mod h1:j9x/tPzZkyxcgEFkiKEEGxfvyumM01BEtsW8xzOahRQ= google.golang.org/grpc v1.74.2 h1:WoosgB65DlWVC9FqI82dGsZhWFNBSLjQ84bjROOpMu4= google.golang.org/grpc v1.74.2/go.mod h1:CtQ+BGjaAIXHs/5YS3i473GqwBBa1zGQNevxdeBEXrM= +google.golang.org/grpc v1.76.0 h1:UnVkv1+uMLYXoIz6o7chp59WfQUYA2ex/BXQ9rHZu7A= +google.golang.org/grpc v1.76.0/go.mod h1:Ju12QI8M6iQJtbcsV+awF5a4hfJMLi4X0JLo94ULZ6c= google.golang.org/protobuf v1.36.6 h1:z1NpPI8ku2WgiWnf+t9wTPsn6eP1L7ksHUlkfLvd9xY= google.golang.org/protobuf v1.36.6/go.mod h1:jduwjTPXsFjZGTmRluh+L6NjiWu7pchiJ2/5YcXBHnY= +google.golang.org/protobuf v1.36.11 h1:fV6ZwhNocDyBLK0dj+fg8ektcVegBBuEolpbTQyBNVE= +google.golang.org/protobuf v1.36.11/go.mod h1:HTf+CrKn2C3g5S8VImy6tdcUvCska2kB7j23XfzDpco= gopkg.in/check.v1 v0.0.0-20161208181325-20d25e280405/go.mod h1:Co6ibVJAznAaIkqp8huTwlJQCZ016jof/cbN4VW5Yz0= gopkg.in/check.v1 v1.0.0-20201130134442-10cb98267c6c h1:Hei/4ADfdWqJk1ZMxUNpqntNwaWcugrBjAiHlqqRiVk= gopkg.in/check.v1 v1.0.0-20201130134442-10cb98267c6c/go.mod h1:JHkPIbrfpd72SG/EVd6muEfDQjcINNoR0C8j2r3qZ4Q= -gopkg.in/yaml.v2 v2.2.2/go.mod h1:hI93XBmqTisBFMUTm0b8Fm+jr3Dg1NNxqwp+5A1VGuI= gopkg.in/yaml.v3 v3.0.0-20200313102051-9f266ea9e77c/go.mod h1:K4uyk7z7BCEPqu6E+C64Yfv1cQ7kz7rIZviUmN+EgEM= gopkg.in/yaml.v3 v3.0.1 h1:fxVm/GzAzEWqLHuvctI91KS9hhNmmWOoWu0XTYJS7CA= gopkg.in/yaml.v3 v3.0.1/go.mod h1:K4uyk7z7BCEPqu6E+C64Yfv1cQ7kz7rIZviUmN+EgEM= @@ -207,5 +192,7 @@ modernc.org/knuth v0.5.4 h1:F8mDs7ME3oN9eyx01n6/xVmJ4F5U/qEhSYPnPXaZrps= modernc.org/knuth v0.5.4/go.mod h1:e5SBb35HQBj2aFwbBO3ClPcViLY3Wi0LzaOd7c/3qMk= modernc.org/token v1.1.0 h1:Xl7Ap9dKaEs5kLoOQeQmPWevfnk/DM5qcLcYlA8ys6Y= modernc.org/token v1.1.0/go.mod h1:UGzOrNV1mAFSEB63lOFHIpNRUVMvYTc6yu1SMY/XTDM= -star-tex.org/x/tex v0.6.0 h1:ZD/4082kR5+2gFzFNgRvZBMCGuXrQWp3hNo5W5LmCeI= -star-tex.org/x/tex v0.6.0/go.mod h1:wJWeUmM2d4qH/mCtMOcioNl2sluKx85mLi+Yv9Nq4Ms= +rsc.io/pdf v0.1.1 h1:k1MczvYDUvJBe93bYd7wrZLLUEcLZAuF824/I4e5Xr4= +rsc.io/pdf v0.1.1/go.mod h1:n8OzWcQ6Sp37PL01nO98y4iUCRdTGarVfzxY20ICaU4= +star-tex.org/x/tex v0.7.1 h1:4qGAByRyY0WQsOjtcHlxz+FgrYxz8fzxIds2Gjepp5U= +star-tex.org/x/tex v0.7.1/go.mod h1:Y3y0U7sZTltTh/CDZIx0oAtMjG7eMaTuTtvDZGdyhJo= diff --git a/physics/phyxyz/editor.go b/physics/phyxyz/editor.go index 3b4eb6ac..e661e284 100644 --- a/physics/phyxyz/editor.go +++ b/physics/phyxyz/editor.go @@ -6,6 +6,7 @@ package phyxyz import ( "fmt" + "time" "cogentcore.org/core/colors" "cogentcore.org/core/core" @@ -199,6 +200,10 @@ func (pe *Editor) Step(n int) { pe.editor.AsyncLock() pe.editor.NeedsRender() pe.editor.AsyncUnlock() + if !pe.Model.GPU { + time.Sleep(time.Nanosecond) // this is essential for web (wasm) running to actually update + // if running in GPU mode, it works, but otherwise the thread never yields and it never updates. + } if pe.stop { pe.stop = false break diff --git a/plot/plots/table_test.go b/plot/plots/table_test.go index 64514d50..88d6c5a1 100644 --- a/plot/plots/table_test.go +++ b/plot/plots/table_test.go @@ -23,7 +23,7 @@ import ( // todo: move into statplot and test everything func TestTable(t *testing.T) { - rand.Seed(1) + rng := rand.New(rand.NewSource(1)) n := 21 tx, ty := tensor.NewFloat64(n), tensor.NewFloat64(n) tl, th := tensor.NewFloat64(n), tensor.NewFloat64(n) @@ -32,9 +32,9 @@ func TestTable(t *testing.T) { for i := range n { tx.SetFloat1D(float64(i*5), i) ty.SetFloat1D(50.0+40*math.Sin((float64(i)/8)*math.Pi), i) - tl.SetFloat1D(5*rand.Float64(), i) - th.SetFloat1D(5*rand.Float64(), i) - ts.SetFloat1D(1+5*rand.Float64(), i) + tl.SetFloat1D(5*rng.Float64(), i) + th.SetFloat1D(5*rng.Float64(), i) + ts.SetFloat1D(1+5*rng.Float64(), i) lbls.SetString1D(strconv.Itoa(i), i) } ptyps := maps.Keys(plot.Plotters) diff --git a/stats/cluster/clust_test.go b/stats/cluster/clust_test.go index bf6b07bc..58b35594 100644 --- a/stats/cluster/clust_test.go +++ b/stats/cluster/clust_test.go @@ -14,10 +14,10 @@ import ( "cogentcore.org/lab/stats/metric" "cogentcore.org/lab/table" "cogentcore.org/lab/tensor" - "github.com/alecthomas/assert/v2" + "github.com/stretchr/testify/assert" ) -var clustres = ` +var clusters = ` 0: 9.181170003996987: 5.534356399283666: From 6615cf6ca9d6ddbeed0df5453bb19de2f6bd38ee Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Fri, 16 Jan 2026 10:49:58 -0800 Subject: [PATCH 94/97] physics: yaegi update --- yaegilab/labsymbols/cogentcore_org-lab-physics.go | 1 + yaegilab/tensorsymbols/cogentcore_org-lab-tensor.go | 6 ++++++ yaegilab/tensorsymbols/cogentcore_org-lab-tensorfs.go | 1 + 3 files changed, 8 insertions(+) diff --git a/yaegilab/labsymbols/cogentcore_org-lab-physics.go b/yaegilab/labsymbols/cogentcore_org-lab-physics.go index b4b4e7b0..a05d3a13 100644 --- a/yaegilab/labsymbols/cogentcore_org-lab-physics.go +++ b/yaegilab/labsymbols/cogentcore_org-lab-physics.go @@ -13,6 +13,7 @@ func init() { Symbols["cogentcore.org/lab/physics/physics"] = map[string]reflect.Value{ // function, constant and variable definitions "AddBroadContacts": reflect.ValueOf(physics.AddBroadContacts), + "AngularAccelAt": reflect.ValueOf(physics.AngularAccelAt), "AngularCorrection": reflect.ValueOf(physics.AngularCorrection), "AngularVelocityAt": reflect.ValueOf(physics.AngularVelocityAt), "Ball": reflect.ValueOf(physics.Ball), diff --git a/yaegilab/tensorsymbols/cogentcore_org-lab-tensor.go b/yaegilab/tensorsymbols/cogentcore_org-lab-tensor.go index e48f83c7..329232ab 100644 --- a/yaegilab/tensorsymbols/cogentcore_org-lab-tensor.go +++ b/yaegilab/tensorsymbols/cogentcore_org-lab-tensor.go @@ -180,10 +180,15 @@ func init() { // type definitions "Arg": reflect.ValueOf((*tensor.Arg)(nil)), "Bool": reflect.ValueOf((*tensor.Bool)(nil)), + "Byte": reflect.ValueOf((*tensor.Byte)(nil)), "Delims": reflect.ValueOf((*tensor.Delims)(nil)), "FilterFunc": reflect.ValueOf((*tensor.FilterFunc)(nil)), + "Float32": reflect.ValueOf((*tensor.Float32)(nil)), + "Float64": reflect.ValueOf((*tensor.Float64)(nil)), "Func": reflect.ValueOf((*tensor.Func)(nil)), "Indexed": reflect.ValueOf((*tensor.Indexed)(nil)), + "Int": reflect.ValueOf((*tensor.Int)(nil)), + "Int32": reflect.ValueOf((*tensor.Int32)(nil)), "Masked": reflect.ValueOf((*tensor.Masked)(nil)), "Reshaped": reflect.ValueOf((*tensor.Reshaped)(nil)), "RowMajor": reflect.ValueOf((*tensor.RowMajor)(nil)), @@ -195,6 +200,7 @@ func init() { "String": reflect.ValueOf((*tensor.String)(nil)), "StringMatch": reflect.ValueOf((*tensor.StringMatch)(nil)), "Tensor": reflect.ValueOf((*tensor.Tensor)(nil)), + "Uint32": reflect.ValueOf((*tensor.Uint32)(nil)), "Values": reflect.ValueOf((*tensor.Values)(nil)), // interface wrapper definitions diff --git a/yaegilab/tensorsymbols/cogentcore_org-lab-tensorfs.go b/yaegilab/tensorsymbols/cogentcore_org-lab-tensorfs.go index 53ada569..8f086790 100644 --- a/yaegilab/tensorsymbols/cogentcore_org-lab-tensorfs.go +++ b/yaegilab/tensorsymbols/cogentcore_org-lab-tensorfs.go @@ -39,5 +39,6 @@ func init() { "DirFile": reflect.ValueOf((*tensorfs.DirFile)(nil)), "File": reflect.ValueOf((*tensorfs.File)(nil)), "Node": reflect.ValueOf((*tensorfs.Node)(nil)), + "Nodes": reflect.ValueOf((*tensorfs.Nodes)(nil)), } } From bba174f30871e59bf68c64d4c6cead5ad114091b Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Fri, 16 Jan 2026 11:01:27 -0800 Subject: [PATCH 95/97] physics: update go versions in github workflows; unclear why test is exiting with err code 1? --- .github/workflows/core.yml | 2 +- .github/workflows/go.yml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/core.yml b/.github/workflows/core.yml index 70c4b14a..9a20dfb6 100644 --- a/.github/workflows/core.yml +++ b/.github/workflows/core.yml @@ -37,7 +37,7 @@ jobs: - name: Install Go uses: actions/setup-go@v5 with: - go-version: '1.23.4' + go-version: '1.25.6' - name: Install Core run: go install cogentcore.org/core@main diff --git a/.github/workflows/go.yml b/.github/workflows/go.yml index 8178466c..de7557d4 100644 --- a/.github/workflows/go.yml +++ b/.github/workflows/go.yml @@ -16,7 +16,7 @@ jobs: - name: Set up Go uses: actions/setup-go@v5 with: - go-version: '1.23.4' + go-version: '1.25.6' - name: Set up Core run: go install cogentcore.org/core@main && core setup From 86476170311540de574eb06cf59fa9e3bd25cb46 Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Fri, 16 Jan 2026 13:04:04 -0800 Subject: [PATCH 96/97] physics: include physics examples, using the special element handler logic -- easy. --- docs/content/physics-balls.md | 8 ++++ docs/content/physics-collide.md | 10 +++++ docs/content/physics-virtroom.md | 8 ++++ docs/content/physics.md | 4 ++ docs/docs.go | 33 +++++++++----- physics/examples/balls/balls.go | 9 ++-- physics/examples/balls/balls/main.go | 16 +++++++ .../collision.go => collide/collide.go} | 8 ++-- physics/examples/collide/collide/main.go | 16 +++++++ physics/examples/collide/typegen.go | 9 ++++ physics/examples/collision/typegen.go | 9 ---- physics/examples/virtroom/typegen.go | 8 ++-- physics/examples/virtroom/virtroom.go | 45 +++++-------------- physics/examples/virtroom/virtroom/main.go | 30 +++++++++++++ 14 files changed, 144 insertions(+), 69 deletions(-) create mode 100644 docs/content/physics-balls.md create mode 100644 docs/content/physics-collide.md create mode 100644 docs/content/physics-virtroom.md create mode 100644 physics/examples/balls/balls/main.go rename physics/examples/{collision/collision.go => collide/collide.go} (97%) create mode 100644 physics/examples/collide/collide/main.go create mode 100644 physics/examples/collide/typegen.go delete mode 100644 physics/examples/collision/typegen.go create mode 100644 physics/examples/virtroom/virtroom/main.go diff --git a/docs/content/physics-balls.md b/docs/content/physics-balls.md new file mode 100644 index 00000000..c1a273d8 --- /dev/null +++ b/docs/content/physics-balls.md @@ -0,0 +1,8 @@ ++++ +Categories = ["Physics"] ++++ + +This example (source in https://github.com/cogentcore/lab/tree/main/physics/examples/balls) demonstrates the parallel processing power of running physics on the GPU. Note that the bounce physics (restitution) is not currently working properly. + + + diff --git a/docs/content/physics-collide.md b/docs/content/physics-collide.md new file mode 100644 index 00000000..30550061 --- /dev/null +++ b/docs/content/physics-collide.md @@ -0,0 +1,10 @@ ++++ +Categories = ["Physics"] ++++ + +This example (source in https://github.com/cogentcore/lab/tree/main/physics/examples/collide) demonstrates the collision dynamics of different shapes. Do `Rebuild` after changing shapes or other parameters such as friction. The `Cylinder` and `Cone` shapes are not well supported for collision. + +The pusher target position slider at the bottom controls how much force is imparted by the pusher "paddle" that hits the first object. + + + diff --git a/docs/content/physics-virtroom.md b/docs/content/physics-virtroom.md new file mode 100644 index 00000000..8713c873 --- /dev/null +++ b/docs/content/physics-virtroom.md @@ -0,0 +1,8 @@ ++++ +Categories = ["Physics"] ++++ + +This example (source in https://github.com/cogentcore/lab/tree/main/physics/examples/virtroom) demonstrates how to use the `PlaneXZ` joint to move an object around on the plane, and how to use the physics API in a simulation context. Sensors for physical values and capturing the "first person" view from the eye of the virtual agent are also demonstrated. + + + diff --git a/docs/content/physics.md b/docs/content/physics.md index 91fd9009..38fa65fc 100644 --- a/docs/content/physics.md +++ b/docs/content/physics.md @@ -1,4 +1,5 @@ +++ +Categories = ["Physics"] bibfile = "ccnlab.json" +++ @@ -332,3 +333,6 @@ The XPBD solver that we implement ([[@MacklinMullerChentanez16]] and [[@MullerMa Furthermore, the [newton-physics](https://github.com/newton-physics/newton) code for XPBD was very directly convertible to Go and GoSL (unlike the situation with bullet), so the overall process was relatively straightforward. +## Physics examples + + diff --git a/docs/docs.go b/docs/docs.go index 8dfad369..2955dac3 100644 --- a/docs/docs.go +++ b/docs/docs.go @@ -14,6 +14,9 @@ import ( "cogentcore.org/core/text/csl" _ "cogentcore.org/core/text/tex" // include this to get math "cogentcore.org/core/tree" + "cogentcore.org/lab/physics/examples/balls" + "cogentcore.org/lab/physics/examples/collide" + "cogentcore.org/lab/physics/examples/virtroom" _ "cogentcore.org/lab/yaegilab" ) @@ -49,19 +52,25 @@ func main() { ctx.LinkButton(w, "https://youtube.com/@CogentCore") w.SetText("Videos").SetIcon(icons.VideoLibrary) }) - tree.Add(p, func(w *core.Button) { - ctx.LinkButton(w, "https://cogentcore.org/blog") - w.SetText("Blog").SetIcon(icons.RssFeed) - }) - tree.Add(p, func(w *core.Button) { - ctx.LinkButton(w, "https://cogentcore.org/community") - w.SetText("Community").SetIcon(icons.Forum) - }) - tree.Add(p, func(w *core.Button) { - ctx.LinkButton(w, "https://github.com/sponsors/cogentcore") - w.SetText("Sponsor").SetIcon(icons.Favorite) - }) }) }) + + ctx.ElementHandlers["physics-balls"] = func(ctx *htmlcore.Context) bool { + balls.Config(ctx.BlockParent) + return true + } + + ctx.ElementHandlers["physics-collide"] = func(ctx *htmlcore.Context) bool { + collide.Config(ctx.BlockParent) + return true + } + + ctx.ElementHandlers["physics-virtroom"] = func(ctx *htmlcore.Context) bool { + ev := &virtroom.Env{} + ev.Defaults() + ev.ConfigGUI(ctx.BlockParent) + return true + } + b.RunMainWindow() } diff --git a/physics/examples/balls/balls.go b/physics/examples/balls/balls.go index 299efa60..749f6500 100644 --- a/physics/examples/balls/balls.go +++ b/physics/examples/balls/balls.go @@ -2,7 +2,7 @@ // Use of this source code is governed by a BSD-style // license that can be found in the LICENSE file. -package main +package balls //go:generate core generate -add-types @@ -10,8 +10,8 @@ import ( "math/rand/v2" "cogentcore.org/core/colors" - "cogentcore.org/core/core" "cogentcore.org/core/math32" + "cogentcore.org/core/tree" _ "cogentcore.org/lab/gosl/slbool/slboolcore" // include to get gui views "cogentcore.org/lab/physics" "cogentcore.org/lab/physics/phyxyz" @@ -61,8 +61,7 @@ func (b *Balls) Defaults() { b.FrictionRolling = 0 } -func main() { - b := core.NewBody("test1").SetTitle("Physics Balls") +func Config(b tree.Node) { ed := phyxyz.NewEditor(b) bs := &Balls{} @@ -113,6 +112,4 @@ func main() { bl.SetBodyFrictionRolling(bs.FrictionRolling) } }) - - b.RunMainWindow() } diff --git a/physics/examples/balls/balls/main.go b/physics/examples/balls/balls/main.go new file mode 100644 index 00000000..3024163a --- /dev/null +++ b/physics/examples/balls/balls/main.go @@ -0,0 +1,16 @@ +// Copyright (c) 2025, Cogent Core. All rights reserved. +// Use of this source code is governed by a BSD-style +// license that can be found in the LICENSE file. + +package main + +import ( + "cogentcore.org/core/core" + "cogentcore.org/lab/physics/examples/balls" +) + +func main() { + b := core.NewBody("balls").SetTitle("Physics Balls") + balls.Config(b) + b.RunMainWindow() +} diff --git a/physics/examples/collision/collision.go b/physics/examples/collide/collide.go similarity index 97% rename from physics/examples/collision/collision.go rename to physics/examples/collide/collide.go index eb8d38db..0cbd7f56 100644 --- a/physics/examples/collision/collision.go +++ b/physics/examples/collide/collide.go @@ -2,13 +2,14 @@ // Use of this source code is governed by a BSD-style // license that can be found in the LICENSE file. -package main +package collide //go:generate core generate -add-types import ( "cogentcore.org/core/core" "cogentcore.org/core/math32" + "cogentcore.org/core/tree" _ "cogentcore.org/lab/gosl/slbool/slboolcore" // include to get gui views "cogentcore.org/lab/physics" "cogentcore.org/lab/physics/phyxyz" @@ -66,8 +67,7 @@ func (cl *Collide) Defaults() { cl.FrictionRolling = 0.01 } -func main() { - b := core.NewBody("collide").SetTitle("Physics Collide") +func Config(b tree.Node) { ed := phyxyz.NewEditor(b) ed.CameraPos = math32.Vec3(0, 20, 20) @@ -120,6 +120,4 @@ func main() { physics.SetJointTargetPos(0, 0, pos, 100) physics.SetJointTargetVel(0, 0, 0, 20) }) - - b.RunMainWindow() } diff --git a/physics/examples/collide/collide/main.go b/physics/examples/collide/collide/main.go new file mode 100644 index 00000000..378a6f62 --- /dev/null +++ b/physics/examples/collide/collide/main.go @@ -0,0 +1,16 @@ +// Copyright (c) 2025, Cogent Core. All rights reserved. +// Use of this source code is governed by a BSD-style +// license that can be found in the LICENSE file. + +package main + +import ( + "cogentcore.org/core/core" + "cogentcore.org/lab/physics/examples/collide" +) + +func main() { + b := core.NewBody("collide").SetTitle("Physics Collide") + collide.Config(b) + b.RunMainWindow() +} diff --git a/physics/examples/collide/typegen.go b/physics/examples/collide/typegen.go new file mode 100644 index 00000000..e27147a6 --- /dev/null +++ b/physics/examples/collide/typegen.go @@ -0,0 +1,9 @@ +// Code generated by "core generate -add-types"; DO NOT EDIT. + +package collide + +import ( + "cogentcore.org/core/types" +) + +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics/examples/collide.Collide", IDName: "collide", Doc: "Collide has sim params", Fields: []types.Field{{Name: "ShapeA", Doc: "Shape of left body"}, {Name: "ShapeB", Doc: "Shape of right body"}, {Name: "SizeA", Doc: "Size of left body (radius, capsule, cylinder, box are 2x taller)"}, {Name: "SizeB", Doc: "Size of right body (radius, capsule, cylinder, box are 2x taller)"}, {Name: "MassA", Doc: "Mass of left object: if lighter than B, it will bounce back more."}, {Name: "MassB", Doc: "Mass of right object: if lighter than B, it will move faster."}, {Name: "ZposA", Doc: "Z (depth) position: offset to get different collision angles."}, {Name: "ZposB", Doc: "Z (depth) position: offset to get different collision angles."}, {Name: "PushMass", Doc: "Mass of the pusher panel: if lighter, it transfers less energy."}, {Name: "Friction", Doc: "Friction is for sliding: around 0.01 seems pretty realistic"}, {Name: "FrictionTortion", Doc: "FrictionTortion is for rotating. Not generally relevant here."}, {Name: "FrictionRolling", Doc: "FrictionRolling is for rolling: around 0.01 seems pretty realistic"}}}) diff --git a/physics/examples/collision/typegen.go b/physics/examples/collision/typegen.go deleted file mode 100644 index 9df92715..00000000 --- a/physics/examples/collision/typegen.go +++ /dev/null @@ -1,9 +0,0 @@ -// Code generated by "core generate -add-types"; DO NOT EDIT. - -package main - -import ( - "cogentcore.org/core/types" -) - -var _ = types.AddType(&types.Type{Name: "main.Collide", IDName: "collide", Doc: "Collide has sim params", Fields: []types.Field{{Name: "ShapeA", Doc: "Shape of left body"}, {Name: "ShapeB", Doc: "Shape of right body"}, {Name: "SizeA", Doc: "Size of left body (radius, capsule, cylinder, box are 2x taller)"}, {Name: "SizeB", Doc: "Size of right body (radius, capsule, cylinder, box are 2x taller)"}, {Name: "MassA", Doc: "Mass of left object: if lighter than B, it will bounce back more."}, {Name: "MassB", Doc: "Mass of right object: if lighter than B, it will move faster."}, {Name: "ZposA", Doc: "Z (depth) position: offset to get different collision angles."}, {Name: "ZposB", Doc: "Z (depth) position: offset to get different collision angles."}, {Name: "PushMass", Doc: "Mass of the pusher panel: if lighter, it transfers less energy."}, {Name: "Friction", Doc: "Friction is for sliding: around 0.01 seems pretty realistic"}, {Name: "FrictionTortion"}, {Name: "FrictionRolling", Doc: "FrictionRolling is for rolling: around 0.01 seems pretty realistic"}}}) diff --git a/physics/examples/virtroom/typegen.go b/physics/examples/virtroom/typegen.go index 4cf6ab45..b7668b3e 100644 --- a/physics/examples/virtroom/typegen.go +++ b/physics/examples/virtroom/typegen.go @@ -1,9 +1,11 @@ -// Code generated by "core generate"; DO NOT EDIT. +// Code generated by "core generate -add-types"; DO NOT EDIT. -package main +package virtroom import ( "cogentcore.org/core/types" ) -var _ = types.AddType(&types.Type{Name: "main.Env", IDName: "env", Doc: "Env encapsulates the virtual environment", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Methods: []types.Method{{Name: "InitState", Doc: "Initstate reinitializes the physics model state.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "GrabEyeImage", Doc: "GrabEyeImage takes a snapshot from the perspective of Emer's right eye", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "ModelStep", Doc: "ModelStep does one step of the physics model.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "StepForward", Doc: "StepForward moves Emer forward in current facing direction one step", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "StepBackward", Doc: "StepBackward moves Emer backward in current facing direction one step.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "RotBodyLeft", Doc: "RotBodyLeft rotates emer left.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "RotBodyRight", Doc: "RotBodyRight rotates emer right.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "RotHeadLeft", Doc: "RotHeadLeft rotates emer left.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "RotHeadRight", Doc: "RotHeadRight rotates emer right.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}}, Fields: []types.Field{{Name: "Emer", Doc: "Emer state"}, {Name: "Stiff", Doc: "Stiffness for actions"}, {Name: "MoveStep", Doc: "how far to move every step"}, {Name: "RotStep", Doc: "how far to rotate every step"}, {Name: "ModelSteps", Doc: "number of model steps to take"}, {Name: "Width", Doc: "width of room"}, {Name: "Depth", Doc: "depth of room"}, {Name: "Height", Doc: "height of room"}, {Name: "Thick", Doc: "thickness of walls of room"}, {Name: "DepthVals", Doc: "current depth map"}, {Name: "Camera", Doc: "offscreen render camera settings"}, {Name: "DepthMap", Doc: "color map to use for rendering depth map"}, {Name: "Physics", Doc: "The core physics elements: Model, Builder, Scene"}, {Name: "SceneEditor", Doc: "3D visualization of the Scene"}, {Name: "EyeRImg", Doc: "snapshot image"}, {Name: "DepthImage", Doc: "depth map image"}}}) +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics/examples/virtroom.Emer", IDName: "emer", Doc: "Emer is the robot agent in the environment.", Fields: []types.Field{{Name: "Angry", Doc: "if true, emer is angry: changes face color"}, {Name: "VestibHRightEar", Doc: "VestibHRightEar is the horizontal rotation vestibular signal measured\nat the right ear, averaged over the steps."}, {Name: "Height", Doc: "full height of emer"}, {Name: "Obj", Doc: "emer object"}, {Name: "XZ", Doc: "PlaneXZ joint for controlling 2D position."}, {Name: "Neck", Doc: "ball joint for the neck."}, {Name: "EyeR", Doc: "Right eye of emer"}}}) + +var _ = types.AddType(&types.Type{Name: "cogentcore.org/lab/physics/examples/virtroom.Env", IDName: "env", Doc: "Env encapsulates the virtual environment", Directives: []types.Directive{{Tool: "types", Directive: "add"}}, Methods: []types.Method{{Name: "InitState", Doc: "Initstate reinitializes the physics model state.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "GrabEyeImage", Doc: "GrabEyeImage takes a snapshot from the perspective of Emer's right eye", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "ModelStep", Doc: "ModelStep does one step of the physics model.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "StepForward", Doc: "StepForward moves Emer forward in current facing direction one step", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "StepBackward", Doc: "StepBackward moves Emer backward in current facing direction one step.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "RotBodyLeft", Doc: "RotBodyLeft rotates emer left.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "RotBodyRight", Doc: "RotBodyRight rotates emer right.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "RotHeadLeft", Doc: "RotHeadLeft rotates emer left.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}, {Name: "RotHeadRight", Doc: "RotHeadRight rotates emer right.", Directives: []types.Directive{{Tool: "types", Directive: "add"}}}}, Fields: []types.Field{{Name: "Emer", Doc: "Emer state"}, {Name: "Stiff", Doc: "Stiffness for actions"}, {Name: "MoveStep", Doc: "how far to move every step"}, {Name: "RotStep", Doc: "how far to rotate every step"}, {Name: "ModelSteps", Doc: "number of model steps to take"}, {Name: "Width", Doc: "width of room"}, {Name: "Depth", Doc: "depth of room"}, {Name: "Height", Doc: "height of room"}, {Name: "Thick", Doc: "thickness of walls of room"}, {Name: "DepthVals", Doc: "current depth map"}, {Name: "Camera", Doc: "offscreen render camera settings"}, {Name: "DepthMap", Doc: "color map to use for rendering depth map"}, {Name: "Physics", Doc: "The core physics elements: Model, Builder, Scene"}, {Name: "SceneEditor", Doc: "3D visualization of the Scene"}, {Name: "EyeRImg", Doc: "snapshot image"}, {Name: "DepthImage", Doc: "depth map image"}}}) diff --git a/physics/examples/virtroom/virtroom.go b/physics/examples/virtroom/virtroom.go index 98dd0bad..2d179fde 100644 --- a/physics/examples/virtroom/virtroom.go +++ b/physics/examples/virtroom/virtroom.go @@ -2,15 +2,14 @@ // Use of this source code is governed by a BSD-style // license that can be found in the LICENSE file. -package main +package virtroom -//go:generate core generate +//go:generate core generate -add-types import ( "fmt" "image" "math/rand/v2" - "os" "cogentcore.org/core/base/iox/imagex" "cogentcore.org/core/colors" @@ -30,23 +29,6 @@ import ( "cogentcore.org/lab/physics/phyxyz" ) -var NoGUI bool - -func main() { - if len(os.Args) > 1 && os.Args[1] == "-nogui" { - NoGUI = true - } - ev := &Env{} - ev.Defaults() - if NoGUI { - ev.NoGUIRun() - return - } - // core.RenderTrace = true - b := ev.ConfigGUI() - b.RunMainWindow() -} - // Emer is the robot agent in the environment. type Emer struct { // if true, emer is angry: changes face color @@ -161,9 +143,10 @@ func (ev *Env) MakeModel(sc *xyz.Scene) { ev.MakeEmer(ew, &ev.Emer, "emer") // vw.Physics.Builder.ReplicateWorld(vw.Physics.Scene, 1, 1, 8) // 1x8 ev.Physics.Build() - // params := physics.GetParams(0) + params := physics.GetParams(0) // params.ControlDt = 0.1 - // params.Gravity.Y = 0 + params.Dt = 0.001 + params.SubSteps = 1 // params.MaxForce = 1.0e3 // params.AngularDamping = 0.5 // params.SubSteps = 1 @@ -226,10 +209,11 @@ func (ev *Env) UpdateView() { // ModelStep does one step of the physics model. func (ev *Env) ModelStep() { //types:add ev.Emer.VestibHRightEar = 0 - for range ev.ModelSteps { - ev.Physics.Step(1) + for range ev.ModelSteps { // we're computing average over sensor data + ev.Physics.StepQuiet(1) ev.Sensors() } + ev.Physics.Step(1) ev.Emer.VestibHRightEar /= float32(ev.ModelSteps) fmt.Println("vestibH right ear:", ev.Emer.VestibHRightEar) ev.Emer.Angry = false @@ -372,10 +356,10 @@ func (ev *Env) MakeEmer(wl *builder.World, em *Emer, name string) { ej.ParentFixed = true } -func (ev *Env) ConfigGUI() *core.Body { +func (ev *Env) ConfigGUI(b tree.Node) { // vgpu.Debug = true - - b := core.NewBody("virtroom").SetTitle("Physics Virtual Room") + tb := core.NewToolbar(b) + tb.Maker(ev.MakeToolbar) split := core.NewSplits(b) core.NewForm(split).SetStruct(ev) @@ -427,13 +411,6 @@ func (ev *Env) ConfigGUI() *core.Body { ev.DepthImage = core.NewImage(imfr) ev.DepthImage.SetName("depth-img") ev.DepthImage.Image = image.NewRGBA(image.Rectangle{Max: ev.Camera.Size}) - - //////// Toolbar - - b.AddTopBar(func(bar *core.Frame) { - core.NewToolbar(bar).Maker(ev.MakeToolbar) - }) - return b } func (ev *Env) MakeToolbar(p *tree.Plan) { diff --git a/physics/examples/virtroom/virtroom/main.go b/physics/examples/virtroom/virtroom/main.go new file mode 100644 index 00000000..63dae09c --- /dev/null +++ b/physics/examples/virtroom/virtroom/main.go @@ -0,0 +1,30 @@ +// Copyright (c) 2025, Cogent Core. All rights reserved. +// Use of this source code is governed by a BSD-style +// license that can be found in the LICENSE file. + +package main + +import ( + "os" + + "cogentcore.org/core/core" + "cogentcore.org/lab/physics/examples/virtroom" +) + +var NoGUI bool + +func main() { + if len(os.Args) > 1 && os.Args[1] == "-nogui" { + NoGUI = true + } + ev := &virtroom.Env{} + ev.Defaults() + if NoGUI { + ev.NoGUIRun() + return + } + // core.RenderTrace = true + b := core.NewBody("virtroom").SetTitle("Physics Virtual Room") + ev.ConfigGUI(b) + b.RunMainWindow() +} From 7292d0b9f9eb83dd4ff745f63a1f2e61aff1edc2 Mon Sep 17 00:00:00 2001 From: "Randall C. O'Reilly" Date: Fri, 16 Jan 2026 14:27:06 -0800 Subject: [PATCH 97/97] physics: update to latest core --- go.mod | 4 ++-- go.sum | 41 ++++++++++++++++------------------------- 2 files changed, 18 insertions(+), 27 deletions(-) diff --git a/go.mod b/go.mod index 95123049..1cbc56db 100644 --- a/go.mod +++ b/go.mod @@ -8,7 +8,7 @@ go 1.25.6 // https://github.com/googleapis/go-genproto/issues/1015 require ( - cogentcore.org/core v0.3.14-0.20260116180322-746be48a586f + cogentcore.org/core v0.3.14-0.20260116221522-8798f92d6991 github.com/cogentcore/readline v0.1.3 github.com/cogentcore/yaegi v0.0.0-20260116172027-700fbf8949f3 github.com/mitchellh/go-homedir v1.1.0 @@ -39,7 +39,7 @@ require ( github.com/fsnotify/fsnotify v1.9.0 // indirect github.com/go-fonts/latin-modern v0.3.3 // indirect github.com/go-gl/glfw/v3.3/glfw v0.0.0-20250301202403-da16c1255728 // indirect - github.com/go-text/typesetting v0.3.2 // indirect + github.com/go-text/typesetting v0.3.1-0.20250707124828-58cd3ef91251 // indirect github.com/gobwas/glob v0.2.3 // indirect github.com/gomarkdown/markdown v0.0.0-20250810172220-2e2c11897d1a // indirect github.com/gorilla/css v1.0.1 // indirect diff --git a/go.sum b/go.sum index e810df60..3c1882f6 100644 --- a/go.sum +++ b/go.sum @@ -1,7 +1,7 @@ codeberg.org/go-pdf/fpdf v0.11.0 h1:n3I8WISQ1cr0S2rvx9DOlE/GypbcimMWqLpel3slHmY= codeberg.org/go-pdf/fpdf v0.11.0/go.mod h1:Y0DGRAdZ0OmnZPvjbMp/1bYxmIPxm0ws4tfoPOc4LjU= -cogentcore.org/core v0.3.14-0.20260116180322-746be48a586f h1:vB/qftLxPr4FoYd+mwWAm2drvbfEcjMBXeHagGxIzX8= -cogentcore.org/core v0.3.14-0.20260116180322-746be48a586f/go.mod h1:BBGmzMSmyLImTGJcwNAzABxxaXG+E5niNZbZMPAWwaU= +cogentcore.org/core v0.3.14-0.20260116221522-8798f92d6991 h1:EBp041C3wDfVsu75AyT3zfpDBeqiKAIv8Qv/5rrrMhw= +cogentcore.org/core v0.3.14-0.20260116221522-8798f92d6991/go.mod h1:gMYI3RWI8c3tlm4cwU7JWuVbzafc+uPbgn4UZPNBhmo= git.sr.ht/~sbinet/cmpimg v0.1.0 h1:E0zPRk2muWuCqSKSVZIWsgtU9pjsw3eKHi8VmQeScxo= git.sr.ht/~sbinet/cmpimg v0.1.0/go.mod h1:FU12psLbF4TfNXkKH2ZZQ29crIqoiqTZmeQ7dkp/pxE= github.com/Bios-Marcel/wastebasket/v2 v2.0.3 h1:TkoDPcSqluhLGE+EssHu7UGmLgUEkWg7kNyHyyJ3Q9g= @@ -51,10 +51,10 @@ github.com/go-logr/logr v1.4.3 h1:CjnDlHq8ikf6E492q6eKboGOC0T8CDaOvkHCIg8idEI= github.com/go-logr/logr v1.4.3/go.mod h1:9T104GzyrTigFIr8wt5mBrctHMim0Nb2HLGrmQ40KvY= github.com/go-logr/stdr v1.2.2 h1:hSWxHoqTgW2S2qGc0LTAI563KZ5YKYRhT3MFKZMbjag= github.com/go-logr/stdr v1.2.2/go.mod h1:mMo/vtBO5dYbehREoey6XUKy/eSumjCCveDpRre4VKE= -github.com/go-text/typesetting v0.3.2 h1:OUOFxp9Rx5PiO0/rh2IY+5gmyXjXsVG8+LfEyk9NMcE= -github.com/go-text/typesetting v0.3.2/go.mod h1:vIRUT25mLQaSh4C8H/lIsKppQz/Gdb8Pu/tNwpi52ts= -github.com/go-text/typesetting-utils v0.0.0-20250618110550-c820a94c77b8 h1:4KCscI9qYWMGTuz6BpJtbUSRzcBrUSSE0ENMJbNSrFs= -github.com/go-text/typesetting-utils v0.0.0-20250618110550-c820a94c77b8/go.mod h1:3/62I4La/HBRX9TcTpBj4eipLiwzf+vhI+7whTc9V7o= +github.com/go-text/typesetting v0.3.1-0.20250707124828-58cd3ef91251 h1:QRcFLinlR2DKeVdwo/hwyz9S5HSzUiXnV0pzX/dhKOg= +github.com/go-text/typesetting v0.3.1-0.20250707124828-58cd3ef91251/go.mod h1:Fle5OPkxGUxdWDeCpKpvSCE31gEoTGBh63ASvTmSXBA= +github.com/go-text/typesetting-utils v0.0.0-20250527170436-63e4acdcf075 h1:zRaPuzKe/+Euzz3WwE0YwjXAX4IwvGPRH6abtmRI/4M= +github.com/go-text/typesetting-utils v0.0.0-20250527170436-63e4acdcf075/go.mod h1:DDxDdQEnB70R8owOx3LVpEFvpMK9eeH1o2r0yZhFI9o= github.com/gobwas/glob v0.2.3 h1:A4xDbljILXROh+kObIiy5kIaPYD8e96x1tgBhUI5J+Y= github.com/gobwas/glob v0.2.3/go.mod h1:d3Ez4x06l9bZtSvzIay5+Yzi0fmZzPgnTbPcKjJAkT8= github.com/golang/protobuf v1.5.4 h1:i7eJL8qZTpSEXOPTxNKhASYpMn+8e5Q6AdndVa1dWek= @@ -125,16 +125,16 @@ github.com/tdewolff/test v1.0.11 h1:FdLbwQVHxqG16SlkGveC0JVyrJN62COWTRyUFzfbtBE= github.com/tdewolff/test v1.0.11/go.mod h1:XPuWBzvdUzhCuxWO1ojpXsyzsA5bFoS3tO/Q3kFuTG8= go.opentelemetry.io/auto/sdk v1.1.0 h1:cH53jehLUN6UFLY71z+NDOiNJqDdPRaXzTel0sJySYA= go.opentelemetry.io/auto/sdk v1.1.0/go.mod h1:3wSPjt5PWp2RhlCcmmOial7AvC4DQqZb7a7wCow3W8A= -go.opentelemetry.io/otel v1.36.0 h1:UumtzIklRBY6cI/lllNZlALOF5nNIzJVb16APdvgTXg= -go.opentelemetry.io/otel v1.36.0/go.mod h1:/TcFMXYjyRNh8khOAO9ybYkqaDBb/70aVwkNML4pP8E= -go.opentelemetry.io/otel/metric v1.36.0 h1:MoWPKVhQvJ+eeXWHFBOPoBOi20jh6Iq2CcCREuTYufE= -go.opentelemetry.io/otel/metric v1.36.0/go.mod h1:zC7Ks+yeyJt4xig9DEw9kuUFe5C3zLbVjV2PzT6qzbs= -go.opentelemetry.io/otel/sdk v1.36.0 h1:b6SYIuLRs88ztox4EyrvRti80uXIFy+Sqzoh9kFULbs= -go.opentelemetry.io/otel/sdk v1.36.0/go.mod h1:+lC+mTgD+MUWfjJubi2vvXWcVxyr9rmlshZni72pXeY= -go.opentelemetry.io/otel/sdk/metric v1.36.0 h1:r0ntwwGosWGaa0CrSt8cuNuTcccMXERFwHX4dThiPis= -go.opentelemetry.io/otel/sdk/metric v1.36.0/go.mod h1:qTNOhFDfKRwX0yXOqJYegL5WRaW376QbB7P4Pb0qva4= -go.opentelemetry.io/otel/trace v1.36.0 h1:ahxWNuqZjpdiFAyrIoQ4GIiAIhxAunQR6MUoKrsNd4w= -go.opentelemetry.io/otel/trace v1.36.0/go.mod h1:gQ+OnDZzrybY4k4seLzPAWNwVBBVlF2szhehOBB/tGA= +go.opentelemetry.io/otel v1.37.0 h1:9zhNfelUvx0KBfu/gb+ZgeAfAgtWrfHJZcAqFC228wQ= +go.opentelemetry.io/otel v1.37.0/go.mod h1:ehE/umFRLnuLa/vSccNq9oS1ErUlkkK71gMcN34UG8I= +go.opentelemetry.io/otel/metric v1.37.0 h1:mvwbQS5m0tbmqML4NqK+e3aDiO02vsf/WgbsdpcPoZE= +go.opentelemetry.io/otel/metric v1.37.0/go.mod h1:04wGrZurHYKOc+RKeye86GwKiTb9FKm1WHtO+4EVr2E= +go.opentelemetry.io/otel/sdk v1.37.0 h1:ItB0QUqnjesGRvNcmAcU0LyvkVyGJ2xftD29bWdDvKI= +go.opentelemetry.io/otel/sdk v1.37.0/go.mod h1:VredYzxUvuo2q3WRcDnKDjbdvmO0sCzOvVAiY+yUkAg= +go.opentelemetry.io/otel/sdk/metric v1.37.0 h1:90lI228XrB9jCMuSdA0673aubgRobVZFhbjxHHspCPc= +go.opentelemetry.io/otel/sdk/metric v1.37.0/go.mod h1:cNen4ZWfiD37l5NhS+Keb5RXVWZWpRE+9WyVCpbo5ps= +go.opentelemetry.io/otel/trace v1.37.0 h1:HLdcFNbRQBE2imdSEgm/kwqmQj1Or1l/7bW6mxVK7z4= +go.opentelemetry.io/otel/trace v1.37.0/go.mod h1:TlgrlQ+PtQO5XFerSPUYG0JSgGyryXewPGyayAWSBS0= golang.org/x/crypto v0.47.0 h1:V6e3FRj+n4dbpw86FJ8Fv7XVOql7TEwpHapKoMJ/GO8= golang.org/x/crypto v0.47.0/go.mod h1:ff3Y9VzzKbwSSEzWqJsJVBnWmRwRSHt/6Op5n9bQc4A= golang.org/x/exp v0.0.0-20260112195511-716be5621a96 h1:Z/6YuSHTLOHfNFdb8zVZomZr7cqNgTJvA8+Qz75D8gU= @@ -163,23 +163,14 @@ golang.org/x/tools v0.0.0-20180917221912-90fa682c2a6e/go.mod h1:n7NCudcB/nEzxVGm golang.org/x/tools v0.41.0 h1:a9b8iMweWG+S0OBnlU36rzLp20z1Rp10w+IY2czHTQc= golang.org/x/tools v0.41.0/go.mod h1:XSY6eDqxVNiYgezAVqqCeihT4j1U2CCsqvH3WhQpnlg= golang.org/x/xerrors v0.0.0-20191204190536-9bdfabe68543/go.mod h1:I/5z698sn9Ka8TeJc9MKroUUfqBBauWjQqLJ2OPfmY0= -gonum.org/v1/gonum v0.15.0 h1:2lYxjRbTYyxkJxlhC+LvJIx3SsANPdRybu1tGj9/OrQ= -gonum.org/v1/gonum v0.15.0/go.mod h1:xzZVBJBtS+Mz4q0Yl2LJTk+OxOg4jiXZ7qBoM0uISGo= -gonum.org/v1/gonum v0.16.0/go.mod h1:fef3am4MQ93R2HHpKnLk4/Tbh/s0+wqD5nfa6Pnwy4E= gonum.org/v1/gonum v0.17.0 h1:VbpOemQlsSMrYmn7T2OUvQ4dqxQXU+ouZFQsZOx50z4= gonum.org/v1/gonum v0.17.0/go.mod h1:El3tOrEuMpv2UdMrbNlKEh9vd86bmQ6vqIcDwxEOc1E= google.golang.org/genproto v0.0.0-20260114163908-3f89685c29c3 h1:rUamZFBwsWVWg4Yb7iTbwYp81XVHUvOXNdrFCoYRRNE= google.golang.org/genproto v0.0.0-20260114163908-3f89685c29c3/go.mod h1:wE6SUYr3iNtF/D0GxVAjT+0CbDFktQNssYs9PVptCt4= -google.golang.org/genproto/googleapis/rpc v0.0.0-20250728155136-f173205681a0 h1:MAKi5q709QWfnkkpNQ0M12hYJ1+e8qYVDyowc4U1XZM= -google.golang.org/genproto/googleapis/rpc v0.0.0-20250728155136-f173205681a0/go.mod h1:qQ0YXyHHx3XkvlzUtpXDkS29lDSafHMZBAZDc03LQ3A= google.golang.org/genproto/googleapis/rpc v0.0.0-20251222181119-0a764e51fe1b h1:Mv8VFug0MP9e5vUxfBcE3vUkV6CImK3cMNMIDFjmzxU= google.golang.org/genproto/googleapis/rpc v0.0.0-20251222181119-0a764e51fe1b/go.mod h1:j9x/tPzZkyxcgEFkiKEEGxfvyumM01BEtsW8xzOahRQ= -google.golang.org/grpc v1.74.2 h1:WoosgB65DlWVC9FqI82dGsZhWFNBSLjQ84bjROOpMu4= -google.golang.org/grpc v1.74.2/go.mod h1:CtQ+BGjaAIXHs/5YS3i473GqwBBa1zGQNevxdeBEXrM= google.golang.org/grpc v1.76.0 h1:UnVkv1+uMLYXoIz6o7chp59WfQUYA2ex/BXQ9rHZu7A= google.golang.org/grpc v1.76.0/go.mod h1:Ju12QI8M6iQJtbcsV+awF5a4hfJMLi4X0JLo94ULZ6c= -google.golang.org/protobuf v1.36.6 h1:z1NpPI8ku2WgiWnf+t9wTPsn6eP1L7ksHUlkfLvd9xY= -google.golang.org/protobuf v1.36.6/go.mod h1:jduwjTPXsFjZGTmRluh+L6NjiWu7pchiJ2/5YcXBHnY= google.golang.org/protobuf v1.36.11 h1:fV6ZwhNocDyBLK0dj+fg8ektcVegBBuEolpbTQyBNVE= google.golang.org/protobuf v1.36.11/go.mod h1:HTf+CrKn2C3g5S8VImy6tdcUvCska2kB7j23XfzDpco= gopkg.in/check.v1 v0.0.0-20161208181325-20d25e280405/go.mod h1:Co6ibVJAznAaIkqp8huTwlJQCZ016jof/cbN4VW5Yz0=