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/gosl.md b/docs/content/gosl.md index 3301e822..c03fb54b 100644 --- a/docs/content/gosl.md +++ b/docs/content/gosl.md @@ -1,6 +1,12 @@ -**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** (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://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 / 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). @@ -23,23 +29,32 @@ 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. -`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. +**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. -## Kernels +`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. + +## Kernel Each distinct compute kernel must be tagged with a `//gosl:kernel` comment directive, as in this example (from `examples/basic`): ```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,8 +83,8 @@ 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. -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. +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. @@ -92,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 @@ -142,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 @@ -149,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] @@ -177,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 +## 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. + +* 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. +* 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 @@ -218,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. @@ -232,21 +259,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 -## Performance + //gosl:wgsl + // nci += ncA + ncB // wgsl now matches Go + //gosl:end +``` + +## Random numbers: slrand + +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. -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 +## 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/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/physics.md b/docs/content/physics.md new file mode 100644 index 00000000..91fd9009 --- /dev/null +++ b/docs/content/physics.md @@ -0,0 +1,334 @@ ++++ +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/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). + +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() { + 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) + stY := 4*hsz.Y + x := -hsz.Y + + 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) + + for i := 1; i < params.NPendula; i++ { + clr := colors.Names[i%len(colors.Names)] + 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 = 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 + } +}) +``` + +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) + 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 `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 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 := 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) +``` + +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 + +{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.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)) +}) + +// 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 + 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(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)) +}) + +// variables to control +posX := float32(0) +posY := float32(0) +posZ := float32(0) +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.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) +}) + +func update() { + 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) +} + +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(1).SetEnforceStep(true) + sld.SendChangeOnInput() + sld.OnChange(func(e events.Event) { + update() + tx.UpdateRender() + }) + core.Bind(val, sld) +} + +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. + +## 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.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. + +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. 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. + +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 + +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. + +## 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. + +* `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.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). + +## Builder + +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). + +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`. + +## 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.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 [[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 + +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: + +* 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/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/docs/docs.go b/docs/docs.go index 9dff8684..8dfad369 100644 --- a/docs/docs.go +++ b/docs/docs.go @@ -11,17 +11,31 @@ 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() { b := core.NewBody("Cogent Lab") 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/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 diff --git a/go.mod b/go.mod index 7c7b0816..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.20250909222513-1cba37a21a69 + 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 @@ -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 @@ -50,16 +51,17 @@ 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 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 +75,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..f651c779 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 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= @@ -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= @@ -102,8 +104,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 +114,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= 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/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 27d39250..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 } @@ -82,8 +94,11 @@ 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()) } } else { if sst, is := ut.(*types.Struct); is { @@ -121,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) @@ -133,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) } @@ -148,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/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..681e2231 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 @@ -150,9 +156,12 @@ 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" "cogentcore.org/lab/gosl/sltype" + "cogentcore.org/lab/gosl/slvec" "cogentcore.org/lab/tensor" `)) for impath := range st.GoImports { @@ -167,16 +176,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("(") @@ -202,43 +204,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/gengpu.go b/gosl/gotosl/gengpu.go index bc8e1d94..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,19 +47,36 @@ import ( "cogentcore.org/lab/tensor" ) -//go:embed %s/*.wgsl -var shaders embed.FS +%s 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 ) ` - 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) @@ -111,11 +130,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 +171,11 @@ func GPURelease() { } gpuRelease := ` - if ComputeGPU != nil { + if !BorrowedGPU && ComputeGPU != nil { ComputeGPU.Release() - ComputeGPU = nil + } + ComputeGPU = nil } ` @@ -162,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/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/gotosl.go b/gosl/gotosl/gotosl.go index 140bed50..5374a1eb 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 @@ -295,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 } @@ -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 99b1525e..5fe94dc1 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)) @@ -525,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 } } @@ -695,14 +697,34 @@ 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 { + 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 + 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", "MulVector", "MulVector3", "MulScalar": + opr = token.MUL + case "Div", "DivScalar": + opr = token.QUO + } + if opr == token.ILLEGAL { + return false + } + 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) + 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) } @@ -2308,6 +2412,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) @@ -2453,6 +2558,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)) @@ -2766,6 +2881,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) @@ -3105,8 +3227,8 @@ func (p *printer) funcDecl(d *ast.FuncDecl) { p.curMethRecv = nil if p.GoToSL.GetFuncGraph { p.GoToSL.FuncGraph[fname] = p.curFunc - p.curFunc = nil } + p.curFunc = nil } func (p *printer) decl(decl ast.Decl) { diff --git a/gosl/gotosl/sledits.go b/gosl/gotosl/sledits.go index 41718c49..b39b63af 100644 --- a/gosl/gotosl/sledits.go +++ b/gosl/gotosl/sledits.go @@ -39,6 +39,24 @@ 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("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.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.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/Compute.golden b/gosl/gotosl/testdata/Compute.golden index 75abc4e1..06c47c53 100644 --- a/gosl/gotosl/testdata/Compute.golden +++ b/gosl/gotosl/testdata/Compute.golden @@ -126,6 +126,15 @@ 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 = 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); + *oP = br+(aP); + *oQ = MulQuats(aQ, bQ); +} alias NeuronFlags = i32; const NeuronOff: NeuronFlags = 0x01; const NeuronHasExt: NeuronFlags = 0x02; // note: 1<<2 does NOT work @@ -154,6 +163,10 @@ 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 + Pos: vec4, + Rot: vec4, Subs: SubParamStruct, } fn ParamStruct_IntegFromRaw(ps: ParamStruct, idx: i32) -> f32 { @@ -165,15 +178,26 @@ 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; + 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 + 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 = MulQuatVector(oq, op); + 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, Index2D(TensorStrides[10], TensorStrides[11], u32(idx), u32(Exp)));return Data[Index2D(TensorStrides[0], TensorStrides[1], + 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))]; } 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; @@ -215,4 +239,31 @@ struct Context { fn Compute(i: u32) { //gosl:kernel let params = Params[0]; ParamStruct_IntegFromRaw(params, i32(i)); +} + +//////// import: "slmath-math.go" +const Pi = 3.141592653589793; + +//////// 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 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 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 2f79bc09..5e83c868 100644 --- a/gosl/gotosl/testdata/CycleUpdt.golden +++ b/gosl/gotosl/testdata/CycleUpdt.golden @@ -44,6 +44,10 @@ 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 + Pos: vec4, + Rot: vec4, Subs: SubParamStruct, } struct Context { @@ -60,4 +64,15 @@ 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-math.go" +const Pi = 3.141592653589793; + +//////// import: "slmath-matrix3.go" + +//////// import: "slmath-quaternion.go" + +//////// import: "slmath-vector2.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 9d65206d..d68613bf 100644 --- a/gosl/gotosl/testdata/basic.go +++ b/gosl/gotosl/testdata/basic.go @@ -8,10 +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 ( @@ -62,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.MulQuatVector(xQ, p) + 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.MulQuatVector(aQ, bP) + *oP = br.Add(aP) + *oQ = slmath.MulQuats(aQ, bQ) +} + // NeuronFlags are bit-flags encoding relevant binary state for neurons type NeuronFlags int32 @@ -126,6 +144,11 @@ type ParamStruct struct { pad float32 // comment this out to trigger alignment warning + VXYf slvec.Vector2 // translates to vec4 + VXYi slvec.Vector2i // translates to vec4 + Pos slvec.Vector3 + Rot math32.Quat + // extra parameters Subs SubParamStruct } @@ -140,11 +163,25 @@ 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 + Data.Values[idx*2] = 55.0 + + a := ps.VXYf.X + 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.MulQuatVector(oq, op) + d = TransformPoint(op, oq, d) + ctx := GetCtx(0) ps.AnotherMeth(ctx, idx, &a) bv := Big.Value(int(idx), int(Integ)) - Big.Set(bv*2, int(idx), int(Exp)) + //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 54ea3d69..fc55e624 100644 --- a/gosl/gotosl/testdata/basic.goal +++ b/gosl/gotosl/testdata/basic.goal @@ -5,10 +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 ( @@ -55,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.MulQuatVector(xQ, p) + 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.MulQuatVector(aQ, bP) + *oP = br.Add(aP) + *oQ = slmath.MulQuats(aQ, bQ) +} + // NeuronFlags are bit-flags encoding relevant binary state for neurons type NeuronFlags int32 @@ -119,6 +137,11 @@ type ParamStruct struct { pad float32 // comment this out to trigger alignment warning + VXYf slvec.Vector2 // translates to vec4 + VXYi slvec.Vector2i // translates to vec4 + Pos slvec.Vector3 + Rot math32.Quat + // extra parameters Subs SubParamStruct } @@ -133,11 +156,25 @@ func (ps *ParamStruct) IntegFromRaw(idx int) float32 { integ += newVal Data[idx, Integ] = integ Data[idx, Exp] = math32.Exp(-integ) - var a float32 + Data.Values[idx*2] = 55.0 + + a := ps.VXYf.X + 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.MulQuatVector(oq, op) + d = TransformPoint(op, oq, d) + ctx := GetCtx(0) ps.AnotherMeth(ctx, idx, &a) bv := Big[idx, Integ] - Big[idx, Exp] = bv * 2 + //gosl:wgsl + // bv *= 2 // will be uncommented in wgsl + //gosl:end + Big[idx, Exp] = bv*2 + c.Y + d.Z return Data[idx, Exp] } @@ -169,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/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/gotosl/translate.go b/gosl/gotosl/translate.go index 7cb90285..44e87757 100644 --- a/gosl/gotosl/translate.go +++ b/gosl/gotosl/translate.go @@ -42,12 +42,6 @@ func (st *State) TranslateDir(pf string) error { // return nil, err files := pkg.GoFiles - serr := alignsl.CheckPackage(pkg) - - if serr != nil { - fmt.Println(serr) - } - st.FuncGraph = make(map[string]*Function) st.GetFuncGraph = true @@ -84,11 +78,24 @@ 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" { + if !st.Config.Keep { + os.Remove(gofp) + } + continue + } var buf bytes.Buffer doFile(gofp, &buf) } @@ -150,6 +157,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 @@ -166,6 +176,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...) @@ -182,6 +195,18 @@ func (st *State) TranslateDir(pf string) error { if nOverBase > 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/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/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/math.go b/gosl/slmath/math.go new file mode 100644 index 00000000..97794d91 --- /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, dealing with the wrap-around issues with angles. +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/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 new file mode 100644 index 00000000..a753ef39 --- /dev/null +++ b/gosl/slmath/quaternion.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 slmath + +import "cogentcore.org/core/math32" + +//gosl:start + +// 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) +} + +// QuatNormalize normalizes the quaternion. +func QuatNormalize(q math32.Quat) math32.Quat { + nq := q + 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 := 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 := Cross3(xyz, v).MulScalar(2) + return v.Sub(t.MulScalar(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 + 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 +} + +// 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 + *oP = MulQuatVector(aQ, bP).Add(aP) + *oQ = MulQuats(aQ, bQ) +} + +// 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 SpatialTransformInverse(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 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 + 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/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 new file mode 100644 index 00000000..aee4cac3 --- /dev/null +++ b/gosl/slmath/vector3.go @@ -0,0 +1,134 @@ +// 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 + +// 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) +} + +// 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 +} + +// 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)) +} + +// 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) +} + +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 == 2 { + nv.Z = val + } + return nv +} + +//gosl:end 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..26cc2ce6 --- /dev/null +++ b/gosl/slvec/slvec.go @@ -0,0 +1,90 @@ +// 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, which are converted to direct ops in WGSL. +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 +} + +// 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/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/README.md b/physics/README.md new file mode 100644 index 00000000..90db1e8a --- /dev/null +++ b/physics/README.md @@ -0,0 +1,20 @@ +# 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). + +See [physics docs](https://cogentcore.org/lab/physics) for the main docs. + +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 + +* 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 new file mode 100644 index 00000000..94e55cf4 --- /dev/null +++ b/physics/body.go @@ -0,0 +1,284 @@ +// 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 + +import ( + "math" + + "cogentcore.org/core/math32" +) + +//gosl:start + +// BodyVars are body state variables stored in tensor.Float32 +type BodyVars int32 //enums:enum + +const ( + // BodyShape is the shape type of the object, as a Shapes type. + BodyShape BodyVars = iota + + // 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. + // NewBody uses [World.CurrentWorld] to initialize. + 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. 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 + + // 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 + + // 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. + BodyMass + + // BodyInvMass is 1/mass of the object or 0 if no mass. + BodyInvMass + + // BodyBounce specifies the COR or coefficient of restitution (0..1), + // which determines how elastic the collision is, + // i.e., final velocity / initial velocity. + BodyBounce + + // 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 + BodyPosZ + + // Quaternion rotation of body. + BodyQuatX + BodyQuatY + BodyQuatZ + BodyQuatW + + // Relative center-of-mass offset from 3D position of body. + BodyComX + BodyComY + BodyComZ + + // Inertia 3x3 matrix (column matrix organization, r,c labels). + BodyInertiaXX + BodyInertiaYX + BodyInertiaZX + BodyInertiaXY + BodyInertiaYY + BodyInertiaZY + BodyInertiaXZ + BodyInertiaYZ + BodyInertiaZZ + + // InvInertia inverse inertia 3x3 matrix (column matrix organization, r,c labels). + BodyInvInertiaXX + BodyInvInertiaYX + BodyInvInertiaZX + BodyInvInertiaXY + BodyInvInertiaYY + BodyInvInertiaZY + BodyInvInertiaXZ + BodyInvInertiaYZ + BodyInvInertiaZZ + + // radius for broadphase collision + BodyRadius +) + +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(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)))) +} + +// 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)) +} + +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. 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) { + 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 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 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 { + 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) { + 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 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 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)) +} + +// 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))) +} + +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)) +} + +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))) +} + +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)) + } +} + +// 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. +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 new file mode 100644 index 00000000..f6e86b03 --- /dev/null +++ b/physics/body.goal @@ -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 ( + "math" + + "cogentcore.org/core/math32" +) + +//gosl:start + +// BodyVars are body state variables stored in tensor.Float32 +type BodyVars int32 //enums:enum + +const ( + // BodyShape is the shape type of the object, as a Shapes type. + BodyShape BodyVars = iota + + // 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. + // NewBody uses [World.CurrentWorld] to initialize. + 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. 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 + + // 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 + + // 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. + BodyMass + + // BodyInvMass is 1/mass of the object or 0 if no mass. + BodyInvMass + + // BodyBounce specifies the COR or coefficient of restitution (0..1), + // which determines how elastic the collision is, + // i.e., final velocity / initial velocity. + BodyBounce + + // 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 + BodyPosZ + + // Quaternion rotation of body. + BodyQuatX + BodyQuatY + BodyQuatZ + BodyQuatW + + // Relative center-of-mass offset from 3D position of body. + BodyComX + BodyComY + BodyComZ + + // Inertia 3x3 matrix (column matrix organization, r,c labels). + BodyInertiaXX + BodyInertiaYX + BodyInertiaZX + BodyInertiaXY + BodyInertiaYY + BodyInertiaZY + BodyInertiaXZ + BodyInertiaYZ + BodyInertiaZZ + + // InvInertia inverse inertia 3x3 matrix (column matrix organization, r,c labels). + BodyInvInertiaXX + BodyInvInertiaYX + BodyInvInertiaZX + BodyInvInertiaXY + BodyInvInertiaYY + BodyInvInertiaZY + BodyInvInertiaXZ + BodyInvInertiaYZ + BodyInvInertiaZZ + + // radius for broadphase collision + BodyRadius +) + +func GetBodyShape(idx int32) Shapes { + return Shapes(math.Float32bits(Bodies[idx, BodyShape])) +} + +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])) +} + +// 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)) +} + +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. 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) { + Bodies[idx, BodyGroup] = math.Float32frombits(uint32(w)) +} + +func GetBodyGroup(idx int32) int32 { + return int32(math.Float32bits(Bodies[idx, BodyGroup])) +} + +func BodyHSize(idx int32) math32.Vector3 { + return math32.Vec3(Bodies[idx, BodyHSizeX], Bodies[idx, BodyHSizeY], Bodies[idx, BodyHSizeZ]) +} + +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 { + return math32.Vec3(Bodies[idx, BodyPosX], Bodies[idx, BodyPosY], Bodies[idx, BodyPosZ]) +} + +func SetBodyPos(idx int32, pos math32.Vector3) { + Bodies[idx, BodyPosX] = pos.X + Bodies[idx, BodyPosY] = pos.Y + Bodies[idx, BodyPosZ] = pos.Z +} + +func BodyQuat(idx int32) math32.Quat { + return math32.NewQuat(Bodies[idx, BodyQuatX], Bodies[idx, BodyQuatY], Bodies[idx, BodyQuatZ], Bodies[idx, BodyQuatW]) +} + +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 +} + +// 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]) +} + +func SetBodyCom(idx int32, pos math32.Vector3) { + Bodies[idx, BodyComX] = pos.X + Bodies[idx, BodyComY] = pos.Y + 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]) +} + +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] + } +} + +// 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. +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/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/body.go b/physics/builder/body.go new file mode 100644 index 00000000..e10af34b --- /dev/null +++ b/physics/builder/body.go @@ -0,0 +1,200 @@ +// 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 { + // 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 + + // 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 + + // 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. + 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 + + // Optional [phyxyz.Skin] for visualizing the body. + Skin *phyxyz.Skin + + // BodyIndex is the index of this body in the [physics.Model] Bodies list, + // once built. + BodyIndex int32 + + // 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{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 + 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 + bd.Group = 1 + 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) + bd.Group = -1 // default static + 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]. +// 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 + bd.Group = 1 + 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) { + var bi, di int32 + if bd.Dynamic { + bi, di = ml.NewDynamic(bd.Shape, bd.Mass, bd.HSize, bd.Pose.Pos, bd.Pose.Quat) + } else { + bi = ml.NewBody(bd.Shape, bd.HSize, bd.Pose.Pos, bd.Pose.Quat) + 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 + } + 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) +} + +// 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.Next, bd.Pose.Pos) + physics.SetDynamicQuat(bd.DynamicIndex, params.Next, bd.Pose.Quat) + } else { + 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/builder.go b/physics/builder/builder.go new file mode 100644 index 00000000..1211df68 --- /dev/null +++ b/physics/builder/builder.go @@ -0,0 +1,203 @@ +// 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 + +//go:generate core generate -add-types -setters + +import ( + "cogentcore.org/core/math32" + "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 { + // 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 { + 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 + 1 + } + 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) { + 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 _, bd := range ob.Bodies { + bd.NewPhysicsBody(ml, wl.World) + if bl.ReplicasN > 0 && wi == bl.ReplicasStart { + bN++ + if bSt < 0 { + bSt = bd.BodyIndex + } + } + } + if len(ob.Joints) == 0 { + continue + } + 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 bN > 0 { + ml.ReplicasN = int32(bl.ReplicasN) + ml.ReplicaBodiesStart = bSt + ml.ReplicaBodiesN = bN + ml.ReplicaJointsStart = jSt + ml.ReplicaJointsN = jN + } +} + +// 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 _, wl := range bl.Worlds { + for _, ob := range wl.Objects { + ob.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. +// 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, 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 { + continue + } + nw := bl.NewWorld() + wi := nw.WorldIndex + nw.Copy(src) + nw.SetWorldIndex(wi) + off := Yoff.MulScalar(float32(y)).Add(Xoff.MulScalar(float32(x))) + nw.Move(off) + if sc != nil { + nw.CopySkins(sc, src) + } + } + } + bl.ReplicasStart = worldIdx + 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 { + 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/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/builder/joint.go b/physics/builder/joint.go new file mode 100644 index 00000000..5d695e5d --- /dev/null +++ b/physics/builder/joint.go @@ -0,0 +1,416 @@ +// 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 { + // 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 + + // 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 + + // 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 + + // AngularDoFN is the number of linear degrees of freedom (3 max). + AngularDoFN int + + // 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 +} + +// 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. + Axis math32.Vector3 + + // Limit has the limits for motion of this DoF. + Limit minmax.F32 + + // Init are the initial control values. + Init Controls + + // Current are the current control values (based on method calls). + Current Controls +} + +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 + +} + +func (jd *Joint) DoF(idx int) *DoF { + 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. +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.ObjectBody + } + idx := len(ob.Joints) + 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() + jd.CPose.Pos = cpos + jd.CPose.Quat = math32.NewQuatIdentity() + ndof := linDoF + angDoF + if ndof > 0 { + jd.DoFs = make([]*DoF, linDoF+angDoF) + for i := range ndof { + dof := &DoF{} + jd.DoFs[i] = dof + dof.Defaults() + } + } + return jd +} + +// 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 { + 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 +// 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 { + 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 +// 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 { + 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 +// 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 { + jd := ob.newJoint(physics.Ball, parent, child, ppos, cpos, 0, 3) + return jd +} + +// 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 { + 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 +// 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(parent, child *Body, ppos, cpos math32.Vector3) *Joint { + jd := ob.newJoint(physics.Free, parent, child, ppos, cpos, 0, 0) + return jd +} + +// 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. +// 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 { + jd := ob.newJoint(physics.PlaneXZ, parent, child, ppos, cpos, 2, 1) + jd.NoLinearRotation = true + return jd +} + +// 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.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: + 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) + } + physics.SetJointParentFixed(ji, jd.ParentFixed) + physics.SetJointNoLinearRotation(ji, jd.NoLinearRotation) + 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.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 { + 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.Init.Pos, d.Init.Stiff) + physics.SetJointTargetVel(ji, di, d.Init.Vel, d.Init.Damp) + d.Axis = physics.JointAxis(ji, di) + } + jd.JointIndex = ji + // fmt.Printf("\tjoint: %p %d\n", jd, jd.JointIndex) + // if pdi < 0 { + // fmt.Println("\t\t\t", jd.PPose.Pos) + // } + return ji +} + +// IsGlobal returns true if this joint has a global world anchor parent. +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() { + if !jd.IsGlobal() { + return + } + physics.SetJointPPos(jd.JointIndex, jd.PPose.Pos) + 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) +} + +// 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. 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 Current target position for given DoF for +// this joint in the physics model, setting stiffness. +func (jd *Joint) AddTargetPos(dof int32, pos, stiff float32) { + d := jd.DoF(int(dof)) + d.Current.Pos += pos + d.Current.Stiff = stiff + physics.SetJointTargetPos(jd.JointIndex, dof, d.Current.Pos, stiff) +} + +// 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 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(ang, delta, stiff float32) { + 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 new file mode 100644 index 00000000..c02d4d86 --- /dev/null +++ b/physics/builder/object.go @@ -0,0 +1,185 @@ +// 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 ( + "slices" + + "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 +// 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 + + // 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 { + return ob.Bodies[idx] +} + +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.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)) + for i := range ob.Bodies { + 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)) + } + copy(ob.Sensors, so.Sensors) +} + +// 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) + } +} + +// InitState initializes current state variables in the object. +func (ob *Object) 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 _, bd := range ob.Bodies { + bd.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 _, bd := range ob.Bodies { + bd.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 _, bd := range ob.Bodies { + bd.Pose.Move(pos) + } + for _, jd := range ob.Joints { + if jd.IsGlobal() { + jd.PPose.Move(pos) + } + } +} + +// RotateAround rotates around a given point +func (ob *Object) RotateAround(rot math32.Quat, around math32.Vector3) { + for _, bd := range ob.Bodies { + bd.Pose.RotateAround(rot, around) + } + for _, jd := range ob.Joints { + 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) +} + +//////// 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/physics.go b/physics/builder/physics.go new file mode 100644 index 00000000..be417527 --- /dev/null +++ b/physics/builder/physics.go @@ -0,0 +1,63 @@ +// 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) + if ph.Scene != nil { + ph.Scene.Init(ph.Model) + } +} + +// InitState calls Scene.InitState or Model.InitState and Builder InitState. +func (ph *Physics) InitState() { + if ph.Scene != nil { + 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. +func (ph *Physics) Step(n int) { + for range n { + ph.Model.Step() + if ph.Scene != nil { + ph.Scene.Update() + } + } +} + +// StepQuiet advances the physics world n steps. +func (ph *Physics) StepQuiet(n int) { + for range n { + ph.Model.Step() + } +} diff --git a/physics/builder/pose.go b/physics/builder/pose.go new file mode 100644 index 00000000..5b65191c --- /dev/null +++ b/physics/builder/pose.go @@ -0,0 +1,148 @@ +// 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 builder + +import ( + "cogentcore.org/core/core" + "cogentcore.org/core/icons" + "cogentcore.org/core/math32" + "cogentcore.org/core/tree" +) + +// 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 +} + +// Defaults sets defaults only if current values are nil +func (ps *Pose) Defaults() { + if ps.Quat.IsNil() { + ps.Quat.SetIdentity() + } +} + +// Transform applies positional and rotational transform to pose. +func (ps *Pose) Transform(pos math32.Vector3, rot math32.Quat) { + ps.Pos = rot.MulVector(ps.Pos).Add(pos) + ps.Quat = rot.Mul(ps.Quat) +} + +//////// 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 *Pose) Move(delta math32.Vector3) { + 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 *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 *Pose) MoveOnAxisAbs(x, y, z, dist float32) { //types:add + delta := math32.Vec3(x, y, z).Normal().MulScalar(dist) + ps.Pos.SetAdd(delta) +} + +//////// 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)) +} + +// SetEulerRotationRad sets the rotation in Euler angles (radians). +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 *Pose) EulerRotation() math32.Vector3 { //types:add + return ps.Quat.ToEuler().MulScalar(math32.RadToDegFactor) +} + +// EulerRotationRad returns the current rotation in Euler angles (radians). +func (ps *Pose) EulerRotationRad() math32.Vector3 { + return ps.Quat.ToEuler() +} + +// SetAxisRotation sets rotation from local axis and angle in degrees. +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 *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 *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 *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 *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 *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/typegen.go b/physics/builder/typegen.go new file mode 100644 index 00000000..018adff3 --- /dev/null +++ b/physics/builder/typegen.go @@ -0,0 +1,310 @@ +// 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: "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."}}}) + +// 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. +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."}, {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 } + +// 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: "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. +// -1 for world-anchored parent. +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 } + +// 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 } + +// 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.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."}}}) + +// 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 } + +// 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 *Controls) SetPos(v float32) *Controls { t.Pos = v; return t } + +// 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 *Controls) SetStiff(v float32) *Controls { t.Stiff = v; return t } + +// 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 *Controls) SetVel(v float32) *Controls { t.Vel = v; return t } + +// 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 *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: "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. +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 } + +// 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]: +// 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]: +// 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 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 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]: +// 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 new file mode 100644 index 00000000..b4603e1f --- /dev/null +++ b/physics/builder/world.go @@ -0,0 +1,91 @@ +// 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/phyxyz" +) + +// World is one world within the Builder. +type World 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 `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 + // 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{World: wl.World, WorldIndex: wl.WorldIndex, Object: idx}) + 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.Objects[i] = &Object{} + 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, ob := range wl.Objects { + ob.CopySkins(sc, ow.Object(i)) + } +} + +// 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 { + 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 _, ob := range wl.Objects { + 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/config.go b/physics/config.go new file mode 100644 index 00000000..ded970f4 --- /dev/null +++ b/physics/config.go @@ -0,0 +1,107 @@ +// 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 + +import ( + // "fmt" + + "cogentcore.org/lab/tensor" +) + +// Config does final configuration prior to running +// after everything has been added. Does SetAsCurrent, GPUInit. +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 (ml *Model) ConfigJoints() { + // accumulate parent and child joints per dynamic + params := &ml.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) + // 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])) + } + bjc[jci] = append(bjc[jci], ji) + maxi = max(maxi, len(bjc[jci])) + } + params.BodyJointsMax = int32(maxi) + if nd == 0 { + nd = 1 + } + if maxi == 0 { + maxi = 1 + } + ml.BodyJoints.SetShapeSizes(int(nd), 2, maxi+1) + for di := range nd { + np := int32(len(bjp[di])) + ml.BodyJoints.Set(np, int(di), int(0), int(0)) + for i, ji := range bjp[di] { + ml.BodyJoints.Set(ji, int(di), int(0), int(1+i)) + } + nc := int32(len(bjc[di])) + ml.BodyJoints.Set(nc, int(di), int(1), int(0)) + for i, ji := range bjc[di] { + ml.BodyJoints.Set(ji, int(di), int(1), int(1+i)) + } + } + 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)) + } +} + +// ConfigBodies updates computed body values from current values. +// Call if body params (mass, size) change. +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)) + ml.SetMass(bi, shape, size, mass) + } +} + +// 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 new file mode 100644 index 00000000..e1972c10 --- /dev/null +++ b/physics/config.goal @@ -0,0 +1,107 @@ +// 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" + + "cogentcore.org/lab/tensor" +) + +// Config does final configuration prior to running +// after everything has been added. Does SetAsCurrent, GPUInit. +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 (ml *Model) ConfigJoints() { + // accumulate parent and child joints per dynamic + params := &ml.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) + // 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])) + } + bjc[jci] = append(bjc[jci], ji) + maxi = max(maxi, len(bjc[jci])) + } + params.BodyJointsMax = int32(maxi) + if nd == 0 { + nd = 1 + } + if maxi == 0 { + maxi = 1 + } + ml.BodyJoints.SetShapeSizes(int(nd), 2, maxi+1) + for di := range nd { + np := int32(len(bjp[di])) + ml.BodyJoints[di, 0, 0] = np + for i, ji := range bjp[di] { + ml.BodyJoints[di, 0, 1+i] = ji + } + nc := int32(len(bjc[di])) + ml.BodyJoints[di, 1, 0] = nc + for i, ji := range bjc[di] { + ml.BodyJoints[di, 1, 1+i] = ji + } + } + 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)) + } +} + +// ConfigBodies updates computed body values from current values. +// Call if body params (mass, size) change. +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] + ml.SetMass(bi, shape, size, mass) + } +} + +// 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/contact.go b/physics/contact.go new file mode 100644 index 00000000..04b9ae3a --- /dev/null +++ b/physics/contact.go @@ -0,0 +1,823 @@ +// Code generated by "goal build"; DO NOT EDIT. +//line contact.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 ( + // "fmt" + "math" + "sync/atomic" + + "cogentcore.org/core/math32" + "cogentcore.org/lab/gosl/slmath" + "cogentcore.org/lab/tensor" +) + +//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 index + ContactA ContactVars = iota + + // the other body index + ContactB + + // contact point index for A-B pair + ContactPointIdx + + // contact point on body A + ContactAPointX + ContactAPointY + ContactAPointZ + + // contact point on body B + ContactBPointX + ContactBPointY + ContactBPointZ + + // 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 + 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 + ContactADeltaZ + + ContactAAngDeltaX + ContactAAngDeltaY + ContactAAngDeltaZ + + // computed contact deltas, B + ContactBDeltaX + ContactBDeltaY + ContactBDeltaZ + + ContactBAngDeltaX + ContactBAngDeltaY + ContactBAngDeltaZ +) + +// 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)) +} + +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 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))) +} + +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 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 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 { + 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 +} + +// newton: geometry/kernels.py: broadphase_collision_pairs + +// CollisionBroad performs broad-phase collision detection, generating Contacts. +func CollisionBroad(i uint32) { //gosl:kernel + params := GetParams(0) + ci := int32(i) + if ci >= params.BodyCollidePairsN { + return + } + biA := BodyCollidePairs.Value(int(ci), int(0)) + biB := BodyCollidePairs.Value(int(ci), int(1)) + + xwAR := BodyDynamicPos(biA, params.Cur) + xwAQ := BodyDynamicQuat(biA, params.Cur) + xwBR := BodyDynamicPos(biB, params.Cur) + // xwBQ := BodyDynamicQuat(bb, params.Cur) + + // note: sA <= sB + sA := GetBodyShape(biA) + sB := GetBodyShape(biB) + + rb := Bodies.Value(int(biB), 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]) + margin := params.ContactMargin + + // bounding sphere check + infPlane := false + if sA == Plane { + szA := BodyHSize(biA) + if szA.X == 0 { + infPlane = true + } + queryB := slmath.MulSpatialPoint(xwAR, xwAQ, xwBR) + closest := ClosestPointPlane(szA.X, szA.Z, queryB) + d := slmath.Length3(queryB.Sub(closest)) + 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)) + if d > ra+rb+margin { + return + } + } + var ncB int32 + ncA := ShapePairContacts(sA, sB, infPlane, &ncB) + + // note: ignoring contact_point_limit code for now + + enci := atomic.AddInt32(&BroadContactsN.Values[0], ncA+ncB) + // Go returns post-added value, while WGSL returns pre-added value + + //gosl:wgsl + // enci += ncA + ncB // wgsl now matches Go + //gosl:end + + 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) +} + +// newton: geometry/kernels.py: allocate_contact_points + +// AddBroadContacts adds broad-phase contact records in prep for narrow phase. +func AddBroadContacts(biA, biB, nci, ncA, ncB int32) { + for i := range ncA { + SetBroadContactA(nci+i, biA) + SetBroadContactB(nci+i, biB) + SetBroadContactPointIdx(nci+i, i) + } + for i := range ncB { + SetBroadContactA(nci+ncA+i, biB) // flipped + SetBroadContactB(nci+ncA+i, biA) + SetBroadContactPointIdx(nci+i, i) + } +} + +// 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) + cmax := BroadContactsN.Values[0] + if ci >= cmax { + return + } + biA := GetBroadContactA(ci) + biB := GetBroadContactB(ci) + cpi := GetBroadContactPointIdx(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 + dist := float32(1.0e6) + 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, 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)) +} + +// 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) + + r1A := BodyDynamicPos(biA, params.Next) + q1A := BodyDynamicQuat(biA, 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(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 + nnorm := ContactNorm(ci) + norm := slmath.Negate3(nnorm) + // margin := params.ContactMargin + + d := slmath.Dot3(norm, ctBw.Sub(ctAw)) - thick + 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) + SetContactBDelta(ci, z) + SetContactAAngDelta(ci, z) + SetContactBAngDelta(ci, z) + 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 w1A, w1B math32.Vector3 + if diA >= 0 { + w1A = DynamicAngDelta(diA, params.Next) + } + if diB >= 0 { + 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(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, q1A, q1B, 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) + ctAm := ctAw.Add(slmath.MulQuatVector(q1A, offA)) + ctBm := ctBw.Add(slmath.MulQuatVector(q1B, offB)) + + // update delta + delta := ctBm.Sub(ctAm) + frDelta := delta.Sub(norm.MulScalar(slmath.Dot3(norm, delta))) + + perp := slmath.Normal3(frDelta) + + 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) + + if err > 0.0 { + 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 + 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 := 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, 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)) + 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, 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, norm)) // 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 + // fmt.Println(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 := norm.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) + SetContactAAngDelta(ci, angDeltaA) + SetContactBAngDelta(ci, angDeltaB) +} + +// 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.Value(int(bi), int(BodyInvMass)) + if invMass == 0 { + return // no updates + } + cmax := ContactsN.Values[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 { // 0 = no actual; else 1 + continue + } + biA := GetContactA(ci) + biB := GetContactB(ci) + if biA == bi { + tw += wt + d := ContactADelta(ci) + linDel = linDel.Add(d) + a := ContactAAngDelta(ci) + angDel = angDel.Add(a) + } + if biB == bi { + tw += wt + d := ContactBDelta(ci) + linDel = linDel.Add(d) + a := ContactBAngDelta(ci) + angDel = angDel.Add(a) + } + } + 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 { + 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 (ml *Model) IsChildDynamic(dip, dic int32) bool { + if dip < 0 || dic < 0 { + return false + } + npja := ml.BodyJoints.Value(int(dip), int(0), int(0)) + for j := range npja { + ji := ml.BodyJoints.Value(int(dip), int(0), int(1+j)) + jci := JointChildIndex(ji) + if jci == dic { + return true + } + } + 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, +// so that shapes are in a canonical order. +func (ml *Model) ConfigBodyCollidePairs() { + params := &ml.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 { + if a == b { + continue + } + 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 ml.IsChildDynamic(dia, dib) || ml.IsChildDynamic(dib, dia) { + continue + } + if np >= nalc { + nalc += int(nb) + pt.SetShapeSizes(nalc, 2) + // fmt.Println("body pairs realoc", nalc) + } + + 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++ + } + } + params.BodyCollidePairsN = int32(np) + if np == 0 { + np = 1 + } + pt.SetShapeSizes(np, 2) + ml.BodyCollidePairs = pt + BodyCollidePairs = pt + // fmt.Println("body pairs over alloc", nalc - np, "total:", np) +} + +// newton: geometry/kernels.py: count_contact_points + +// SetMaxContacts computes [Params.MaxContacts] based on current list of +// [BodyCollidePairs]. +func (ml *Model) SetMaxContacts() { + params := &ml.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 := BodyHSize(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 = 4 * max(int32(math32.Sqrt(float32(n))), int32(Bodies.DimSize(0))) + // fmt.Println("> 1000", params.BodyCollidePairsN, n) + } + 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 new file mode 100644 index 00000000..514d8f92 --- /dev/null +++ b/physics/contact.goal @@ -0,0 +1,823 @@ +// 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" + "math" + "sync/atomic" + + "cogentcore.org/core/math32" + "cogentcore.org/lab/gosl/slmath" + "cogentcore.org/lab/tensor" +) + +//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 index + ContactA ContactVars = iota + + // the other body index + ContactB + + // contact point index for A-B pair + ContactPointIdx + + // contact point on body A + ContactAPointX + ContactAPointY + ContactAPointZ + + // contact point on body B + ContactBPointX + ContactBPointY + ContactBPointZ + + // 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 + 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 + ContactADeltaZ + + ContactAAngDeltaX + ContactAAngDeltaY + ContactAAngDeltaZ + + // computed contact deltas, B + ContactBDeltaX + ContactBDeltaY + ContactBDeltaZ + + ContactBAngDeltaX + ContactBAngDeltaY + ContactBAngDeltaZ +) + +// 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)) +} + +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 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]) +} + +func SetContactNorm(idx int32, pos math32.Vector3) { + Contacts[idx, ContactNormX] = pos.X + Contacts[idx, ContactNormY] = pos.Y + Contacts[idx, ContactNormZ] = pos.Z +} + +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 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 { + 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 +} + +// newton: geometry/kernels.py: broadphase_collision_pairs + +// CollisionBroad performs broad-phase collision detection, generating Contacts. +func CollisionBroad(i uint32) { //gosl:kernel + params := GetParams(0) + ci := int32(i) + if ci >= params.BodyCollidePairsN { + return + } + biA := BodyCollidePairs[ci, 0] + biB := BodyCollidePairs[ci, 1] + + xwAR := BodyDynamicPos(biA, params.Cur) + xwAQ := BodyDynamicQuat(biA, params.Cur) + xwBR := BodyDynamicPos(biB, params.Cur) + // xwBQ := BodyDynamicQuat(bb, params.Cur) + + // note: sA <= sB + sA := GetBodyShape(biA) + sB := GetBodyShape(biB) + + rb := Bodies[biB, 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 := BodyHSize(biA) + if szA.X == 0 { + infPlane = true + } + queryB := slmath.MulSpatialPoint(xwAR, xwAQ, xwBR) + closest := ClosestPointPlane(szA.X, szA.Z, queryB) + d := slmath.Length3(queryB.Sub(closest)) + 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] + if d > ra+rb+margin { + return + } + } + var ncB int32 + ncA := ShapePairContacts(sA, sB, infPlane, &ncB) + + // note: ignoring contact_point_limit code for now + + enci := atomic.AddInt32(&BroadContactsN.Values[0], ncA + ncB) + // Go returns post-added value, while WGSL returns pre-added value + + //gosl:wgsl + // enci += ncA + ncB // wgsl now matches Go + //gosl:end + + 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) +} + +// newton: geometry/kernels.py: allocate_contact_points + +// AddBroadContacts adds broad-phase contact records in prep for narrow phase. +func AddBroadContacts(biA, biB, nci, ncA, ncB int32) { + for i := range ncA { + SetBroadContactA(nci + i, biA) + SetBroadContactB(nci + i, biB) + SetBroadContactPointIdx(nci+i, i) + } + for i := range ncB { + SetBroadContactA(nci + ncA + i, biB) // flipped + SetBroadContactB(nci + ncA + i, biA) + SetBroadContactPointIdx(nci+i, i) + } +} + +// 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) + cmax := BroadContactsN.Values[0] + if ci >= cmax { + return + } + biA := GetBroadContactA(ci) + biB := GetBroadContactB(ci) + cpi := GetBroadContactPointIdx(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 + dist := float32(1.0e6) + 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, 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 +} + +// 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) + + r1A := BodyDynamicPos(biA, params.Next) + q1A := BodyDynamicQuat(biA, 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(r1A, q1A, ctA) + ctBw := slmath.MulSpatialPoint(r1B, q1B, ctB) + thickA := Contacts[ci, ContactAThick] + thickB := Contacts[ci, ContactBThick] + 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 { // todo: should this be margin or not? + 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) + mInvA := Bodies[biA, BodyInvMass] + iInvA := BodyInvInertia(biA) + + comB := BodyCom(biB) + mInvB := Bodies[biB, BodyInvMass] + iInvB := BodyInvInertia(biB) + + var w1A, w1B math32.Vector3 + if diA >= 0 { + w1A = DynamicAngDelta(diA, params.Next) + } + if diB >= 0 { + 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(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, q1A, q1B, 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) + ctAm := ctAw.Add(slmath.MulQuatVector(q1A, offA)) + ctBm := ctBw.Add(slmath.MulQuatVector(q1B, offB)) + + // update delta + delta := ctBm.Sub(ctAm) + frDelta := delta.Sub(norm.MulScalar(slmath.Dot3(norm, delta))) + + perp := slmath.Normal3(frDelta) + + 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) + + if err > 0.0 { + 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 + 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 := 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, 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)) + 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, 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, norm)) // 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 + // fmt.Println(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 := norm.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) + SetContactAAngDelta(ci, angDeltaA) + SetContactBAngDelta(ci, angDeltaB) +} + +// 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] + + 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 { // 0 = no actual; else 1 + continue + } + biA := GetContactA(ci) + biB := GetContactB(ci) + if biA == bi { + tw += wt + d := ContactADelta(ci) + linDel = linDel.Add(d) + a := ContactAAngDelta(ci) + angDel = angDel.Add(a) + } + if biB == bi { + tw += wt + d := ContactBDelta(ci) + linDel = linDel.Add(d) + a := ContactBAngDelta(ci) + angDel = angDel.Add(a) + } + } + 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 { + 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 (ml *Model) IsChildDynamic(dip, dic int32) bool { + if dip < 0 || dic < 0 { + return false + } + npja := ml.BodyJoints[dip, 0, 0] + for j := range npja { + ji := ml.BodyJoints[dip, 0, 1+j] + jci := JointChildIndex(ji) + if jci == dic { + return true + } + } + 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, +// so that shapes are in a canonical order. +func (ml *Model) ConfigBodyCollidePairs() { + params := &ml.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 { + if a == b { + continue + } + 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 ml.IsChildDynamic(dia, dib) || ml.IsChildDynamic(dib, dia) { + continue + } + if np >= nalc { + nalc += int(nb) + pt.SetShapeSizes(nalc, 2) + // fmt.Println("body pairs realoc", nalc) + } + + 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++ + } + } + params.BodyCollidePairsN = int32(np) + if np == 0 { + np = 1 + } + pt.SetShapeSizes(np, 2) + ml.BodyCollidePairs = pt + BodyCollidePairs = pt + // fmt.Println("body pairs over alloc", nalc - np, "total:", np) +} + +// newton: geometry/kernels.py: count_contact_points + +// SetMaxContacts computes [Params.MaxContacts] based on current list of +// [BodyCollidePairs]. +func (ml *Model) SetMaxContacts() { + params := &ml.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 := BodyHSize(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 = 4 * max(int32(math32.Sqrt(float32(n))), int32(Bodies.DimSize(0))) + // fmt.Println("> 1000", params.BodyCollidePairsN, n) + } + 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/control.go b/physics/control.go new file mode 100644 index 00000000..90873a57 --- /dev/null +++ b/physics/control.go @@ -0,0 +1,106 @@ +// 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 JointDoFs. +type JointControlVars int32 //enums:enum + +const ( + // Joint force and torque inputs + JointControlForce JointControlVars = iota + + // 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. + JointTargetStiff + + // 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 +) + +// SetJointControl sets the control for given joint, dof and parameter +// to given value. +func SetJointControl(idx, dof int32, vr JointControlVars, value float32) { + JointControls.Set(value, int(JointDoFIndex(idx, dof)), int(vr)) +} + +func JointControl(idx, dof int32, vr JointControlVars) float32 { + return JointControls.Value(int(JointDoFIndex(idx, dof)), int(vr)) +} + +// SetJointControlForce sets the force for given joint, dof to given value. +func SetJointControlForce(idx, dof int32, value float32) { + SetJointControl(idx, dof, JointControlForce, 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; 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 { + 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; 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 new file mode 100644 index 00000000..116d0034 --- /dev/null +++ b/physics/control.goal @@ -0,0 +1,104 @@ +// 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 JointDoFs. +type JointControlVars int32 //enums:enum + +const ( + // Joint force and torque inputs + JointControlForce JointControlVars = iota + + // 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. + JointTargetStiff + + // 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 +) + +// SetJointControl sets the control for given joint, dof and parameter +// to given value. +func SetJointControl(idx, dof int32, vr JointControlVars, value float32) { + JointControls[JointDoFIndex(idx, dof), vr] = value +} + +func JointControl(idx, dof int32, vr JointControlVars) float32 { + return JointControls[JointDoFIndex(idx, dof), vr] +} + +// SetJointControlForce sets the force for given joint, dof to given value. +func SetJointControlForce(idx, dof int32, value float32) { + SetJointControl(idx, dof, JointControlForce, 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; 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 { + 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; 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/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/physics/dynamics.go b/physics/dynamics.go new file mode 100644 index 00000000..fcc3d76a --- /dev/null +++ b/physics/dynamics.go @@ -0,0 +1,261 @@ +// 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 ( + // "fmt" + "math" + + "cogentcore.org/core/math32" + "cogentcore.org/lab/gosl/slmath" +) + +//gosl:start + +// DynamicVars are dynamic body variables stored in tensor.Float32. +type DynamicVars int32 //enums:enum + +const ( + // Index of body in list of bodies. + DynBody DynamicVars = iota + + // 3D position of structural center. + DynPosX + DynPosY + DynPosZ + + // Quaternion rotation. + DynQuatX + DynQuatY + DynQuatZ + DynQuatW + + // 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. These accumulate over time via StepBodyDeltas. + DynDeltaX + DynDeltaY + DynDeltaZ + + // Angular deltas. These accumulate over time via StepBodyDeltas. + DynAngDeltaX + DynAngDeltaY + DynAngDeltaZ + + // integrated weight of all contacts + DynContactWeight +) + +// 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 DynamicBody(idx int32) int32 { + return int32(math.Float32bits(Dynamics.Value(int(idx), int(0), int(DynBody)))) +} + +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 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 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 { + 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)) +} + +//////// 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))) +} + +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 + +// 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 (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 + 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()) +} + +// 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 +} + +// 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 +} + +// 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 new file mode 100644 index 00000000..212c51d3 --- /dev/null +++ b/physics/dynamics.goal @@ -0,0 +1,260 @@ +// 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" + "math" + + "cogentcore.org/core/math32" + "cogentcore.org/lab/gosl/slmath" +) + +//gosl:start + +// DynamicVars are dynamic body variables stored in tensor.Float32. +type DynamicVars int32 //enums:enum + +const ( + // Index of body in list of bodies. + DynBody DynamicVars = iota + + // 3D position of structural center. + DynPosX + DynPosY + DynPosZ + + // Quaternion rotation. + DynQuatX + DynQuatY + DynQuatZ + DynQuatW + + // 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. These accumulate over time via StepBodyDeltas. + DynDeltaX + DynDeltaY + DynDeltaZ + + // Angular deltas. These accumulate over time via StepBodyDeltas. + DynAngDeltaX + DynAngDeltaY + DynAngDeltaZ + + // integrated weight of all contacts + DynContactWeight +) + +// 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 DynamicBody(idx int32) int32 { + return int32(math.Float32bits(Dynamics[idx, 0, DynBody])) +} + +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 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 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 { + 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 +} + +//////// Accumulating deltas + +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 + +// 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 (ml *Model) 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 { + invm = 1.0 / mass + } + Bodies[idx, BodyInvMass] = invm + inertia := shape.Inertia(size, mass) + 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 +} + +// 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 +} + +// 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/enumgen.go b/physics/enumgen.go new file mode 100644 index 00000000..18b78860 --- /dev/null +++ b/physics/enumgen.go @@ -0,0 +1,422 @@ +// 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, 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 = 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, `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: `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: `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) } + +// 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 _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 = 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, `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: `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: `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) } + +// 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 _JointControlVarsValues = []JointControlVars{0, 1, 2, 3, 4, 5} + +// JointControlVarsN is the highest valid value for type JointControlVars, plus one. +// +//gosl:start +const JointControlVarsN JointControlVars = 6 + +//gosl:end + +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 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: `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) } + +// 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 _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 = 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, `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. 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`} + +// 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, 7, 8, 9, 10, 11, 12} + +// GPUVarsN is the highest valid value for type GPUVars, plus one. +// +//gosl:start +const GPUVarsN GPUVars = 13 + +//gosl:end + +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: ``, 12: ``} + +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) } + +// 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 _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 = 8 + +//gosl:end + +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) for movement.`} + +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) } + +// 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} + +// JointVarsN is the highest valid value for type JointVars, plus one. +// +//gosl:start +const JointVarsN JointVars = 46 + +//gosl:end + +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: `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: `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) } + +// 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 _JointDoFVarsValues = []JointDoFVars{0, 1, 2, 3, 4} + +// JointDoFVarsN is the highest valid value for type JointDoFVars, plus one. +// +//gosl:start +const JointDoFVarsN JointDoFVars = 5 + +//gosl:end + +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: ``} + +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) } + +// SetString sets the JointDoFVars value from its string representation, +// and returns an error if the string is invalid. +func (i *JointDoFVars) SetString(s string) error { + return enums.SetString(i, s, _JointDoFVarsValueMap, "JointDoFVars") +} + +// Int64 returns the JointDoFVars value as an int64. +func (i JointDoFVars) Int64() int64 { return int64(i) } + +// SetInt64 sets the JointDoFVars value from an int64. +func (i *JointDoFVars) SetInt64(in int64) { *i = JointDoFVars(in) } + +// Desc returns the description of the JointDoFVars value. +func (i JointDoFVars) Desc() string { return enums.Desc(i, _JointDoFVarsDescMap) } + +// JointDoFVarsValues returns all possible values for the type JointDoFVars. +func JointDoFVarsValues() []JointDoFVars { return _JointDoFVarsValues } + +// 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 JointDoFVars) MarshalText() ([]byte, error) { return []byte(i.String()), nil } + +// UnmarshalText implements the [encoding.TextUnmarshaler] interface. +func (i *JointDoFVars) UnmarshalText(text []byte) error { + return enums.UnmarshalText(i, text, "JointDoFVars") +} + +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 = 6 + +//gosl:end + +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 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`} + +// 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/balls/balls.go b/physics/examples/balls/balls.go new file mode 100644 index 00000000..299efa60 --- /dev/null +++ b/physics/examples/balls/balls.go @@ -0,0 +1,118 @@ +// 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 ( + "math/rand/v2" + + "cogentcore.org/core/colors" + "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" +) + +// 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() { + b := core.NewBody("test1").SetTitle("Physics Balls") + ed := phyxyz.NewEditor(b) + + 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 + // ml.ReportTotalKE = true + sc := ed.Scene + rot := math32.NewQuatIdentity() + 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 + sc.NewBody(ml, "back-wall", physics.Box, "#0000FFA0", math32.Vec3(hw, hh, ht), + math32.Vec3(0, hh, -hd), rot) + sc.NewBody(ml, "left-wall", physics.Box, "#FF0000A0", math32.Vec3(ht, hh, hd), + math32.Vec3(-hw, hh, 0), rot) + sc.NewBody(ml, "right-wall", physics.Box, "#00FF00A0", math32.Vec3(ht, hh, hd), + math32.Vec3(hw, hh, 0), rot) + sc.NewBody(ml, "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 { + 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 := 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.BodyIndex, int32(i+1)) // only collide within same group + } + bl.SetBodyBounce(bs.Bounce) + bl.SetBodyFriction(bs.Friction) + bl.SetBodyFrictionTortion(bs.FrictionTortion) + bl.SetBodyFrictionRolling(bs.FrictionRolling) + } + }) + + b.RunMainWindow() +} diff --git a/physics/examples/collision/collision.go b/physics/examples/collision/collision.go new file mode 100644 index 00000000..eb8d38db --- /dev/null +++ b/physics/examples/collision/collision.go @@ -0,0 +1,125 @@ +// 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 { + // 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 is for rotating. Not generally relevant here. + FrictionTortion float32 + + // FrictionRolling is for rolling: around 0.01 seems pretty realistic + FrictionRolling 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 + cl.PushMass = 1 + cl.Friction = 0.01 + cl.FrictionTortion = 0.01 + cl.FrictionRolling = 0.01 +} + +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 + // 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) + 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(0, 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() + 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/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/examples/pendula/pendula.go b/physics/examples/pendula/pendula.go new file mode 100644 index 00000000..8367c875 --- /dev/null +++ b/physics/examples/pendula/pendula.go @@ -0,0 +1,154 @@ +// 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 ( + "fmt" + + "cogentcore.org/core/colors" + "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/builder" + "cogentcore.org/lab/physics/phyxyz" +) + +// Pendula has sim params +type Pendula struct { + + // Number of bar elements to add to the pendulum. More interesting the more you add! + NPendula 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 to add + Force float32 + + // half-size of the pendulum elements. + HSize math32.Vector3 + + // Mass of each bar (kg) + Mass float32 + + // do the elements collide with each other? this is currently bad! + Collide bool + + // Stiff is the strength of the positional constraint to keep + // each bar in a vertical position. + Stiff float32 + + // Damp is the strength of the velocity constraint to keep each + // bar not moving. + 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 + b.Stiff = 0 +} + +func main() { + b := core.NewBody("test1").SetTitle("Physics Pendula") + ed := phyxyz.NewEditor(b) + ed.CameraPos = math32.Vec3(0, 3, 3) + + ps := &Pendula{} + ps.Defaults() + + ed.SetUserParams(ps) + + bld := builder.NewBuilder() + + var botJoint *builder.Joint + + ed.SetConfigFunc(func() { + bld.Reset() + wld := bld.NewWorld() + obj := wld.NewObject() + + 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) + + if ps.StartVert { + rleft = rot + } + + stY := 4 * ps.HSize.Y + x := -ps.HSize.Y + y := stY + if ps.StartVert { + x = 0 + y -= ps.HSize.Y + } + pb := obj.NewDynamicSkin(sc, "top", physics.Capsule, "blue", ps.Mass, ps.HSize, math32.Vec3(x, y, 0), rleft) + if !ps.Collide { + pb.SetGroup(1) + } + + 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).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)] + x := -float32(i)*ps.HSize.Y*2 - ps.HSize.Y + y := stY + if ps.StartVert { + y = stY + x + x = 0 + } + cb := obj.NewDynamicSkin(sc, "child", physics.Capsule, clr, ps.Mass, ps.HSize, math32.Vec3(x, y, 0), rleft) + if !ps.Collide { + 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).Init.SetPos(targ).SetStiff(ps.Stiff).SetVel(0).SetDamp(ps.Damp) + pb = cb + botJoint = jd + } + bld.ReplicateWorld(nil, 0, 2, 2, math32.Vec3(0, 0, -1), math32.Vec3(1, 0, 0)) + + 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.JointIndex, 0, ps.Force) + } else { + physics.SetJointControlForce(botJoint.JointIndex, 0, 0) + } + }) + + b.RunMainWindow() +} 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/examples/virtroom/README.md b/physics/examples/virtroom/README.md new file mode 100644 index 00000000..482fc5db --- /dev/null +++ b/physics/examples/virtroom/README.md @@ -0,0 +1,8 @@ +# 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. + +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..4cf6ab45 --- /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: "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 new file mode 100644 index 00000000..98dd0bad --- /dev/null +++ b/physics/examples/virtroom/virtroom.go @@ -0,0 +1,513 @@ +// 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/v2" + "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/builder" + "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 + Angry bool + + // VestibHRightEar is the horizontal rotation vestibular signal measured + // at the right ear, averaged over the steps. + VestibHRightEar float32 + + // full height of emer + Height float32 + + // emer object + Obj *builder.Object `display:"-"` + + // 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 + + // Emer state + Emer Emer `new-window:"+"` + + // Stiffness for actions + Stiff float32 + + // how far to move every step + MoveStep float32 + + // how far to rotate every step + RotStep float32 + + // number of model steps to take + ModelSteps int + + // 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 phyxyz.Camera + + // color map to use for rendering depth map + DepthMap core.ColorMapName + + // The core physics elements: Model, Builder, Scene + Physics builder.Physics + + // 3D visualization of the Scene + SceneEditor *xyzcore.SceneEditor + + // snapshot image + EyeRImg *core.Image `display:"-"` + + // 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.Stiff = 1000 + ev.MoveStep = ev.Emer.Height * .2 + ev.RotStep = 15 + ev.ModelSteps = 100 + ev.DepthMap = core.ColorMapName("ColdHot") + ev.Camera.Defaults() + ev.Camera.FOV = 90 +} + +func (ev *Env) MakeModel(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) + + 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.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, &ev.Emer, "emer") + // vw.Physics.Builder.ReplicateWorld(vw.Physics.Scene, 1, 1, 8) // 1x8 + ev.Physics.Build() + // params := physics.GetParams(0) + // params.ControlDt = 0.1 + // params.Gravity.Y = 0 + // params.MaxForce = 1.0e3 + // params.AngularDamping = 0.5 + // params.SubSteps = 1 +} + +// Initstate reinitializes the physics model state. +func (ev *Env) InitState() { //types:add + ev.Physics.InitState() + ev.UpdateView() +} + +// 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 { + if ev.Emer.EyeR == nil { + return nil + } + return ev.Physics.Scene.RenderFrom(ev.Emer.EyeR.Skin, &ev.Camera)[0] +} + +// 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) + ev.EyeRImg.NeedsRender() + } + // depth, err := ev.View3D.DepthImage() + // if err == nil && depth != nil { + // ev.DepthVals = depth + // ev.ViewDepth(depth) + // } +} + +// 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)] + img := image.NewRGBA(image.Rectangle{Max: ev.Camera.Size}) + ev.DepthImage.SetImage(img) + phyxyz.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() + } +} + +// 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) + 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 { + 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.GrabEyeImage() + ev.UpdateView() +} + +// 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) + // ev.Emer.PoseToPhysics() + 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. +func (ev *Env) StepBackward() { //types:add + 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. +func (ev *Env) RotBodyLeft() { //types:add + ev.Emer.XZ.AddTargetAngle(2, ev.RotStep, ev.Stiff) + ev.ModelStep() +} + +// 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. +func (ev *Env) RotHeadLeft() { //types:add + ev.Emer.Neck.AddTargetAngle(0, ev.RotStep, ev.Stiff) + ev.ModelStep() +} + +// RotHeadRight rotates emer right. +func (ev *Env) RotHeadRight() { //types:add + ev.Emer.Neck.AddTargetAngle(0, -ev.RotStep, ev.Stiff) + ev.ModelStep() +} + +// MakeRoom constructs a new room with given params +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 + obj := wl.NewObject() + sc := ev.Physics.Scene + 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), + math32.Vec3(-hw, hh, 0), rot) + obj.NewBodySkin(sc, name+"_right-wall", physics.Box, "green", math32.Vec3(ht, hh, hd), + math32.Vec3(hw, hh, 0), rot) + 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(wl *builder.World, em *Emer, name string) { + hh := em.Height / 2 + hw := hh * .4 + hd := hh * .15 + headsz := hd * 1.5 + eyesz := headsz * .2 + mass := float32(1) // kg + rot := math32.NewQuatIdentity() + obj := wl.NewObject() + 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) + 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) + // head.Group = 0 + hdsk := head.Skin + hdsk.InitSkin = func(sld *xyz.Solid) { + hdsk.BoxInit(sld) + sld.Updater(func() { + clr := hdsk.Color + if ev.Emer.Angry { + clr = "pink" + } + hdsk.UpdateColor(clr, sld) + }) + } + // 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)) + + 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 + ej := obj.NewJointFixed(head, bd, eyeoff, math32.Vec3(0, 0, -eyesz*.3)) + ej.ParentFixed = true + + 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)) + ej.ParentFixed = true +} + +func (ev *Env) ConfigGUI() *core.Body { + // vgpu.Debug = true + + b := core.NewBody("virtroom").SetTitle("Physics Virtual Room") + split := core.NewSplits(b) + + core.NewForm(split).SetStruct(ev) + imfr := core.NewFrame(split) + tbvw := core.NewTabs(split) + scfr, _ := tbvw.NewTab("3D View") + + split.SetSplits(.2, .2, .6) + + //////// 3D Scene + + etb := core.NewToolbar(scfr) + _ = etb + ev.SceneEditor = xyzcore.NewSceneEditor(scfr) + ev.SceneEditor.UpdateWidget() + sc := ev.SceneEditor.SceneXYZ() + ev.MakeModel(sc) + + // local toolbar for manipulating emer + // etb.Maker(phyxyz.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.InitState).SetText("Init").SetIcon(icons.Update) + }) + tree.Add(p, func(w *core.FuncButton) { + w.SetFunc(ev.GrabEyeImage).SetText("Grab Image").SetIcon(icons.Image) + }) + 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) { + 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 := phyxyz.NoDisplayScene(gp, dev) + ev.MakeModel(sc) + + img := ev.RenderEyeImg() + if img != nil { + imagex.Save(img, "eyer_0.png") + } +} diff --git a/physics/gosl.go b/physics/gosl.go new file mode 100644 index 00000000..a360159d --- /dev/null +++ b/physics/gosl.go @@ -0,0 +1,905 @@ +// 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 + 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 +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(PhysicsParams{})), 1, gpu.ComputeShader) + 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("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("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, "shaders/CollisionBroad.wgsl", sy) + pl.AddVarUsed(0, "TensorStrides") + pl.AddVarUsed(1, "Bodies") + pl.AddVarUsed(1, "BodyCollidePairs") + pl.AddVarUsed(2, "BroadContacts") + pl.AddVarUsed(2, "BroadContactsN") + 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, "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/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") + 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") + pl.AddVarUsed(2, "Contacts") + pl.AddVarUsed(2, "ContactsN") + 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(3, "JointControls") + 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/StepJointForces.wgsl", sy) + pl.AddVarUsed(0, "TensorStrides") + 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) + pl.AddVarUsed(0, "TensorStrides") + pl.AddVarUsed(1, "Bodies") + pl.AddVarUsed(2, "Dynamics") + pl.AddVarUsed(3, "JointControls") + pl.AddVarUsed(1, "JointDoFs") + pl.AddVarUsed(1, "Joints") + pl.AddVarUsed(1, "Objects") + 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 +} + +// 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) + } +} +// 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) + } +} +// 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) + } +} +// 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 +// 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) + } +} +// 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 +// 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 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 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(120) + TensorStrides.SetInt1D(Bodies.Shape().Strides[0], 0) + TensorStrides.SetInt1D(Bodies.Shape().Strides[1], 1) + 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) +} + +// 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 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 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 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() + gpu.ReadToBytes(v, Joints.Values) + case JointDoFsVar: + v, _ := syVars.ValueByIndex(1, "JointDoFs", 0) + v.ReadSync() + gpu.ReadToBytes(v, JointDoFs.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] []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) *PhysicsParams { + return &Params[idx] +} diff --git a/physics/joint.go b/physics/joint.go new file mode 100644 index 00000000..63a03971 --- /dev/null +++ b/physics/joint.go @@ -0,0 +1,559 @@ +// 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 ( + // "fmt" + "math" + + "cogentcore.org/core/math32" +) + +//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 + + // PlaneXZ is a version of D6 for navigation in the X-Z plane, + // which creates 2 linear DoF (X, Z) for movement. + PlaneXZ +) + +// 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 + + // 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 + + // 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 + + // JointChild is the dynamic body index for child body. + 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 + JointCQuatW + + // 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, Distance, D6 + JointDoF4 + JointDoF5 + JointDoF6 + + // 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 + + // Computed linear lambdas. + JointLinLambdaX + JointLinLambdaY + JointLinLambdaZ + + // Computed angular lambdas. + JointAngLambdaX + JointAngLambdaY + JointAngLambdaZ +) + +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 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 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 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)) +} + +func JointParentIndex(idx int32) int32 { + return int32(math.Float32bits(Joints.Value(int(idx), int(JointParent)))) +} + +func SetJointChild(idx, bodyIdx int32) { + Joints.Set(math.Float32frombits(uint32(bodyIdx)), int(idx), int(JointChild)) +} + +func JointChildIndex(idx int32) int32 { + return int32(math.Float32bits(Joints.Value(int(idx), int(JointChild)))) +} + +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 GetJointAngularDoFN(idx int32) int32 { + return int32(math.Float32bits(Joints.Value(int(idx), int(JointAngularDoFN)))) +} + +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))) +} + +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 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 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 { + 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 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 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 { + 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) { + 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 { + 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) { + 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 { + 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) { + 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 { + 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) { + Joints.Set(t.X, int(idx), int(JointCTorqueX)) + Joints.Set(t.Y, int(idx), int(JointCTorqueY)) + 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 + +const ( + // axis of articulation for the DoF + JointAxisX JointDoFVars = iota + JointAxisY + JointAxisZ + + // joint limits + JointLimitLower + JointLimitUpper +) + +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))) +} + +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)) +} + +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 + +func (ml *Model) JointDefaults(idx int32) { + rot := math32.NewQuatIdentity() + SetJointPQuat(idx, rot) + SetJointCQuat(idx, rot) +} + +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)) +} + +// 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 (ml *Model) NewJointFixed(parent, child int32, ppos, cpos math32.Vector3) int32 { + idx := ml.newJoint(Fixed, parent, child, ppos, cpos) + SetJointNoLinearRotation(idx, true) + return idx +} + +// 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. +func (ml *Model) NewJointPrismatic(parent, child int32, ppos, cpos, axis math32.Vector3) int32 { + idx := ml.newJoint(Prismatic, parent, child, ppos, cpos) + SetJointLinearDoFN(idx, 1) + ml.newJointDoF(idx, 0, axis) + 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. +func (ml *Model) NewJointRevolute(parent, child int32, ppos, cpos, axis math32.Vector3) int32 { + idx := ml.newJoint(Revolute, parent, child, ppos, cpos) + SetJointAngularDoFN(idx, 1) + ml.newJointDoF(idx, 0, axis) + 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. +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) + ml.newJointDoF(idx, int32(d), axis) + } + 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 two linear DoF control position in X, Z, and 3rd angular +// 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. +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 +} + +// 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. +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) + ml.newJointDoF(idx, int32(d), axis) + } + for d := range math32.W { + axis := math32.Vector3{} + axis.SetDim(d, 1) + ml.newJointDoF(idx, int32(d), axis) + } + // only on the X linear axis + SetJointDoF(idx, 0, JointLimitLower, minDist) + SetJointDoF(idx, 0, JointLimitUpper, maxDist) + return idx +} + +// 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. +func (ml *Model) NewJointFree(parent, child int32, ppos, cpos math32.Vector3) int32 { + idx := ml.newJoint(Free, parent, child, ppos, cpos) + SetJointLinearDoFN(idx, 0) + SetJointAngularDoFN(idx, 0) + 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 (ml *Model) newJoint(joint JointTypes, parent, child int32, ppos, cpos math32.Vector3) int32 { + sizes := ml.Joints.ShapeSizes() + idx := int32(sizes[0]) + params := &ml.Params[0] + params.JointsN = idx + 1 + ml.Joints.SetShapeSizes(int(idx+1), int(JointVarsN)) + ml.JointDefaults(idx) + SetJointType(idx, joint) + SetJointEnabled(idx, true) + SetJointParent(idx, parent) + 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 +} + +// newJointDoF adds new JointDoFs and JointControls entries +// initialized to detfaults. Returns index. +func (ml *Model) newJointDoF(jidx, dof int32, axis math32.Vector3) int32 { + sizes := ml.JointDoFs.ShapeSizes() + didx := int32(sizes[0]) + 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 new file mode 100644 index 00000000..f79df4c9 --- /dev/null +++ b/physics/joint.goal @@ -0,0 +1,558 @@ +// 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" + "math" + + "cogentcore.org/core/math32" +) + +//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 + + // PlaneXZ is a version of D6 for navigation in the X-Z plane, + // which creates 2 linear DoF (X, Z) for movement. + PlaneXZ +) + +// 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 + + // 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 + + // 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 + + // JointChild is the dynamic body index for child body. + 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 + JointCQuatW + + // 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, Distance, D6 + JointDoF4 + JointDoF5 + JointDoF6 + + // 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 + + // Computed linear lambdas. + JointLinLambdaX + JointLinLambdaY + JointLinLambdaZ + + // Computed angular lambdas. + JointAngLambdaX + JointAngLambdaY + JointAngLambdaZ +) + +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 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 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 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)) +} + +func JointParentIndex(idx int32) int32 { + return int32(math.Float32bits(Joints[idx, JointParent])) +} + +func SetJointChild(idx, bodyIdx int32) { + Joints[idx, JointChild] = math.Float32frombits(uint32(bodyIdx)) +} + +func JointChildIndex(idx int32) int32 { + return int32(math.Float32bits(Joints[idx, JointChild])) +} + +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 GetJointAngularDoFN(idx int32) int32 { + return int32(math.Float32bits(Joints[idx, JointAngularDoFN])) +} + +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]) +} + +func SetJointPPos(idx int32, pos math32.Vector3) { + Joints[idx, JointPPosX] = pos.X + Joints[idx, JointPPosY] = pos.Y + Joints[idx, JointPPosZ] = pos.Z +} + +func JointPQuat(idx int32) math32.Quat { + return math32.NewQuat(Joints[idx, JointPQuatX], Joints[idx, JointPQuatY], Joints[idx, JointPQuatZ], Joints[idx, JointPQuatW]) +} + +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 { + 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 JointCQuat(idx int32) math32.Quat { + return math32.NewQuat(Joints[idx, JointCQuatX], Joints[idx, JointCQuatY], Joints[idx, JointCQuatZ], Joints[idx, JointCQuatW]) +} + +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 { + return math32.Vec3(Joints[idx, JointPForceX], Joints[idx, JointPForceY], Joints[idx, JointPForceZ]) +} + +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 { + return math32.Vec3(Joints[idx, JointPTorqueX], Joints[idx, JointPTorqueY], Joints[idx, JointPTorqueZ]) +} + +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 { + return math32.Vec3(Joints[idx, JointCForceX], Joints[idx, JointCForceY], Joints[idx, JointCForceZ]) +} + +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 { + return math32.Vec3(Joints[idx, JointCTorqueX], Joints[idx, JointCTorqueY], Joints[idx, JointCTorqueZ]) +} + +func SetJointCTorque(idx int32, t math32.Vector3) { + Joints[idx, JointCTorqueX] = t.X + Joints[idx, JointCTorqueY] = t.Y + 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 + +const ( + // axis of articulation for the DoF + JointAxisX JointDoFVars = iota + JointAxisY + JointAxisZ + + // joint limits + JointLimitLower + JointLimitUpper +) + +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 +} + +//gosl:end + +func (ml *Model) JointDefaults(idx int32) { + rot := math32.NewQuatIdentity() + SetJointPQuat(idx, rot) + SetJointCQuat(idx, rot) +} + +func (ml *Model) JointDoFDefaults(didx int32) { + JointDoFs[didx, JointLimitLower] = -JointLimitUnlimited + JointDoFs[didx, JointLimitUpper] = JointLimitUnlimited + 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 (ml *Model) NewJointFixed(parent, child int32, ppos, cpos math32.Vector3) int32 { + idx := ml.newJoint(Fixed, parent, child, ppos, cpos) + SetJointNoLinearRotation(idx, true) + return idx +} + +// 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. +func (ml *Model) NewJointPrismatic(parent, child int32, ppos, cpos, axis math32.Vector3) int32 { + idx := ml.newJoint(Prismatic, parent, child, ppos, cpos) + SetJointLinearDoFN(idx, 1) + ml.newJointDoF(idx, 0, axis) + 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. +func (ml *Model) NewJointRevolute(parent, child int32, ppos, cpos, axis math32.Vector3) int32 { + idx := ml.newJoint(Revolute, parent, child, ppos, cpos) + SetJointAngularDoFN(idx, 1) + ml.newJointDoF(idx, 0, axis) + 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. +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) + ml.newJointDoF(idx, int32(d), axis) + } + 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 two linear DoF control position in X, Z, and 3rd angular +// 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. +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 +} + +// 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. +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) + ml.newJointDoF(idx, int32(d), axis) + } + for d := range math32.W { + axis := math32.Vector3{} + axis.SetDim(d, 1) + ml.newJointDoF(idx, int32(d), axis) + } + // only on the X linear axis + SetJointDoF(idx, 0, JointLimitLower, minDist) + SetJointDoF(idx, 0, JointLimitUpper, maxDist) + return idx +} + +// 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. +func (ml *Model) NewJointFree(parent, child int32, ppos, cpos math32.Vector3) int32 { + idx := ml.newJoint(Free, parent, child, ppos, cpos) + SetJointLinearDoFN(idx, 0) + SetJointAngularDoFN(idx, 0) + 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 (ml *Model) newJoint(joint JointTypes, parent, child int32, ppos, cpos math32.Vector3) int32 { + sizes := ml.Joints.ShapeSizes() + idx := int32(sizes[0]) + params := &ml.Params[0] + params.JointsN = idx + 1 + ml.Joints.SetShapeSizes(int(idx+1), int(JointVarsN)) + ml.JointDefaults(idx) + SetJointType(idx, joint) + SetJointEnabled(idx, true) + SetJointParent(idx, parent) + 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 +} + +// newJointDoF adds new JointDoFs and JointControls entries +// initialized to detfaults. Returns index. +func (ml *Model) newJointDoF(jidx, dof int32, axis math32.Vector3) int32 { + sizes := ml.JointDoFs.ShapeSizes() + didx := int32(sizes[0]) + 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/model.go b/physics/model.go new file mode 100644 index 00000000..04a7a6d0 --- /dev/null +++ b/physics/model.go @@ -0,0 +1,290 @@ +// Code generated by "goal build"; DO NOT EDIT. +//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. + +package physics + +import ( + "cogentcore.org/core/math32" + "cogentcore.org/lab/tensor" +) + +//go:generate core generate -add-types -gosl + +// Model contains and manages all of the physics elements. +type Model struct { + // GPU determines whether to use GPU (else CPU). + GPU bool + + // Params are global parameters. + Params []PhysicsParams + + // 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. + CurrentWorld int + + // CurrentObject is the Object to use when creating new joints. + // Call NewObject to increment. + CurrentObject int `edit:"-"` + + // CurrentObjectJoint is the Joint index in CurrentObject + // to use when creating new joints. + CurrentObjectJoint int `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 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. + // [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. + // 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][MaxObjectJoints+1] + 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] + 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"` + + // 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] + Dynamics *tensor.Float32 `display:"no-inline"` + + // BroadContactsN has number of points of broad contact + // between bodies. [1] + BroadContactsN *tensor.Int32 `display:"no-inline"` + + // 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 + // (in correspondence with [JointDoFs]). This can be uploaded to the + // GPU at every step. + // [dof][JointControlVarsN] + JointControls *tensor.Float32 `display:"no-inline"` +} + +func NewModel() *Model { + ml := &Model{} + ml.Init() + return ml +} + +// Init makes initial vars. Called in NewModel. +// Must call Config once configured. +func (ml *Model) Init() { + ml.GPU = true + ml.Params = make([]PhysicsParams, 1) + ml.Params[0].Defaults() + ml.Reset() +} + +// 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) + 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() +} + +// 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 { + 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) + if shape == Capsule { + hsize.Y = max(hsize.Y, hsize.X*1.01) + } + SetBodyHSize(idx, hsize) + SetBodyPos(idx, pos) + SetBodyQuat(idx, rot) + SetBodyGroup(idx, -1) // assume static + SetBodyWorld(idx, int32(ml.CurrentWorld)) + 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, hsize, pos math32.Vector3, rot math32.Quat) (bodyIdx, dynIdx int32) { + if shape == Plane { + panic("physics.NewDynamic: shape cannot be Plane") + } + bodyIdx = ml.NewBody(shape, hsize, pos, rot) + sizes := ml.Dynamics.ShapeSizes() + dynIdx = int32(sizes[0]) + 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 + ml.SetMass(bodyIdx, shape, hsize, mass) + return +} + +// 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 (ml *Model) SetAsCurrent() { + isCur := (Bodies == ml.Bodies) + CurModel = ml + ml.SetAsCurrentVars() + if GPUInitialized && !isCur { + ml.ToGPUInfra() + } +} + +// SetAsCurrentVars sets these as the current global values that are +// processed in the code (on the GPU). +func (ml *Model) SetAsCurrentVars() { + Params = ml.Params + Bodies = ml.Bodies + Objects = ml.Objects + 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 (ml *Model) GPUInit() { + GPUInit() + 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 (ml *Model) ToGPUInfra() { + ToGPUTensorStrides() + ToGPU(ParamsVar, BodiesVar, ObjectsVar, JointsVar, JointDoFsVar, BodyJointsVar, BodyCollidePairsVar, DynamicsVar, BroadContactsNVar, BroadContactsVar, ContactsNVar, ContactsVar, JointControlsVar) +} + +// 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) ReplicasBodyIndexes(bi, replica int32) (bodyIdx, dynIdx int32) { + start := ml.ReplicaBodiesStart + n := ml.ReplicaBodiesN + if ml.ReplicasN == 0 || bi < start { + return bi, GetBodyDynamic(bi) + } + rbi := (bi - start) % n + bodyIdx = start + rbi + replica*n + 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 { + start := ml.ReplicaJointsStart + n := ml.ReplicaJointsN + if ml.ReplicasN == 0 || ji < start { + return ji + } + rji := (ji - start) % n + nji := start + rji + replica*n + return nji +} diff --git a/physics/model.goal b/physics/model.goal new file mode 100644 index 00000000..f460e9b3 --- /dev/null +++ b/physics/model.goal @@ -0,0 +1,288 @@ +// 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 + +// Model contains and manages all of the physics elements. +type Model struct { + // GPU determines whether to use GPU (else CPU). + GPU bool + + // Params are global parameters. + Params []PhysicsParams + + // 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. + CurrentWorld int + + // CurrentObject is the Object to use when creating new joints. + // Call NewObject to increment. + CurrentObject int `edit:"-"` + + // CurrentObjectJoint is the Joint index in CurrentObject + // to use when creating new joints. + CurrentObjectJoint int `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 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. + // [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. + // 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][MaxObjectJoints+1] + 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] + 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"` + + // 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] + Dynamics *tensor.Float32 `display:"no-inline"` + + // BroadContactsN has number of points of broad contact + // between bodies. [1] + BroadContactsN *tensor.Int32 `display:"no-inline"` + + // 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 + // (in correspondence with [JointDoFs]). This can be uploaded to the + // GPU at every step. + // [dof][JointControlVarsN] + JointControls *tensor.Float32 `display:"no-inline"` +} + +func NewModel() *Model { + ml := &Model{} + ml.Init() + return ml +} + +// Init makes initial vars. Called in NewModel. +// Must call Config once configured. +func (ml *Model) Init() { + ml.GPU = true + ml.Params = make([]PhysicsParams, 1) + ml.Params[0].Defaults() + ml.Reset() +} + +// 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) + 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() +} + +// 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 { + 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) + if shape == Capsule { + hsize.Y = max(hsize.Y, hsize.X*1.01) + } + SetBodyHSize(idx, hsize) + SetBodyPos(idx, pos) + SetBodyQuat(idx, rot) + SetBodyGroup(idx, -1) // assume static + SetBodyWorld(idx, int32(ml.CurrentWorld)) + 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, hsize, pos math32.Vector3, rot math32.Quat) (bodyIdx, dynIdx int32) { + if shape == Plane { + panic("physics.NewDynamic: shape cannot be Plane") + } + bodyIdx = ml.NewBody(shape, hsize, pos, rot) + sizes := ml.Dynamics.ShapeSizes() + dynIdx = int32(sizes[0]) + 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 + ml.SetMass(bodyIdx, shape, hsize, mass) + return +} + +// 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 (ml *Model) SetAsCurrent() { + isCur := (Bodies == ml.Bodies) + CurModel = ml + ml.SetAsCurrentVars() + if GPUInitialized && !isCur { + ml.ToGPUInfra() + } +} + +// SetAsCurrentVars sets these as the current global values that are +// processed in the code (on the GPU). +func (ml *Model) SetAsCurrentVars() { + Params = ml.Params + Bodies = ml.Bodies + Objects = ml.Objects + 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 (ml *Model) GPUInit() { + GPUInit() + 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 (ml *Model) ToGPUInfra() { + ToGPUTensorStrides() + ToGPU(ParamsVar, BodiesVar, ObjectsVar, JointsVar, JointDoFsVar, BodyJointsVar, BodyCollidePairsVar, DynamicsVar, BroadContactsNVar, BroadContactsVar, ContactsNVar, ContactsVar, JointControlsVar) +} + +// 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) ReplicasBodyIndexes(bi, replica int32) (bodyIdx, dynIdx int32) { + start := ml.ReplicaBodiesStart + n := ml.ReplicaBodiesN + if ml.ReplicasN == 0 || bi < start { + return bi, GetBodyDynamic(bi) + } + rbi := (bi - start) % n + bodyIdx = start + rbi + replica*n + 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 { + start := ml.ReplicaJointsStart + n := ml.ReplicaJointsN + if ml.ReplicasN == 0 || ji < start { + return ji + } + rji := (ji - start) % n + nji := start + rji + replica*n + return nji +} diff --git a/physics/params.go b/physics/params.go new file mode 100644 index 00000000..3acdd50d --- /dev/null +++ b/physics/params.go @@ -0,0 +1,178 @@ +// 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/slbool" + "cogentcore.org/lab/gosl/slvec" +) + +//gosl:start + +// 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"` + + // Dt is the integration stepsize. + // 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"` + + // 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"` + + // ControlDt is the stepsize for integrating joint control position values + // [JointTargetPos] over time, to avoid sudden strong changes in force. + // 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 + ContactMargin float32 `default:"0,0.1"` + + // ContactRelax is rigid contact relaxation constant. + // Higher values cause errros + ContactRelax float32 `default:"0.8"` // 0.8 def + + // Contact weighting: balances contact forces? + ContactWeighting slbool.Bool `default:"true"` // true + + // Restitution takes into account bounciness of objects. + Restitution slbool.Bool `default:"false"` // false + + // JointLinearRelax is joint linear relaxation constant. + JointLinearRelax float32 `default:"0.7"` // 0.7 def + + // JointAngularRelax is joint angular relaxation constant. + JointAngularRelax float32 `default:"0.4"` // 0.4 def + + // JointLinearComply is joint linear compliance constant. + JointLinearComply float32 `default:"0"` // 0 def + + // JointAngularComply is joint angular compliance constant. + JointAngularComply float32 `default:"0"` // 0 def + + // AngularDamping is damping of angular motion. + AngularDamping float32 `default:"0"` // 0 def + + // 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"` + + // 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"` + + // Maximum number of contacts to process at any given point. + ContactsMax 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:"-"` + + // BodiesN is number of rigid bodies. + BodiesN int32 `edit:"-"` + + // 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:"-"` + + // JointDoFsN is number of joint DoFs. + JointDoFsN int32 `edit:"-"` + + // BodyJointsMax is max number of joints per body + 1 for actual n. + BodyJointsMax int32 `edit:"-"` + + // BodyCollidePairsN is the total number of pre-compiled collision pairs + // to examine. + BodyCollidePairsN int32 `edit:"-"` + + pad, pad1, pad2 int32 + + // Gravity is the gravity acceleration function + Gravity slvec.Vector3 +} + +func (pr *PhysicsParams) Defaults() { + pr.Iterations = 1 + pr.Dt = 0.0001 + pr.SubSteps = 10 + pr.ControlDt = 1 + pr.ControlDtThr = 1 + pr.Gravity.Set(0, -9.81, 0) + + pr.ContactMargin = 0 + pr.ContactRelax = 0.8 + pr.ContactWeighting.SetBool(true) + pr.Restitution.SetBool(false) + + pr.JointLinearRelax = 0.7 + pr.JointAngularRelax = 0.4 + pr.JointLinearComply = 0 + pr.JointAngularComply = 0 + + pr.AngularDamping = 0 + pr.SoftRelax = 0.9 + pr.MaxForce = 1.0e5 + pr.MaxDelta = 2 + pr.MaxGeomIter = 10 +} + +// Reset resets the N's +func (pr *PhysicsParams) 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 *PhysicsParams) 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/phyxyz/camera.go b/physics/phyxyz/camera.go new file mode 100644 index 00000000..b593ffa9 --- /dev/null +++ b/physics/phyxyz/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 phyxyz + +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/phyxyz/depth.go b/physics/phyxyz/depth.go new file mode 100644 index 00000000..9a64149c --- /dev/null +++ b/physics/phyxyz/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 phyxyz + +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/phyxyz/editor.go b/physics/phyxyz/editor.go new file mode 100644 index 00000000..3b4eb6ac --- /dev/null +++ b/physics/phyxyz/editor.go @@ -0,0 +1,291 @@ +// 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 phyxyz + +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. It creates and manages its own +// [physics.Model] and [phyxyz.Scene]. +type Editor struct { //types:add + core.Frame + + // Model has the physics simulation. + Model *physics.Model + + // 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 [physics.Model]. + 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) + + // 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 + + // Replica is the replica world to view, if replicas are present in model. + Replica 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 + + // editor is the xyz GUI visualization widget. + editor *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.CameraPos = math32.Vec3(0, 25, 20) + + pe.Styler(func(s *styles.Style) { + s.Grow.Set(1, 1) + s.Direction = styles.Column + }) + + 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 + if pe.UserParams != nil { + pe.userParamsForm.SetStruct(pe.UserParams) + } + params := &pe.Model.Params[0] + pe.paramsForm.SetStruct(params) + }) + }) + + tree.AddChildAt(w, "scene", func(w *xyzcore.SceneEditor) { + pe.editor = w + w.UpdateWidget() + sc := pe.editor.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.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)) + 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 = pe.CameraPos + sc.Camera.LookAt(math32.Vec3(0, 0, 0), math32.Vec3(0, 1, 0)) + sc.SaveCamera("1") + sc.SaveCamera("default") + + pe.ConfigModel() + }) + }) +} + +// ConfigModel configures the physics model. +func (pe *Editor) ConfigModel() { + if pe.isRunning { + core.MessageSnackbar(pe, "Simulation is still running...") + return + } + pe.Scene.Reset() + pe.Model.Reset() + if pe.ConfigFunc != nil { + pe.ConfigFunc() + } + 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). +func (pe *Editor) Restart() bool { + if pe.isRunning { + core.MessageSnackbar(pe, "Simulation is still running...") + return false + } + pe.stop = false + pe.TimeStep = 0 + pe.Scene.InitState(pe.Model) + pe.editor.NeedsRender() + 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) + 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 pe.Step(n) + }) + 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) { + pe.ConfigModel() + }) + 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 := int32(0) + if physics.CurModel != nil && pe.Scene != nil { + replN = physics.CurModel.ReplicasN + pe.Scene.ReplicasView = replN > 0 + } + w.SetMax(float32(replN - 1)) + s.SetEnabled(replN > 1) + }) + w.OnChange(func(e events.Event) { + pe.Scene.ReplicasIndex = pe.Replica + pe.Scene.Update() + pe.NeedsRender() + }) + }) +} diff --git a/physics/phyxyz/nogui.go b/physics/phyxyz/nogui.go new file mode 100644 index 00000000..3c55d0fe --- /dev/null +++ b/physics/phyxyz/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 phyxyz + +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/phyxyz/scene.go b/physics/phyxyz/scene.go new file mode 100644 index 00000000..b7446672 --- /dev/null +++ b/physics/phyxyz/scene.go @@ -0,0 +1,164 @@ +// 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 phyxyz implements visualization of [physics] using [xyz] +// 3D graphics. +package phyxyz + +//go:generate core generate -add-types + +import ( + "image" + + "cogentcore.org/core/math32" + "cogentcore.org/core/tree" + "cogentcore.org/core/xyz" + "cogentcore.org/lab/physics" +) + +// 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 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 + + // 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]. +// with given [xyz.Scene], making a top-level Root group in the scene. +func NewScene(sc *xyz.Scene) *Scene { + rgp := xyz.NewGroup(sc) + rgp.SetName("world") + xysc := &Scene{Scene: sc, Root: rgp} + return xysc +} + +// 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). This calls Update(). +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 { + sc.Update() + return + } + sc.Root.Maker(func(p *tree.Plan) { + for _, sk := range sc.Skins { + sk.Add(p) + } + }) + 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 + if sc.Scene != nil { + sc.Scene.Update() + } +} + +// Update updates the xyz scene from current physics node state. +// (use physics.Model.SetAsCurrent()). +func (sc *Scene) Update() { + sc.UpdateFromPhysics() + if sc.Scene != nil { + sc.Scene.Update() + } +} + +// UpdateFromPhysics updates the Scene from currently active +// physics state (use physics.Model.SetAsCurrent()). +func (sc *Scene) UpdateFromPhysics() { + for _, sk := range sc.Skins { + sk.UpdateFromPhysics(sc) + } +} + +// RenderFrom does an offscreen render using given [Skin] +// 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. +func (sc *Scene) RenderFrom(sk *Skin, cam *Camera) []image.Image { + xysc := sc.Scene + camnm := "scene-renderfrom-save" + xysc.SaveCamera(camnm) + rep := sc.ReplicasIndex + + xysc.Camera.FOV = cam.FOV + xysc.Camera.Near = cam.Near + xysc.Camera.Far = cam.Far + xysc.Camera.Pose.Pos = sk.Pos + xysc.Camera.Pose.Quat = sk.Quat + xysc.Camera.Pose.Scale.Set(1, 1, 1) + + xysc.UseAltFrame(cam.Size) + + 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.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 + } + 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 +// 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 +} + +// 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 new file mode 100644 index 00000000..18ef7655 --- /dev/null +++ b/physics/phyxyz/skin.go @@ -0,0 +1,363 @@ +// 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 phyxyz + +import ( + "fmt" + "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" +) + +// 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. + Shape physics.Shapes + + // Color is the color of the element. + Color string + + // 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 + + // Quat is the rotation as a quaternion. + Quat math32.Quat + + // NewSkin is a function that returns a new [xyz.Node] + // to represent this element. If nil, uses appropriate defaults. + NewSkin func() tree.Node + + // InitSkin is a function that initializes a new [xyz.Node] + // that represents this element. If nil, uses appropriate defaults. + InitSkin func(sld *xyz.Solid) + + // BodyIndex is the index of the body in [physics.Bodies] + BodyIndex int32 + + // DynamicIndex is the index in [physics.Dynamics] (-1 if not dynamic). + DynamicIndex int32 +} + +// NewBody adds a new body with given parameters. +// Returns the Skin which can then be further customized. +// Use this for Static elements; NewDynamic for dynamic elements. +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 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 Skin from physics state. +func (sk *Skin) UpdateFromPhysics(sc *Scene) { + params := physics.GetParams(0) + di := int32(sk.DynamicIndex) + bi := int32(sk.BodyIndex) + if sc.ReplicasView { + bi, di = physics.CurModel.ReplicasBodyIndexes(bi, int32(sc.ReplicasIndex)) + } + if di >= 0 { + sk.Pos = physics.DynamicPos(di, params.Cur) + sk.Quat = physics.DynamicQuat(di, params.Cur) + } else { + sk.Pos = physics.BodyPos(bi) + sk.Quat = physics.BodyQuat(bi) + } +} + +// 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 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 NewSkin +// function on the node, or default. +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) { sk.Init(n.(*xyz.Solid)) }) +} + +// 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 sk.Shape { + case physics.Plane: + sk.PlaneInit(sld) + case physics.Sphere: + sk.SphereInit(sld) + case physics.Capsule: + sk.CapsuleInit(sld) + case physics.Cylinder: + sk.CylinderInit(sld) + case physics.Box: + sk.BoxInit(sld) + } +} + +// 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 (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 = sk.HSize.MulScalar(2) + sk.UpdateColor(sk.Color, sld) + sld.Updater(func() { + sk.UpdatePose(sld) + }) +} + +// 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 (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 sk.HSize.X == 0 { + inf := float32(1e3) + sld.Pose.Scale = math32.Vec3(inf, 1, inf) + } else { + sld.Pose.Scale = sk.HSize.MulScalar(2) + } + sk.UpdateColor(sk.Color, sld) + sld.Updater(func() { + sk.UpdatePose(sld) + }) +} + +// 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 (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 = sk.HSize + sld.Pose.Scale.Y *= 2 + sk.UpdateColor(sk.Color, sld) + sld.Updater(func() { + sk.UpdatePose(sld) + }) +} + +// 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 (sk *Skin) CapsuleInit(sld *xyz.Solid) { + rat := sk.HSize.Y / sk.HSize.X + mnm := fmt.Sprintf("physics.Capsule_%g", math32.Truncate(rat, 3)) + if ms, _ := sld.Scene.MeshByName(mnm); ms == nil { + 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, sk.HSize.X, sk.HSize.X) + sk.UpdateColor(sk.Color, sld) + sld.Updater(func() { + sk.UpdatePose(sld) + }) +} + +// 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 (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(sk.HSize.X) + sk.UpdateColor(sk.Color, sld) + sld.Updater(func() { + 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 (sk *Skin) SetBodyWorld(world int) { + physics.SetBodyWorld(sk.BodyIndex, 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 (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 (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 (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 (sk *Skin) SetBodyFrictionTortion(val float32) { + physics.Bodies.Set(val, int(sk.BodyIndex), int(physics.BodyFrictionTortion)) +} + +// SetBodyFrictionRolling is resistance to rolling motion at contact. +func (sk *Skin) SetBodyFrictionRolling(val float32) { + physics.Bodies.Set(val, int(sk.BodyIndex), int(physics.BodyFrictionRolling)) +} + +// 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 (sc *Scene) NewJointFixed(ml *physics.Model, parent, child *Skin, ppos, cpos math32.Vector3) int32 { + pidx := int32(-1) + if parent != nil { + pidx = parent.DynamicIndex + } + return ml.NewJointFixed(pidx, child.DynamicIndex, ppos, cpos) +} + +// 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 (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 ml.NewJointPrismatic(pidx, child.DynamicIndex, ppos, cpos, axis) +} + +// 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 (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 ml.NewJointRevolute(pidx, child.DynamicIndex, ppos, cpos, axis) +} + +// 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 (sc *Scene) NewJointBall(ml *physics.Model, parent, child *Skin, ppos, cpos math32.Vector3) int32 { + pidx := int32(-1) + if parent != nil { + pidx = parent.DynamicIndex + } + 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, +// 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 (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 ml.NewJointDistance(pidx, child.DynamicIndex, ppos, cpos, minDist, maxDist) +} + +// 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 (sc *Scene) NewJointFree(ml *physics.Model, parent, child *Skin, ppos, cpos math32.Vector3) int32 { + pidx := int32(-1) + if parent != nil { + pidx = parent.DynamicIndex + } + return ml.NewJointFree(pidx, child.DynamicIndex, ppos, cpos) +} diff --git a/physics/phyxyz/typegen.go b/physics/phyxyz/typegen.go new file mode 100644 index 00000000..edfc779b --- /dev/null +++ b/physics/phyxyz/typegen.go @@ -0,0 +1,105 @@ +// Code generated by "core generate -add-types"; DO NOT EDIT. + +package phyxyz + +import ( + "cogentcore.org/core/math32" + "cogentcore.org/core/tree" + "cogentcore.org/core/types" + "cogentcore.org/core/xyz" + "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. 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. 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]: +// Model has the physics simulation. +func (t *Editor) SetModel(v *physics.Model) *Editor { t.Model = 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. +// 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 [physics.Model]. +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 } + +// 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 } + +// 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]."}, {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)."}}}) + +// 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 new file mode 100644 index 00000000..89423ea9 --- /dev/null +++ b/physics/shaders/CollisionBroad.wgsl @@ -0,0 +1,515 @@ +// 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(0) +var Bodies: array; +@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) +var Dynamics: array; +@group(2) @binding(1) +var BroadContactsN: array>; +@group(2) @binding(2) +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; + +@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; +} + +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 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 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))]); +} +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 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 SetBroadContactA(idx: i32,bodIdx: i32) { + BroadContacts[Index2D(TensorStrides[80], TensorStrides[81], u32(idx), u32(ContactA))] = bitcast(u32(bodIdx)); +} +fn SetBroadContactB(idx: i32,bodIdx: i32) { + BroadContacts[Index2D(TensorStrides[80], TensorStrides[81], u32(idx), u32(ContactB))] = bitcast(u32(bodIdx)); +} +fn SetBroadContactPointIdx(idx: i32,ptIdx: i32) { + BroadContacts[Index2D(TensorStrides[80], TensorStrides[81], u32(idx), u32(ContactPointIdx))] = bitcast(u32(ptIdx)); +} +fn CollisionBroad(i: u32) { //gosl:kernel + var params = Params[0]; + var ci = i32(i); + if (ci >= params.BodyCollidePairsN) { + return; + } + 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); + 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 = BodyHSize(biA); + if (szA.x == 0) { + infPlane = true; + } + var queryB = MulSpatialPoint(xwAR, xwAQ, xwBR); + var closest = ClosestPointPlane(szA.x, szA.z, 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(&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; + } + 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 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 = 6; +const DynamicVarsN: DynamicVars = 33; +const GPUVarsN: GPUVars = 13; +const JointTypesN: JointTypes = 8; +const JointVarsN: JointVars = 46; +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 JointParentFixed: JointVars = 2; +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; +const JointAxisZ: JointDoFVars = 2; +const JointLimitLower: JointDoFVars = 3; +const JointLimitUpper: JointDoFVars = 4; + +//////// import: "params.go" +struct PhysicsParams { + Iterations: i32, + Dt: f32, + SubSteps: i32, + ControlDt: f32, + ControlDtThr: f32, + 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, + MaxDelta: f32, + MaxGeomIter: i32, + ContactsMax: i32, + Cur: i32, + Next: i32, + BodiesN: i32, + DynamicsN: i32, + ObjectsN: i32, + MaxObjectJoints: i32, + JointsN: i32, + JointDoFsN: i32, + BodyJointsMax: i32, + BodyCollidePairsN: i32, + pad: i32, + pad1: i32, + pad2: 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" +fn ClosestPointPlane(width: f32,length: f32, pt: vec3) -> vec3 { + var cp = pt; + cp.y = f32(0); + if (width == 0.0) { + return cp; + } + cp.x = clamp(pt.x, -width, width); + cp.z = clamp(pt.z, -length, length); +return cp; +} + +//////// 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; +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: { // note: Cone has no collision points! + return i32(0); + } + } +} + +//////// import: "slmath-math.go" +const Pi = 3.141592653589793; + +//////// 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 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 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" + +//////// import: "step_joint.go" \ No newline at end of file diff --git a/physics/shaders/CollisionNarrow.wgsl b/physics/shaders/CollisionNarrow.wgsl new file mode 100644 index 00000000..dfc05f93 --- /dev/null +++ b/physics/shaders/CollisionNarrow.wgsl @@ -0,0 +1,1034 @@ +// 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(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] + +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; +} + +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 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 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))]); +} +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 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 GetBroadContactA(idx: i32) -> i32 { + return i32(bitcast(BroadContacts[Index2D(TensorStrides[80], TensorStrides[81], u32(idx), u32(ContactA))])); +} +fn GetBroadContactB(idx: i32) -> i32 { + return i32(bitcast(BroadContacts[Index2D(TensorStrides[80], TensorStrides[81], u32(idx), u32(ContactB))])); +} +fn GetBroadContactPointIdx(idx: i32) -> i32 { + return i32(bitcast(BroadContacts[Index2D(TensorStrides[80], TensorStrides[81], + u32(idx), u32(ContactPointIdx))])); +} +fn SetContactA(idx: i32,bodIdx: i32) { + Contacts[Index2D(TensorStrides[100], TensorStrides[101], u32(idx), u32(ContactA))] = bitcast(u32(bodIdx)); +} +fn SetContactB(idx: i32,bodIdx: i32) { + Contacts[Index2D(TensorStrides[100], TensorStrides[101], u32(idx), u32(ContactB))] = bitcast(u32(bodIdx)); +} +fn SetContactPointIdx(idx: i32,ptIdx: i32) { + Contacts[Index2D(TensorStrides[100], TensorStrides[101], u32(idx), u32(ContactPointIdx))] = bitcast(u32(ptIdx)); +} +fn SetContactAPoint(idx: i32, pos: vec3) { + 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[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[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[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[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]; + var ci = i32(i); + var cmax = BroadContactsN[0]; + if (ci >= cmax) { + return; + } + 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 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, 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: { + } + } + } + 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[100], TensorStrides[101], u32(nci), u32(ContactAThick))] = offMagA; + Contacts[Index2D(TensorStrides[100], TensorStrides[101], u32(nci), u32(ContactBThick))] = offMagB; + Params[0] = params; +} + +//////// import: "control.go" +alias JointControlVars = i32; //enums:enum +const JointControlForce: JointControlVars = 0; +const JointTargetPos: JointControlVars = 1; +const JointTargetPosCur: JointControlVars = 2; +const JointTargetStiff: JointControlVars = 3; +const JointTargetVel: JointControlVars = 4; +const JointTargetDamp: JointControlVars = 5; + +//////// 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 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 = 6; +const DynamicVarsN: DynamicVars = 33; +const GPUVarsN: GPUVars = 13; +const JointTypesN: JointTypes = 8; +const JointVarsN: JointVars = 46; +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 JointParentFixed: JointVars = 2; +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; +const JointAxisZ: JointDoFVars = 2; +const JointLimitLower: JointDoFVars = 3; +const JointLimitUpper: JointDoFVars = 4; + +//////// import: "params.go" +struct PhysicsParams { + Iterations: i32, + Dt: f32, + SubSteps: i32, + ControlDt: f32, + ControlDtThr: f32, + 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, + MaxDelta: f32, + MaxGeomIter: i32, + ContactsMax: i32, + Cur: i32, + Next: i32, + BodiesN: i32, + DynamicsN: i32, + ObjectsN: i32, + MaxObjectJoints: i32, + JointsN: i32, + JointDoFsN: i32, + BodyJointsMax: i32, + BodyCollidePairsN: i32, + pad: i32, + pad1: i32, + pad2: 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, +} +fn NewGeomData(bi: i32,cni: i32, shp: Shapes) -> GeomData { + var gd: GeomData; + gd.BodyIdx = bi; + gd.Shape = shp; + 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 ((*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 - (*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((*gdB).WbR, (*gdB).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, 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)); + 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 - (*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); + 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, 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)); + 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 - (*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); + 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, 1, 0)); + if (width > 0 && length > 0) { + if (abs(queryB.x) > width || abs(queryB.z) > length) { + return f32( // invalid + 1e6); + } + } + } 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 + 1e6); + } + 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, 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 - (*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); + 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, 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, 1, 0)); + var plPos = MulSpatialPoint((*gdB).WbR, (*gdB).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; + 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 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; + 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)); + *norm = n; +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; + cp.y = f32(0); + if (width == 0.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 +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-math.go" +const Pi = 3.141592653589793; + +//////// 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 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)); + *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-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 { + 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 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/DynamicsCurToNext.wgsl b/physics/shaders/DynamicsCurToNext.wgsl new file mode 100644 index 00000000..64ee43b6 --- /dev/null +++ b/physics/shaders/DynamicsCurToNext.wgsl @@ -0,0 +1,323 @@ +// 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, 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; + 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 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 JointTargetPosCur: JointControlVars = 2; +const JointTargetStiff: JointControlVars = 3; +const JointTargetVel: JointControlVars = 4; +const JointTargetDamp: JointControlVars = 5; + +//////// 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 = 6; +const DynamicVarsN: DynamicVars = 33; +const GPUVarsN: GPUVars = 13; +const JointTypesN: JointTypes = 8; +const JointVarsN: JointVars = 46; +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 JointParentFixed: JointVars = 2; +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; +const JointAxisZ: JointDoFVars = 2; +const JointLimitLower: JointDoFVars = 3; +const JointLimitUpper: JointDoFVars = 4; + +//////// import: "params.go" +struct PhysicsParams { + Iterations: i32, + Dt: f32, + SubSteps: i32, + ControlDt: f32, + ControlDtThr: f32, + 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, + MaxDelta: f32, + MaxGeomIter: i32, + ContactsMax: i32, + Cur: i32, + Next: i32, + BodiesN: i32, + DynamicsN: i32, + ObjectsN: i32, + MaxObjectJoints: i32, + JointsN: i32, + JointDoFsN: i32, + BodyJointsMax: i32, + BodyCollidePairsN: i32, + pad: i32, + pad1: i32, + pad2: 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-math.go" +const Pi = 3.141592653589793; + +//////// import: "slmath-matrix3.go" + +//////// import: "slmath-quaternion.go" + +//////// import: "slmath-vector2.go" + +//////// import: "slmath-vector3.go" + +//////// import: "step.go" + +//////// import: "step_body.go" +fn DynamicsCurToNext(i: u32) { //gosl:kernel + var params = Params[0]; + var ii = i32(i); + if (ii >= params.DynamicsN) { + return; + } + for (var di = DynBody; + di < DynamicVarsN; 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; +} + +//////// import: "step_joint.go" \ No newline at end of file diff --git a/physics/shaders/ForcesFromJoints.wgsl b/physics/shaders/ForcesFromJoints.wgsl new file mode 100644 index 00000000..45ccebfa --- /dev/null +++ b/physics/shaders/ForcesFromJoints.wgsl @@ -0,0 +1,371 @@ +// 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(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; +// // 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; + ForcesFromJoints(idx); +} + +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" + +//////// 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 JointTargetPosCur: JointControlVars = 2; +const JointTargetStiff: JointControlVars = 3; +const JointTargetVel: JointControlVars = 4; +const JointTargetDamp: JointControlVars = 5; + +//////// 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 SetDynamicForce(idx: i32,cni: i32, force: vec3) { + 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[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" +const BodyVarsN: BodyVars = 43; +const ContactVarsN: ContactVars = 33; +const JointControlVarsN: JointControlVars = 6; +const DynamicVarsN: DynamicVars = 33; +const GPUVarsN: GPUVars = 13; +const JointTypesN: JointTypes = 8; +const JointVarsN: JointVars = 46; +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 JointParentFixed: JointVars = 2; +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))]); +} +fn JointPTorque(idx: i32) -> vec3 { + 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[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[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; +const JointAxisY: JointDoFVars = 1; +const JointAxisZ: JointDoFVars = 2; +const JointLimitLower: JointDoFVars = 3; +const JointLimitUpper: JointDoFVars = 4; + +//////// import: "params.go" +struct PhysicsParams { + Iterations: i32, + Dt: f32, + SubSteps: i32, + ControlDt: f32, + ControlDtThr: f32, + 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, + MaxDelta: f32, + MaxGeomIter: i32, + ContactsMax: i32, + Cur: i32, + Next: i32, + BodiesN: i32, + DynamicsN: i32, + ObjectsN: i32, + MaxObjectJoints: i32, + JointsN: i32, + JointDoFsN: i32, + BodyJointsMax: i32, + BodyCollidePairsN: i32, + pad: i32, + pad1: i32, + pad2: 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-math.go" +const Pi = 3.141592653589793; + +//////// import: "slmath-matrix3.go" + +//////// import: "slmath-quaternion.go" + +//////// import: "slmath-vector2.go" + +//////// import: "slmath-vector3.go" + +//////// import: "step.go" + +//////// import: "step_body.go" +fn ForcesFromJoints(i: u32) { //gosl:kernel + var 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); + 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 new file mode 100644 index 00000000..28143271 --- /dev/null +++ b/physics/shaders/InitDynamics.wgsl @@ -0,0 +1,341 @@ +// 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; +// // 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; + InitDynamics(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 JointTargetPosCur: JointControlVars = 2; +const JointTargetStiff: JointControlVars = 3; +const JointTargetVel: JointControlVars = 4; +const JointTargetDamp: JointControlVars = 5; + +//////// 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[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 = 6; +const DynamicVarsN: DynamicVars = 33; +const GPUVarsN: GPUVars = 13; +const JointTypesN: JointTypes = 8; +const JointVarsN: JointVars = 46; +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 JointParentFixed: JointVars = 2; +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; +const JointAxisZ: JointDoFVars = 2; +const JointLimitLower: JointDoFVars = 3; +const JointLimitUpper: JointDoFVars = 4; + +//////// import: "params.go" +struct PhysicsParams { + Iterations: i32, + Dt: f32, + SubSteps: i32, + ControlDt: f32, + ControlDtThr: f32, + 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, + MaxDelta: f32, + MaxGeomIter: i32, + ContactsMax: i32, + Cur: i32, + Next: i32, + BodiesN: i32, + DynamicsN: i32, + ObjectsN: i32, + MaxObjectJoints: i32, + JointsN: i32, + JointDoFsN: i32, + BodyJointsMax: i32, + BodyCollidePairsN: i32, + pad: i32, + pad1: i32, + pad2: 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-math.go" +const Pi = 3.141592653589793; + +//////// import: "slmath-matrix3.go" + +//////// import: "slmath-quaternion.go" + +//////// import: "slmath-vector2.go" + +//////// import: "slmath-vector3.go" + +//////// import: "step.go" + +//////// import: "step_body.go" +fn InitDynamics(i: u32) { //gosl:kernel + var params = Params[0]; + var ii = i32(i); + if (ii >= params.DynamicsN) { + return; + } + for (var cni=0; cni<2; cni++) { + var bi = DynamicBody(ii); + 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[60], TensorStrides[61], TensorStrides[62], + 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/StepBodyContactDeltas.wgsl b/physics/shaders/StepBodyContactDeltas.wgsl new file mode 100644 index 00000000..f4084ef9 --- /dev/null +++ b/physics/shaders/StepBodyContactDeltas.wgsl @@ -0,0 +1,551 @@ +// 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[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[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[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[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[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]; + 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[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 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))]); +} +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 = 6; +const DynamicVarsN: DynamicVars = 33; +const GPUVarsN: GPUVars = 13; +const JointTypesN: JointTypes = 8; +const JointVarsN: JointVars = 46; +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 JointParentFixed: JointVars = 2; +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; +const JointAxisZ: JointDoFVars = 2; +const JointLimitLower: JointDoFVars = 3; +const JointLimitUpper: JointDoFVars = 4; + +//////// import: "params.go" +struct PhysicsParams { + Iterations: i32, + Dt: f32, + SubSteps: i32, + ControlDt: f32, + ControlDtThr: f32, + 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, + MaxDelta: f32, + MaxGeomIter: i32, + ContactsMax: i32, + Cur: i32, + Next: i32, + BodiesN: i32, + DynamicsN: i32, + ObjectsN: i32, + MaxObjectJoints: i32, + JointsN: i32, + JointDoFsN: i32, + BodyJointsMax: i32, + BodyCollidePairsN: i32, + pad: i32, + pad1: i32, + pad2: 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-math.go" +const Pi = 3.141592653589793; + +//////// 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; +} +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); +} +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); + 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))); + 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); + 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 { + 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 new file mode 100644 index 00000000..9186f72e --- /dev/null +++ b/physics/shaders/StepBodyContacts.wgsl @@ -0,0 +1,578 @@ +// 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 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[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]; +; if (ci >= cmax) { + return; +}; var biA = GetContactA(ci); +; var biB = GetContactB(ci); +; var diA = GetBodyDynamic(biA); +; var diB = GetBodyDynamic(biB); +; 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(r1A, q1A, ctA); +; var ctBw = MulSpatialPoint(r1B, q1B, ctB); +; 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[100], TensorStrides[101], 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 w1A: vec3; +var w1B: vec3;; if (diA >= 0) { + w1A = DynamicAngDelta(diA, params.Next); +}; if (diB >= 0) { + 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 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, 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) { + 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); + 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); + lambdaFr = max(lambdaFr, -lambdaN*mu); + linDeltaA = linDeltaA-(perp*(lambdaFr)); + linDeltaB = linDeltaB+(perp*(lambdaFr)); + angDeltaA = angDeltaA+(angA*(lambdaFr)); + angDeltaB = angDeltaB+(angB*(lambdaFr)); + } +}; 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, 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)); + } +}; 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, 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, norm)); // 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 = norm*(mInvB * dv); + var dwB = MulQuatVector(q0B, iInvB*(dBnew)*(dv)); + linDeltaB = linDeltaB+(dvB); + angDeltaB = angDeltaB+(dwB); + } + } +}; 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; + 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 JointTargetPosCur: JointControlVars = 2; +const JointTargetStiff: JointControlVars = 3; +const JointTargetVel: JointControlVars = 4; +const JointTargetDamp: JointControlVars = 5; + +//////// 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 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))]); +} +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 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))]); +} + +//////// import: "enumgen.go" +const BodyVarsN: BodyVars = 43; +const ContactVarsN: ContactVars = 33; +const JointControlVarsN: JointControlVars = 6; +const DynamicVarsN: DynamicVars = 33; +const GPUVarsN: GPUVars = 13; +const JointTypesN: JointTypes = 8; +const JointVarsN: JointVars = 46; +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 JointParentFixed: JointVars = 2; +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; +const JointAxisZ: JointDoFVars = 2; +const JointLimitLower: JointDoFVars = 3; +const JointLimitUpper: JointDoFVars = 4; + +//////// import: "params.go" +struct PhysicsParams { + Iterations: i32, + Dt: f32, + SubSteps: i32, + ControlDt: f32, + ControlDtThr: f32, + 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, + MaxDelta: f32, + MaxGeomIter: i32, + ContactsMax: i32, + Cur: i32, + Next: i32, + BodiesN: i32, + DynamicsN: i32, + ObjectsN: i32, + MaxObjectJoints: i32, + JointsN: i32, + JointDoFsN: i32, + BodyJointsMax: i32, + BodyCollidePairsN: i32, + pad: i32, + pad1: i32, + pad2: 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-math.go" +const Pi = 3.141592653589793; + +//////// 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" +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/StepIntegrateBodies.wgsl b/physics/shaders/StepIntegrateBodies.wgsl new file mode 100644 index 00000000..8b4f4d99 --- /dev/null +++ b/physics/shaders/StepIntegrateBodies.wgsl @@ -0,0 +1,449 @@ +// 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, 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; + 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 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 JointTargetPosCur: JointControlVars = 2; +const JointTargetStiff: JointControlVars = 3; +const JointTargetVel: JointControlVars = 4; +const JointTargetDamp: JointControlVars = 5; + +//////// 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[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 = 6; +const DynamicVarsN: DynamicVars = 33; +const GPUVarsN: GPUVars = 13; +const JointTypesN: JointTypes = 8; +const JointVarsN: JointVars = 46; +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 JointParentFixed: JointVars = 2; +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; +const JointAxisZ: JointDoFVars = 2; +const JointLimitLower: JointDoFVars = 3; +const JointLimitUpper: JointDoFVars = 4; + +//////// import: "params.go" +struct PhysicsParams { + Iterations: i32, + Dt: f32, + SubSteps: i32, + ControlDt: f32, + ControlDtThr: f32, + 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, + MaxDelta: f32, + MaxGeomIter: i32, + ContactsMax: i32, + Cur: i32, + Next: i32, + BodiesN: i32, + DynamicsN: i32, + ObjectsN: i32, + MaxObjectJoints: i32, + JointsN: i32, + JointDoFsN: i32, + BodyJointsMax: i32, + BodyCollidePairsN: i32, + pad: i32, + pad1: i32, + pad2: 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-math.go" +const Pi = 3.141592653589793; + +//////// 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; +} +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-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); +} + +//////// 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 + 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))]; + 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); + 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)+(r0); + 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 + 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); + 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); + 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 new file mode 100644 index 00000000..aeebf7cb --- /dev/null +++ b/physics/shaders/StepJointForces.wgsl @@ -0,0 +1,491 @@ +// 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(3) +var Joints: array; +@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) +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] +@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 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))]); +} + +//////// 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 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))]; +} + +//////// 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[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 = 6; +const DynamicVarsN: DynamicVars = 33; +const GPUVarsN: GPUVars = 13; +const JointTypesN: JointTypes = 8; +const JointVarsN: JointVars = 46; +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 JointParentFixed: JointVars = 2; +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))])); +} +fn GetJointEnabled(idx: i32) -> bool { + 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[30], TensorStrides[31], u32(idx), u32(JointParent))])); +} +fn JointChildIndex(idx: i32) -> i32 { + return i32(bitcast(Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointChild))])); +} +fn GetJointLinearDoFN(idx: i32) -> i32 { + return i32(bitcast(Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointLinearDoFN))])); +} +fn GetJointAngularDoFN(idx: i32) -> i32 { + 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[30], TensorStrides[31], u32(idx), u32(i32(JointDoF1) + dof))])); +} +fn JointPPos(idx: i32) -> vec3 { + 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[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[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[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[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[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; +const JointAxisY: JointDoFVars = 1; +const JointAxisZ: JointDoFVars = 2; +const JointLimitLower: JointDoFVars = 3; +const JointLimitUpper: JointDoFVars = 4; +fn JointAxisDoF(didx: i32) -> vec3 { + 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)); +} + +//////// import: "params.go" +struct PhysicsParams { + Iterations: i32, + Dt: f32, + SubSteps: i32, + ControlDt: f32, + ControlDtThr: f32, + 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, + MaxDelta: f32, + MaxGeomIter: i32, + ContactsMax: i32, + Cur: i32, + Next: i32, + BodiesN: i32, + DynamicsN: i32, + ObjectsN: i32, + MaxObjectJoints: i32, + JointsN: i32, + JointDoFsN: i32, + BodyJointsMax: i32, + BodyCollidePairsN: i32, + pad: i32, + pad1: i32, + pad2: 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-math.go" +const Pi = 3.141592653589793; + +//////// 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 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 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); +} + +//////// 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); +} + +//////// import: "step.go" + +//////// import: "step_body.go" + +//////// import: "step_joint.go" +fn StepJointForces(i: u32) { //gosl:kernel + var params = Params[0]; + var ji = i32(i); + 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; + } + var jPi = JointParentIndex(ji); + var jPbi = i32(-1); + if (jPi >= 0) { + jPbi = DynamicBody(jPi); + } + 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; + 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(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(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 = MulQuatVector(xwPQ, axis)*(JointControl(ji, i32(i32(0)), JointControlForce)); + } + case Prismatic: { + var axis = JointAxis(ji, i32(i32(0))); + f = MulQuatVector(xwPQ, axis)*(JointControl(ji, i32(i32(0)), JointControlForce)); + } + default: { + for (var dof=0; dof 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 Objects: array; +@group(1) @binding(3) +var Joints: array; +@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) +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] +@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 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 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))]; +} + +//////// 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[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 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))]); +} +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 = 6; +const DynamicVarsN: DynamicVars = 33; +const GPUVarsN: GPUVars = 13; +const JointTypesN: JointTypes = 8; +const JointVarsN: JointVars = 46; +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 JointParentFixed: JointVars = 2; +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))])); +} +fn GetJointEnabled(idx: i32) -> bool { + 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 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))])); +} +fn JointChildIndex(idx: i32) -> i32 { + return i32(bitcast(Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointChild))])); +} +fn GetJointLinearDoFN(idx: i32) -> i32 { + return i32(bitcast(Joints[Index2D(TensorStrides[30], TensorStrides[31], u32(idx), u32(JointLinearDoFN))])); +} +fn GetJointAngularDoFN(idx: i32) -> i32 { + 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[30], TensorStrides[31], u32(idx), u32(i32(JointDoF1) + dof))])); +} +fn JointPPos(idx: i32) -> vec3 { + 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[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[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[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; +const JointAxisZ: JointDoFVars = 2; +const JointLimitLower: JointDoFVars = 3; +const JointLimitUpper: JointDoFVars = 4; +fn JointAxisDoF(didx: i32) -> vec3 { + 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[40], TensorStrides[41], u32(JointDoFIndex(idx, dof)), u32(vr))]; +} + +//////// import: "params.go" +struct PhysicsParams { + Iterations: i32, + Dt: f32, + SubSteps: i32, + ControlDt: f32, + ControlDtThr: f32, + 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, + MaxDelta: f32, + MaxGeomIter: i32, + ContactsMax: i32, + Cur: i32, + Next: i32, + BodiesN: i32, + DynamicsN: i32, + ObjectsN: i32, + MaxObjectJoints: i32, + JointsN: i32, + JointDoFsN: i32, + BodyJointsMax: i32, + BodyCollidePairsN: i32, + pad: i32, + pad1: i32, + pad2: 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-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" +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; +} +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); +} +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); +} +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-vector2.go" + +//////// import: "slmath-vector3.go" +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; +} +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 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); +} +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 == 2) { + nv.z = val; + }return nv; +} + +//////// 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); + 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))); + 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); + 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 { + var l = Length3(v); + if (l < lim) { + return v; + }return v*((lim / l)); +} + +//////// import: "step_joint.go" +fn StepSolveJoints(i: u32) { //gosl:kernel + var params = Params[0]; + var oi = i32(i); + if (oi >= params.ObjectsN) { + 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; + } + StepSolveJoint(ji); + } + Params[0] = params; +} +fn StepSolveJoint(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 noLinearRot = GetJointNoLinearRotation(ji); + var jLinearN = GetJointLinearDoFN(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 + 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); + 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 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 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); + var lambdaPrev = JointLinLambda(ji); + var lambdaNext = vec3(0, 0, 0); + 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); // 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); + 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)); + linDeltaC = linDeltaC+(linearC*(dLambda * params.JointLinearRelax)); + if (!noLinearRot) { + angDeltaP = angDeltaP+(angularP*(dLambda * params.JointAngularRelax)); + angDeltaC = angDeltaC+(angularC*(dLambda * params.JointAngularRelax)); + } + } + } else { + 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; + 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 = 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; + } + } + if (abs(err) > 1e-9 || abs(derrRel) > 1e-9) { + 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)); + if (!noLinearRot) { + angDeltaP = angDeltaP+(angularP*(dLambda * params.JointAngularRelax)); + angDeltaC = angDeltaC+(angularC*(dLambda * params.JointAngularRelax)); + } + lambdaNext = SetDim3(lambdaNext, dim, dLambda); + } + } + } + SetJointLinLambda(ji, lambdaNext); + var jAngularN = GetJointAngularDoFN(ji); + 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; + lambdaPrev = JointAngLambda(ji); + lambdaNext = vec3(0, 0, 0); + _ = lambdaPrev; + 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 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 = MinAngleDiff(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)); + lambdaNext = SetDim3(lambdaNext, dim, dLambda); + } + SetJointAngLambda(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 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, linA: vec3,linB: vec3,angA: vec3,angB: vec3, lambdaIn: f32,compliance: f32,damping: f32,dt: f32) -> f32 { + var denom = f32(0.0); + 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 lambda = -(err + alpha*lambdaIn + gamma*derr); + if (denom+alpha > 0.0) { + lambda /= (dt+gamma)*denom + alpha/dt; + }return lambda; +} +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(rotAngA, iInvA*(rotAngA)); + denom += Dot3(rotAngB, iInvB*(rotAngB)); + 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(*axisLimitsA, up); + } +} \ No newline at end of file diff --git a/physics/shapecollide.go b/physics/shapecollide.go new file mode 100644 index 00000000..1bc61174 --- /dev/null +++ b/physics/shapecollide.go @@ -0,0 +1,416 @@ +// 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. + +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 = 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 gd.Shape == Sphere || gd.Shape == Capsule || gd.Shape == Cone { + gd.Radius = gd.Size.X + } +} + +// 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 +// 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 + +// pAw = point in A, world coords; b = body coords + +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, maxIter int32, gdA *GeomData, gdB *GeomData, pA, pB, norm *math32.Vector3) float32 { + var pAw, pBw, diff math32.Vector3 + 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(gdB.WbR, gdB.WbQ, pBb) + diff = pAw.Sub(pBw) + if gdB.Size.X > 0 { + *norm = slmath.Normal3(diff) + } else { + *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 + 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, 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)) + 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)) + } + *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 - 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)) + 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, 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)) + 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 - 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) + 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) + 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, 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 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: + // + // // 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 1e6 // 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, 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 - 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)) + 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, 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, 1, 0)) + plPos := slmath.MulSpatialPoint(gdB.WbR, gdB.WbQ, math32.Vec3(0, 0, 0)) + + // World-space cylinder params + cylCtr := gdA.WbR + cylAx := slmath.Normal3(slmath.MulQuatVector(gdA.WbQ, math32.Vec3(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) + lenSqr := slmath.Dot3(vec, vec) + + // If vector is nondegenerate, normalize and scale by radius + // Otherwise use cylinder's x-axis scaled by radius + 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) + + // 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)) + *norm = n + 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 new file mode 100644 index 00000000..7e8c08b5 --- /dev/null +++ b/physics/shapecollide.goal @@ -0,0 +1,413 @@ +// 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 = 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 gd.Shape == Sphere || gd.Shape == Capsule || gd.Shape == Cone { + gd.Radius = gd.Size.X + } +} + +// 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 +// 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 + +// pAw = point in A, world coords; b = body coords + +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, maxIter int32, gdA *GeomData, gdB *GeomData, pA, pB, norm *math32.Vector3) float32 { + var pAw, pBw, diff math32.Vector3 + 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(gdB.WbR, gdB.WbQ, pBb) + diff = pAw.Sub(pBw) + if gdB.Size.X > 0 { + *norm = slmath.Normal3(diff) + } else { + *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 + 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, 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)) + 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)) + } + *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 - 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)) + 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, 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)) + 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 - 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) + 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) + 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, 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 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: + // // 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 1e6 // 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, 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 - 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)) + 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, 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, 1, 0)) + plPos := slmath.MulSpatialPoint(gdB.WbR, gdB.WbQ, math32.Vec3(0, 0, 0)) + + // World-space cylinder params + cylCtr := gdA.WbR + cylAx := slmath.Normal3(slmath.MulQuatVector(gdA.WbQ, math32.Vec3(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) + lenSqr := slmath.Dot3(vec, vec) + + // If vector is nondegenerate, normalize and scale by radius + // Otherwise use cylinder's x-axis scaled by radius + 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) + + // 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)) + *norm = n + 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/shapegeom.go b/physics/shapegeom.go new file mode 100644 index 00000000..b3db709d --- /dev/null +++ b/physics/shapegeom.go @@ -0,0 +1,315 @@ +// 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 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 + } + 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 + cp.Y = 0 + if width == 0.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) { + eid := edgeId + if eid < 4 { + // edges along x: 0-1, 2-3, 4-5, 6-7 + i := eid * 2 + j := i + 1 + *edge0 = BoxVertex(i, upper) + *edge1 = BoxVertex(j, upper) + } else if eid < 8 { + // edges along y: 0-2, 1-3, 4-6, 5-7 + 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 + eid -= 8 + i := eid + 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 = math32.Vec3(p0x, 0, p0z) + *edge1 = math32.Vec3(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/shapegeom_test.go b/physics/shapegeom_test.go new file mode 100644 index 00000000..041c4352 --- /dev/null +++ b/physics/shapegeom_test.go @@ -0,0 +1,296 @@ +// 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 ( + "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.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} + InitGeomData(0, &gdA) + InitGeomData(0, &gdB) + + var ptA, ptB, norm math32.Vector3 + dist := ColSphereSphere(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(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) + 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 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{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 + } + 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, 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) + } +} + +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) + } +} diff --git a/physics/shapes.go b/physics/shapes.go new file mode 100644 index 00000000..e089ae8e --- /dev/null +++ b/physics/shapes.go @@ -0,0 +1,258 @@ +// 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" +) + +// see: newton/geometry for lots of helpful methods. + +//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 +// center-based body coordinates. +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 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. + Capsule + + // todo: Ellipsoid goes here + + // 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. + Cylinder + + // 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 + +// 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: // note: Cone has no collision points! + 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: + return sz.Y // full half-height + case Cylinder: + return sz.X + 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 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), math32.Vec3(sz.X, sz.Y, sz.X)) + case Cylinder: + 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) + } + 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 { + // todo: other shapes!! see below. + 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 Capsule: + r := sz.X + 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)) + 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 + 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) + } + return inertia +} + +/* + +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) + +*/ diff --git a/physics/step.go b/physics/step.go new file mode 100644 index 00000000..5e7c525d --- /dev/null +++ b/physics/step.go @@ -0,0 +1,163 @@ +// 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. + +// 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 ( + "fmt" + + "cogentcore.org/core/math32" +) + +//gosl:start +//gosl:import "cogentcore.org/lab/gosl/slmath" + +func OneIfNonzero(f float32) float32 { + if f != 0.0 { + return 1.0 + } + 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 + } + for j := range params.JointDoFsN { + tpos := JointControls.Value(int(j), int(JointTargetPos)) + tcur := JointControls.Value(int(j), int(JointTargetPosCur)) + if math32.Abs(tpos-tcur) < params.ControlDtThr { + tcur = tpos + } else { + tcur += params.ControlDt * (tpos - tcur) + } + JointControls.Set(tcur, int(j), int(JointTargetPosCur)) + } +} + +// 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) +// 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, + +//gosl:end + +// 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 (ml *Model) Step() { + params := GetParams(0) + ToGPU(ParamsVar, JointControlsVar) + if params.SubSteps > 1 { + for range params.SubSteps - 1 { + ml.StepGet() + } + } + 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) + } + if ml.ReportTotalKE { + ke := ml.TotalKineticEnergy() + fmt.Println("Total KE:", ke) + } +} + +// StepGet runs one physics step and gets the given vars back +// from the GPU. +func (ml *Model) StepGet(vars ...GPUVars) { + params := GetParams(0) + RunStepInit(1) + ml.StepCollision() + ml.StepJointForces() + ml.StepIntegrateBodies() + + for range params.Iterations { + ml.StepSolveJoints() + ml.StepBodyContacts() + } + RunDone(vars...) +} + +func (ml *Model) StepCollision() { + params := GetParams(0) + RunCollisionBroad(int(params.BodyCollidePairsN)) + // note: time getting BroadContactsN back down and using that vs. running full + RunCollisionNarrow(int(params.ContactsMax)) + // 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) +} + +func (ml *Model) StepJointForces() { + params := GetParams(0) + RunStepJointForces(int(params.JointsN)) + RunForcesFromJoints(int(params.DynamicsN)) +} + +func (ml *Model) StepIntegrateBodies() { + params := GetParams(0) + RunStepIntegrateBodies(int(params.DynamicsN)) +} + +func (ml *Model) StepSolveJoints() { + params := GetParams(0) + RunStepSolveJoints(int(params.ObjectsN)) +} + +func (ml *Model) StepBodyContacts() { + params := GetParams(0) + if !ml.GPU { + cmax := int(ContactsN.Values[0]) + if cmax > 0 { + RunStepBodyContacts(cmax) + } + 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 new file mode 100644 index 00000000..fa8e631e --- /dev/null +++ b/physics/step.goal @@ -0,0 +1,161 @@ +// 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 ( + "fmt" + + "cogentcore.org/core/math32" +) + +//gosl:start +//gosl:import "cogentcore.org/lab/gosl/slmath" + +func OneIfNonzero(f float32) float32 { + if f != 0.0 { + return 1.0 + } + 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 + } + for j := range params.JointDoFsN { + tpos := JointControls[j, JointTargetPos] + tcur := JointControls[j, JointTargetPosCur] + if math32.Abs(tpos-tcur) < params.ControlDtThr { + tcur = tpos + } else { + tcur += params.ControlDt * (tpos - tcur) + } + JointControls[j, JointTargetPosCur] = tcur + } +} + +// 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) +// 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, + +//gosl:end + +// 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 (ml *Model) Step() { + params := GetParams(0) + ToGPU(ParamsVar, JointControlsVar) + if params.SubSteps > 1 { + for range params.SubSteps - 1 { + ml.StepGet() + } + } + 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) + } + if ml.ReportTotalKE { + ke := ml.TotalKineticEnergy() + fmt.Println("Total KE:", ke) + } +} + +// StepGet runs one physics step and gets the given vars back +// from the GPU. +func (ml *Model) StepGet(vars ...GPUVars) { + params := GetParams(0) + RunStepInit(1) + ml.StepCollision() + ml.StepJointForces() + ml.StepIntegrateBodies() + + for range params.Iterations { + ml.StepSolveJoints() + ml.StepBodyContacts() + } + RunDone(vars...) +} + +func (ml *Model) StepCollision() { + params := GetParams(0) + RunCollisionBroad(int(params.BodyCollidePairsN)) + // note: time getting BroadContactsN back down and using that vs. running full + RunCollisionNarrow(int(params.ContactsMax)) + // 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) +} + +func (ml *Model) StepJointForces() { + params := GetParams(0) + RunStepJointForces(int(params.JointsN)) + RunForcesFromJoints(int(params.DynamicsN)) +} + +func (ml *Model) StepIntegrateBodies() { + params := GetParams(0) + RunStepIntegrateBodies(int(params.DynamicsN)) +} + +func (ml *Model) StepSolveJoints() { + params := GetParams(0) + RunStepSolveJoints(int(params.ObjectsN)) +} + +func (ml *Model) StepBodyContacts() { + params := GetParams(0) + if !ml.GPU { + cmax := int(ContactsN.Values[0]) + if cmax > 0 { + RunStepBodyContacts(cmax) + } + 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 new file mode 100644 index 00000000..5877fbab --- /dev/null +++ b/physics/step_body.go @@ -0,0 +1,274 @@ +// 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 ( + // "fmt" + "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 + params := GetParams(0) + ii := int32(i) + if ii >= params.DynamicsN { + return + } + for cni := range 2 { + 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)) + + 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)) + } + } +} + +// DynamicsCurToNext copies [Dynamics] state from Cur to Next. +func DynamicsCurToNext(i uint32) { //gosl:kernel + params := GetParams(0) + ii := int32(i) + if ii >= params.DynamicsN { + return + } + for di := DynBody; di < DynamicVarsN; di++ { + Dynamics.Set(Dynamics.Value(int(ii), int(params.Cur), int(di)), int(ii), int(params.Next), int(di)) + } +} + +// 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) +} + +// newton: solvers/solver.py: integrate_rigid_body + +// StepIntegrateBodies applies forces to update pos and deltas +func StepIntegrateBodies(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)) + inertia := BodyInertia(bi) + invInertia := BodyInvInertia(bi) + grav := params.Gravity.V() + + com := BodyCom(bi) + + // current pos + r0 := DynamicPos(di, params.Cur) + q0 := DynamicQuat(di, params.Cur) + + // current deltas + v0 := DynamicDelta(di, params.Cur) + w0 := DynamicAngDelta(di, params.Cur) + + // new forces integrated from joints + f0 := DynamicForce(di, params.Next) + t0 := DynamicTorque(di, params.Next) + + pcom := slmath.MulQuatVector(q0, com).Add(r0) + + // linear part + 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) + 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) + SetDynamicAngDelta(di, params.Next, w1) +} + +// newton: solvers/xpbd/kernels.py: apply_body_deltas + +// StepBodyDeltas updates Next position with deltas from joints +// 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) + + invMass := Bodies.Value(int(bi), int(BodyInvMass)) + inertia := BodyInertia(bi) + invInertia := BodyInvInertia(bi) + + // starting pos (from force integration) + r0 := DynamicPos(di, params.Next) + q0 := DynamicQuat(di, params.Next) + + // starting deltas + v0 := DynamicDelta(di, params.Next) + w0 := DynamicAngDelta(di, params.Next) + + weight := float32(1.0) + if contacts && params.ContactWeighting.IsTrue() { + if cWt > 0 { + weight = 1.0 / cWt + } + } + + // weighted + dp := linDel.MulScalar(invMass * weight) + dq := angDel.MulScalar(weight) + + // note: this is essential for rationalizing PlaneXZ and ball collision behavior! + dp = LimitDelta(dp, params.MaxDelta) + dq = LimitDelta(dq, params.MaxDelta) + + 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(params.Dt))) + + // update orientation + 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) + + // update position + com := BodyCom(bi) + pcom := slmath.MulQuatVector(q0, com).Add(r0) + + p1 := pcom.Add(dp.MulScalar(params.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, 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 +func LimitDelta(v math32.Vector3, lim float32) math32.Vector3 { + l := slmath.Length3(v) + if l < lim { + return v + } + return v.MulScalar((lim / l)) +} + +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 new file mode 100644 index 00000000..33c7af37 --- /dev/null +++ b/physics/step_body.goal @@ -0,0 +1,274 @@ +// 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 ( + // "fmt" + "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 + params := GetParams(0) + ii := int32(i) + if ii >= params.DynamicsN { + return + } + for cni := range 2 { + bi := DynamicBody(ii) + Dynamics[ii, cni, DynPosX] = Bodies[bi, BodyPosX] + Dynamics[ii, cni, DynPosY] = Bodies[bi, BodyPosY] + Dynamics[ii, cni, DynPosZ] = Bodies[bi, BodyPosZ] + + 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 + } + } +} + +// DynamicsCurToNext copies [Dynamics] state from Cur to Next. +func DynamicsCurToNext(i uint32) { //gosl:kernel + params := GetParams(0) + ii := int32(i) + if ii >= params.DynamicsN { + return + } + for di := DynBody; di < DynamicVarsN; di++ { + Dynamics[ii, params.Next, di] = Dynamics[ii, params.Cur, di] + } +} + +// 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) +} + +// newton: solvers/solver.py: integrate_rigid_body + +// StepIntegrateBodies applies forces to update pos and deltas +func StepIntegrateBodies(i uint32) { //gosl:kernel + params := GetParams(0) + di := int32(i) + if di >= params.DynamicsN { + return + } + bi := DynamicBody(di) + + invMass := Bodies[bi, BodyInvMass] + inertia := BodyInertia(bi) + invInertia := BodyInvInertia(bi) + grav := params.Gravity.V() + + com := BodyCom(bi) + + // current pos + r0 := DynamicPos(di, params.Cur) + q0 := DynamicQuat(di, params.Cur) + + // current deltas + v0 := DynamicDelta(di, params.Cur) + w0 := DynamicAngDelta(di, params.Cur) + + // new forces integrated from joints + f0 := DynamicForce(di, params.Next) + t0 := DynamicTorque(di, params.Next) + + pcom := slmath.MulQuatVector(q0, com).Add(r0) + + // linear part + 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) + 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) + SetDynamicAngDelta(di, params.Next, w1) +} + +// newton: solvers/xpbd/kernels.py: apply_body_deltas + +// StepBodyDeltas updates Next position with deltas from joints +// 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) + + invMass := Bodies[bi, BodyInvMass] + inertia := BodyInertia(bi) + invInertia := BodyInvInertia(bi) + + // starting pos (from force integration) + r0 := DynamicPos(di, params.Next) + q0 := DynamicQuat(di, params.Next) + + // starting deltas + v0 := DynamicDelta(di, params.Next) + w0 := DynamicAngDelta(di, params.Next) + + weight := float32(1.0) + if contacts && params.ContactWeighting.IsTrue() { + if cWt > 0 { + weight = 1.0 / cWt + } + } + + // weighted + dp := linDel.MulScalar(invMass * weight) + dq := angDel.MulScalar(weight) + + // note: this is essential for rationalizing PlaneXZ and ball collision behavior! + dp = LimitDelta(dp, params.MaxDelta) + dq = LimitDelta(dq, params.MaxDelta) + + 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(params.Dt))) + + // update orientation + 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) + + // update position + com := BodyCom(bi) + pcom := slmath.MulQuatVector(q0, com).Add(r0) + + p1 := pcom.Add(dp.MulScalar(params.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, 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 +func LimitDelta(v math32.Vector3, lim float32) math32.Vector3 { + l := slmath.Length3(v) + if l < lim { + return v + } + return v.MulScalar((lim / l)) +} + +func VelocityAtPoint(lin, ang, r math32.Vector3) math32.Vector3 { + return lin.Add(slmath.Cross3(ang, r)) +} + +//gosl:end + + diff --git a/physics/step_joint.go b/physics/step_joint.go new file mode 100644 index 00000000..4b0fe754 --- /dev/null +++ b/physics/step_joint.go @@ -0,0 +1,575 @@ +// 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 ( + // "fmt" + "cogentcore.org/core/math32" + "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" + +// newton: solvers/xpbd/kernels.py: apply_joint_forces + +// StepJointForces computes joint forces. +func StepJointForces(i uint32) { //gosl:kernel + params := GetParams(0) + ji := int32(i) + 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 + } + + jPi := JointParentIndex(ji) + jPbi := int32(-1) + if jPi >= 0 { + jPbi = DynamicBody(jPi) + } + jCi := JointChildIndex(ji) + jCbi := DynamicBody(jCi) + + jLinearN := GetJointLinearDoFN(ji) + jAngularN := GetJointAngularDoFN(ji) + + jPR := JointPPos(ji) + jPQ := JointPQuat(ji) + + // parent world transform + 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) + } + dP := xwPR.Sub(slmath.MulSpatialPoint(posePR, posePQ, comP)) // parent moment arm + + // child world transform + 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(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: + // 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.MulQuatVector(xwPQ, axis).MulScalar(JointControl(ji, 0, JointControlForce)) + case Prismatic: + axis := JointAxis(ji, 0) + f = slmath.MulQuatVector(xwPQ, axis).MulScalar(JointControl(ji, 0, JointControlForce)) + default: + for dof := range jLinearN { + axis := JointAxis(ji, int32(dof)) + 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.MulQuatVector(xwPQ, axis).MulScalar(JointControl(ji, di, JointControlForce))) + } + } + // These are unique to joint: aggregate into dynamics Next in [ForcesFromJoints] + SetJointPForce(ji, slmath.Negate3(f)) + SetJointCForce(ji, 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 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) + 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 + } + StepSolveJoint(ji) + } +} + +// StepSolveJoint applies target positions to linear DoFs. +// Position is updated prior to computing angulars. +func StepSolveJoint(ji int32) { + params := GetParams(0) + + jt := GetJointType(ji) + jPi := JointParentIndex(ji) + jPbi := int32(-1) + parentFixed := true + if jPi >= 0 { + jPbi = DynamicBody(jPi) + parentFixed = GetJointParentFixed(ji) + } + jCi := JointChildIndex(ji) + jCbi := DynamicBody(jCi) + noLinearRot := GetJointNoLinearRotation(ji) + + 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 comP, vP, 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) + 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 + 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 + + relPoseR := xwPR + relPoseQ := xwPQ + slmath.SpatialTransformInverse(xwPR, xwPQ, &relPoseR, &relPoseQ) + slmath.MulSpatialTransforms(relPoseR, relPoseQ, xwCR, xwCQ, &relPoseR, &relPoseQ) + + 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) + 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) // note: multiple iter is supposed to increment these + compliance := params.JointLinearComply + ke := JointControl(ji, 0, JointTargetStiff) + kd := JointControl(ji, 0, JointTargetDamp) + 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)) + linDeltaC = linDeltaC.Add(linearC.MulScalar(dLambda * params.JointLinearRelax)) + if !noLinearRot { + angDeltaP = angDeltaP.Add(angularP.MulScalar(dLambda * params.JointAngularRelax)) + angDeltaC = angDeltaC.Add(angularC.MulScalar(dLambda * params.JointAngularRelax)) + } + } + } else { + // all joints impose linear constraints! + var axisLimitsD, axisLimitsA math32.Vector3 + var axisTargetPosKeD, axisTargetPosKeA math32.Vector3 + var axisTargetVelKdD, axisTargetVelKdA math32.Vector3 + + for dof := range jLinearN { + axis := JointAxis(ji, dof) + 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, JointTargetPosCur) + 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) + } + } + + axisStiffness := axisTargetPosKeA + axisDamping := axisTargetVelKdA + axisTargetPosKeD = slmath.DivSafe3(axisTargetPosKeD, axisStiffness) + axisTargetVelKdD = slmath.DivSafe3(axisTargetVelKdD, axisDamping) + axisLimitsLower := axisLimitsD + axisLimitsUpper := axisLimitsA + // 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 + // 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, 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 := 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) + + 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 + } + } + if math32.Abs(err) > 1e-9 || math32.Abs(derrRel) > 1e-9 { + // 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)) + if !noLinearRot { + angDeltaP = angDeltaP.Add(angularP.MulScalar(dLambda * params.JointAngularRelax)) + angDeltaC = angDeltaC.Add(angularC.MulScalar(dLambda * params.JointAngularRelax)) + } + + lambdaNext = slmath.SetDim3(lambdaNext, dim, dLambda) + } + } + } + SetJointLinLambda(ji, lambdaNext) + + //////// Angular DoFs + + jAngularN := GetJointAngularDoFN(ji) + + 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 + lambdaPrev = JointAngLambda(ji) + lambdaNext = math32.Vec3(0, 0, 0) + _ = lambdaPrev + + 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, JointTargetPosCur) + 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) + 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 = 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 := 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: + 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) + } + if mInvC > 0 { + StepBodyDeltas(jCi, jCbi, false, 0, linDeltaC, angDeltaC) + } +} + +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, linA, linB, angA, angB math32.Vector3, lambdaIn, compliance, damping, 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(tfaQ, angA) + rotAngB := slmath.MulQuatVectorInverse(tfbQ, angB) + + denom += slmath.Dot3(rotAngA, iInvA.MulVector3(rotAngA)) + denom += slmath.Dot3(rotAngB, iInvB.MulVector3(rotAngB)) + + alpha := compliance + gamma := compliance * damping + + lambda := -(err + alpha*lambdaIn + gamma*derr) + if denom+alpha > 0.0 { + lambda /= (dt+gamma)*denom + alpha/dt + } + + return lambda +} + +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) + rotAngA := slmath.MulQuatVectorInverse(tfaQ, angA) + rotAngB := slmath.MulQuatVectorInverse(tfbQ, angB) + + denom := float32(0.0) + 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) + if denom+alpha > 0.0 { + deltaLambda /= (dt+gamma)*denom + alpha/dt + } + 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(*axisLimitsA, up) + } +} + +//gosl:end diff --git a/physics/step_joint.goal b/physics/step_joint.goal new file mode 100644 index 00000000..1f96acaf --- /dev/null +++ b/physics/step_joint.goal @@ -0,0 +1,573 @@ +// 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 ( + // "fmt" + "cogentcore.org/core/math32" + "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" + +// newton: solvers/xpbd/kernels.py: apply_joint_forces + +// StepJointForces computes joint forces. +func StepJointForces(i uint32) { //gosl:kernel + params := GetParams(0) + ji := int32(i) + 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 + } + + jPi := JointParentIndex(ji) + jPbi := int32(-1) + if jPi >= 0 { + jPbi = DynamicBody(jPi) + } + jCi := JointChildIndex(ji) + jCbi := DynamicBody(jCi) + + jLinearN := GetJointLinearDoFN(ji) + jAngularN := GetJointAngularDoFN(ji) + + jPR := JointPPos(ji) + jPQ := JointPQuat(ji) + + // parent world transform + 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) + } + dP := xwPR.Sub(slmath.MulSpatialPoint(posePR, posePQ, comP)) // parent moment arm + + // child world transform + 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(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: + // 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.MulQuatVector(xwPQ, axis).MulScalar(JointControl(ji, 0, JointControlForce)) + case Prismatic: + axis := JointAxis(ji, 0) + f = slmath.MulQuatVector(xwPQ, axis).MulScalar(JointControl(ji, 0, JointControlForce)) + default: + for dof := range jLinearN { + axis := JointAxis(ji, int32(dof)) + 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.MulQuatVector(xwPQ, axis).MulScalar(JointControl(ji, di, JointControlForce))) + } + } + // These are unique to joint: aggregate into dynamics Next in [ForcesFromJoints] + SetJointPForce(ji, slmath.Negate3(f)) + SetJointCForce(ji, 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 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) + 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 + } + StepSolveJoint(ji) + } +} + +// StepSolveJoint applies target positions to linear DoFs. +// Position is updated prior to computing angulars. +func StepSolveJoint(ji int32) { + params := GetParams(0) + + jt := GetJointType(ji) + jPi := JointParentIndex(ji) + jPbi := int32(-1) + parentFixed := true + if jPi >= 0 { + jPbi = DynamicBody(jPi) + parentFixed = GetJointParentFixed(ji) + } + jCi := JointChildIndex(ji) + jCbi := DynamicBody(jCi) + noLinearRot := GetJointNoLinearRotation(ji) + + 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 comP, vP, 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) + 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 + 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 + + relPoseR := xwPR + relPoseQ := xwPQ + slmath.SpatialTransformInverse(xwPR, xwPQ, &relPoseR, &relPoseQ) + slmath.MulSpatialTransforms(relPoseR, relPoseQ, xwCR, xwCQ, &relPoseR, &relPoseQ) + + 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) + 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) // note: multiple iter is supposed to increment these + compliance := params.JointLinearComply + ke := JointControl(ji, 0, JointTargetStiff) + kd := JointControl(ji, 0, JointTargetDamp) + 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)) + linDeltaC = linDeltaC.Add(linearC.MulScalar(dLambda * params.JointLinearRelax)) + if !noLinearRot { + angDeltaP = angDeltaP.Add(angularP.MulScalar(dLambda * params.JointAngularRelax)) + angDeltaC = angDeltaC.Add(angularC.MulScalar(dLambda * params.JointAngularRelax)) + } + } + } else { + // all joints impose linear constraints! + var axisLimitsD, axisLimitsA math32.Vector3 + var axisTargetPosKeD, axisTargetPosKeA math32.Vector3 + var axisTargetVelKdD, axisTargetVelKdA math32.Vector3 + + for dof := range jLinearN { + axis := JointAxis(ji, dof) + 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, JointTargetPosCur) + 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) + } + } + + axisStiffness := axisTargetPosKeA + axisDamping := axisTargetVelKdA + axisTargetPosKeD = slmath.DivSafe3(axisTargetPosKeD, axisStiffness) + axisTargetVelKdD = slmath.DivSafe3(axisTargetVelKdD, axisDamping) + axisLimitsLower := axisLimitsD + axisLimitsUpper := axisLimitsA + // 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 + // 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, 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 := 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) + + 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 + } + } + if math32.Abs(err) > 1e-9 || math32.Abs(derrRel) > 1e-9 { + // 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)) + if !noLinearRot { + angDeltaP = angDeltaP.Add(angularP.MulScalar(dLambda * params.JointAngularRelax)) + angDeltaC = angDeltaC.Add(angularC.MulScalar(dLambda * params.JointAngularRelax)) + } + + lambdaNext = slmath.SetDim3(lambdaNext, dim, dLambda) + } + } + } + SetJointLinLambda(ji, lambdaNext) + + //////// Angular DoFs + + jAngularN := GetJointAngularDoFN(ji) + + 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 + lambdaPrev = JointAngLambda(ji) + lambdaNext = math32.Vec3(0,0,0) + _ = lambdaPrev + + 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, JointTargetPosCur) + 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) + 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 = 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 := 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: + 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) + } + if mInvC > 0 { + StepBodyDeltas(jCi, jCbi, false, 0, linDeltaC, angDeltaC) + } +} + +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, linA, linB, angA, angB math32.Vector3, lambdaIn, compliance, damping, 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(tfaQ, angA) + rotAngB := slmath.MulQuatVectorInverse(tfbQ, angB) + + denom += slmath.Dot3(rotAngA, iInvA.MulVector3(rotAngA)) + denom += slmath.Dot3(rotAngB, iInvB.MulVector3(rotAngB)) + + alpha := compliance + gamma := compliance * damping + + lambda := -(err + alpha*lambdaIn + gamma*derr) + if denom+alpha > 0.0 { + lambda /= (dt+gamma)*denom + alpha/dt + } + + return lambda +} + +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) + rotAngA := slmath.MulQuatVectorInverse(tfaQ, angA) + rotAngB := slmath.MulQuatVectorInverse(tfbQ, angB) + + denom := float32(0.0) + 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) + if denom+alpha > 0.0 { + deltaLambda /= (dt+gamma)*denom + alpha/dt + } + 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(*axisLimitsA, up) + } +} + +//gosl:end diff --git a/physics/typegen.go b/physics/typegen.go new file mode 100644 index 00000000..bfda31ba --- /dev/null +++ b/physics/typegen.go @@ -0,0 +1,31 @@ +// Code generated by "core generate -add-types -gosl"; DO NOT EDIT. + +package physics + +import ( + "cogentcore.org/core/types" +) + +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."}) + +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.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: "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"}}}) + +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."}) diff --git a/physics/vars.go b/physics/vars.go new file mode 100644 index 00000000..035e4aaa --- /dev/null +++ b/physics/vars.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 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 + +// CurModel is the currently active [Model]. +var CurModel *Model + +//gosl:start + +// vars are all the global vars for axon GPU / CPU computation. +// +//gosl:vars +var ( + // Params are global parameters. + //gosl:group Params + Params []PhysicsParams + + // 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 + + // 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][MaxObjectJoints+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] + //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 + + // 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 Dynamics + //gosl:dims 3 + Dynamics *tensor.Float32 + + // BroadContactsN has number of points of broad contact + // between bodies. [1] + //gosl:dims 1 + BroadContactsN *tensor.Int32 + + // 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 + + // 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 +) + +//gosl:end 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)) 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/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 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...) + } + } +} 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 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..b1af4a00 --- /dev/null +++ b/yaegilab/labsymbols/cogentcore_org-lab-physics-builder.go @@ -0,0 +1,27 @@ +// 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)), + "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-phyxyz.go b/yaegilab/labsymbols/cogentcore_org-lab-physics-phyxyz.go new file mode 100644 index 00000000..cd80b3be --- /dev/null +++ b/yaegilab/labsymbols/cogentcore_org-lab-physics-phyxyz.go @@ -0,0 +1,25 @@ +// 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), + "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)), + "Skin": reflect.ValueOf((*phyxyz.Skin)(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..b4b4e7b0 --- /dev/null +++ b/yaegilab/labsymbols/cogentcore_org-lab-physics.go @@ -0,0 +1,505 @@ +// 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), + "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), + "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), + "CurModel": reflect.ValueOf(&physics.CurModel).Elem(), + "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), + "GetJointNoLinearRotation": reflect.ValueOf(physics.GetJointNoLinearRotation), + "GetJointParentFixed": reflect.ValueOf(physics.GetJointParentFixed), + "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), + "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), + "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), + "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), + "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), + "JointNoLinearRotation": reflect.ValueOf(physics.JointNoLinearRotation), + "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), + "JointParentFixed": reflect.ValueOf(physics.JointParentFixed), + "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), + "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), + "LimitDelta": reflect.ValueOf(physics.LimitDelta), + "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), + "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), + "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), + "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), + "SetBodyThick": reflect.ValueOf(physics.SetBodyThick), + "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), + "SetJointAngLambda": reflect.ValueOf(physics.SetJointAngLambda), + "SetJointAngularDoFN": reflect.ValueOf(physics.SetJointAngularDoFN), + "SetJointAxis": reflect.ValueOf(physics.SetJointAxis), + "SetJointAxisDoF": reflect.ValueOf(physics.SetJointAxisDoF), + "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), + "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), + "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), + "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), + "StepBodyKinetics": reflect.ValueOf(physics.StepBodyKinetics), + "StepInit": reflect.ValueOf(physics.StepInit), + "StepIntegrateBodies": reflect.ValueOf(physics.StepIntegrateBodies), + "StepJointForces": reflect.ValueOf(physics.StepJointForces), + "StepSolveJoint": reflect.ValueOf(physics.StepSolveJoint), + "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 + "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)), + "Model": reflect.ValueOf((*physics.Model)(nil)), + "PhysicsParams": reflect.ValueOf((*physics.PhysicsParams)(nil)), + "Shapes": reflect.ValueOf((*physics.Shapes)(nil)), + } +} diff --git a/yaegilab/labsymbols/make b/yaegilab/labsymbols/make index f1a5b976..f78f4426 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 physics/builder plot plot/plots plotcore tensorcore lab 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),